int main(int argc, char** argv) { ros::init(argc, argv, "parser_test"); ros::NodeHandle nh; try { std::string name = "red_arm"; RobotPtr robot = std::make_shared<Robot>(name); ParserPtr parser = std::make_shared<Parser>(); std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/red_arm.yaml"; parser->load(path, robot); ros::MultiThreadedSpinner spinner; TfPublisherPtr tf_publisher = std::make_shared<TfPublisher>(); const std::string mnp_name = "mnp"; unsigned long cnt = 0; ros::Rate r(10.0); while(ros::ok()) { Eigen::VectorXd q = Eigen::VectorXd::Zero(robot->getDOF(mnp_name)); ManipulatorPtr mnp = robot->getManipulator(mnp_name); double goal = sin(2.0 * M_PI * 0.2 * cnt * 0.1); ++cnt; for(uint32_t i = 0; i < q.rows(); ++i) { q.coeffRef(6) = M_PI / 4.0 * goal; } std::cout << M_PI / 4.0 * goal << std::endl; //q = Eigen::VectorXd::Constant(q.rows(), 0.0 * M_PI / 4.0); robot->update(q); robot->computeJacobian(mnp_name); robot->computeMassMatrix(mnp_name); M = robot->getMassMatrix(mnp_name); J = robot->getJacobian(mnp_name, "gripper"); calc(); tf_publisher->publish(robot, false); r.sleep(); } } catch(ahl_utils::Exception& e) { ROS_ERROR_STREAM(e.what()); } return 0; }
virtual int run(int, char*[]) { // // Down-cast the proxy to a Directory proxy. // DirectoryPrx rootDir = DirectoryPrx::checkedCast(communicator()->propertyToProxy("RootDir.Proxy")); if(!rootDir) { throw "Invalid proxy"; } ParserPtr p = new Parser(rootDir); return p->parse(); }
int main(int argc, char** argv) { ros::init(argc, argv, "lwr_server"); ros::NodeHandle nh; ros::Timer timer_update_model = nh.createTimer(ros::Duration(0.01), updateModel); ros::Timer timer_control = nh.createTimer(ros::Duration(0.001), control); robot = RobotPtr(new Robot("lwr")); ParserPtr parser = ParserPtr(new Parser()); std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/lwr.yaml"; parser->load(path, robot); controller = RobotControllerPtr(new RobotController()); controller->init(robot); using namespace ahl_gazebo_if; gazebo_interface = GazeboInterfacePtr(new GazeboInterface()); gazebo_interface->setDuration(0.010); gazebo_interface->addJoint("lwr::joint1"); gazebo_interface->addJoint("lwr::joint2"); gazebo_interface->addJoint("lwr::joint3"); gazebo_interface->addJoint("lwr::joint4"); gazebo_interface->addJoint("lwr::joint5"); gazebo_interface->addJoint("lwr::joint6"); gazebo_interface->addJoint("lwr::joint7"); gazebo_interface->connect(); tf_pub = TfPublisherPtr(new TfPublisher()); ManipulatorPtr mnp = robot->getManipulator("mnp"); gravity_compensation = TaskPtr(new GravityCompensation(robot)); damping = TaskPtr(new Damping(mnp)); joint_control = TaskPtr(new JointControl(mnp)); joint_limit = TaskPtr(new JointLimit(mnp, 0.087)); position_control = TaskPtr(new PositionControl(mnp, "gripper", 0.001)); orientation_control = TaskPtr(new OrientationControl(mnp, "gripper", 0.001)); controller->addTask(gravity_compensation, 0); controller->addTask(damping, 0); controller->addTask(joint_control, 0); controller->addTask(joint_limit, 100); ros::MultiThreadedSpinner spinner; spinner.spin(); return 0; }
int compile_chain(string filename) { cout << "[ Compiling... ]" << endl; InputPtr input = Input::open_file(filename); if (input != nullptr) { ScannerPtr scanner = ScannerPtr(new Scanner(input)); SemanticAnalyzerPtr analyzer = SemanticAnalyzerPtr(new SemanticAnalyzer(filename)); ParserPtr parser = ParserPtr(new Parser(scanner, analyzer)); parser->parse(); parser->get_analyzer()->generate_all(); report_msg_type("Success", "Compilation terminated successfully"); } else { return -1; } cout << "[ End ]" << endl; return 0; }
static void check_parser(ParserPtr parser) { DeckPtr deck = parser->parseString(pvtoData, ParseMode()); DeckKeywordConstPtr kw1 = deck->getKeyword("PVTO" , 0); BOOST_CHECK_EQUAL(5U , kw1->size()); DeckRecordConstPtr record0 = kw1->getRecord(0); DeckRecordConstPtr record1 = kw1->getRecord(1); DeckRecordConstPtr record2 = kw1->getRecord(2); DeckRecordConstPtr record3 = kw1->getRecord(3); DeckRecordConstPtr record4 = kw1->getRecord(4); DeckItemConstPtr item0_0 = record0->getItem("RS"); DeckItemConstPtr item0_1 = record0->getItem("DATA"); BOOST_CHECK_EQUAL(1U , item0_0->size()); BOOST_CHECK_EQUAL(9U , item0_1->size()); BOOST_CHECK_EQUAL(2U , record0->size()); DeckItemConstPtr item1_0 = record1->getItem("RS"); DeckItemConstPtr item1_1 = record1->getItem("DATA"); BOOST_CHECK_EQUAL(1U , item1_0->size()); BOOST_CHECK_EQUAL(9U , item1_1->size()); BOOST_CHECK_EQUAL(2U , record1->size()); DeckItemConstPtr item2_0 = record2->getItem("RS"); DeckItemConstPtr item2_1 = record2->getItem("DATA"); BOOST_CHECK(item2_0->defaultApplied(0)); BOOST_CHECK_EQUAL(0U , item2_1->size()); BOOST_CHECK_EQUAL(2U , record2->size()); DeckItemConstPtr item3_0 = record3->getItem("RS"); DeckItemConstPtr item3_1 = record3->getItem("DATA"); BOOST_CHECK_EQUAL(1U , item3_0->size()); BOOST_CHECK_EQUAL(9U , item3_1->size()); BOOST_CHECK_EQUAL(2U , record3->size()); DeckItemConstPtr item4_0 = record4->getItem("RS"); DeckItemConstPtr item4_1 = record4->getItem("DATA"); BOOST_CHECK_EQUAL(1U , item4_0->size()); BOOST_CHECK_EQUAL(9U , item4_1->size()); BOOST_CHECK_EQUAL(2U , record4->size()); Opm::PvtoTable pvtoTable; pvtoTable.initFORUNITTESTONLY(kw1, /*tableIdx=*/0); const auto &outerTable = *pvtoTable.getOuterTable(); const auto &innerTable0 = *pvtoTable.getInnerTable(0); BOOST_CHECK_EQUAL(2, outerTable.numRows()); BOOST_CHECK_EQUAL(4, outerTable.numColumns()); BOOST_CHECK_EQUAL(3, innerTable0.numRows()); BOOST_CHECK_EQUAL(3, innerTable0.numColumns()); BOOST_CHECK_EQUAL(1e-3, outerTable.getGasSolubilityColumn()[0]); BOOST_CHECK_EQUAL(1.0e5, outerTable.getPressureColumn()[0]); BOOST_CHECK_EQUAL(outerTable.getPressureColumn()[0], innerTable0.getPressureColumn()[0]); BOOST_CHECK_EQUAL(1.01, outerTable.getOilFormationFactorColumn()[0]); BOOST_CHECK_EQUAL(outerTable.getOilFormationFactorColumn()[0], innerTable0.getOilFormationFactorColumn()[0]); BOOST_CHECK_EQUAL(1.02e-3, outerTable.getOilViscosityColumn()[0]); BOOST_CHECK_EQUAL(outerTable.getOilViscosityColumn()[0], innerTable0.getOilViscosityColumn()[0]); }
static void check_parser(ParserPtr parser) { DeckPtr deck = parser->parseString(pvtoData, ParseContext()); const auto& kw1 = deck->getKeyword("PVTO" , 0); BOOST_CHECK_EQUAL(5U , kw1.size()); const auto& record0 = kw1.getRecord(0); const auto& record1 = kw1.getRecord(1); const auto& record2 = kw1.getRecord(2); const auto& record3 = kw1.getRecord(3); const auto& record4 = kw1.getRecord(4); const auto& item0_0 = record0.getItem("RS"); const auto& item0_1 = record0.getItem("DATA"); BOOST_CHECK_EQUAL(1U , item0_0.size()); BOOST_CHECK_EQUAL(9U , item0_1.size()); BOOST_CHECK_EQUAL(2U , record0.size()); const auto& item1_0 = record1.getItem("RS"); const auto& item1_1 = record1.getItem("DATA"); BOOST_CHECK_EQUAL(1U , item1_0.size()); BOOST_CHECK_EQUAL(9U , item1_1.size()); BOOST_CHECK_EQUAL(2U , record1.size()); const auto& item2_0 = record2.getItem("RS"); const auto& item2_1 = record2.getItem("DATA"); BOOST_CHECK(item2_0.defaultApplied(0)); BOOST_CHECK_EQUAL(0U , item2_1.size()); BOOST_CHECK_EQUAL(2U , record2.size()); const auto& item3_0 = record3.getItem("RS"); const auto& item3_1 = record3.getItem("DATA"); BOOST_CHECK_EQUAL(1U , item3_0.size()); BOOST_CHECK_EQUAL(9U , item3_1.size()); BOOST_CHECK_EQUAL(2U , record3.size()); const auto& item4_0 = record4.getItem("RS"); const auto& item4_1 = record4.getItem("DATA"); BOOST_CHECK_EQUAL(1U , item4_0.size()); BOOST_CHECK_EQUAL(9U , item4_1.size()); BOOST_CHECK_EQUAL(2U , record4.size()); Opm::PvtoTable pvtoTable(kw1 , 0); BOOST_CHECK_EQUAL(2, pvtoTable.size()); const auto &table0 = pvtoTable.getUnderSaturatedTable(0); const auto& BO = table0.getColumn( "BO" ); BOOST_CHECK_EQUAL( 3, table0.numRows()); BOOST_CHECK_EQUAL( 3, table0.numColumns()); BOOST_CHECK_EQUAL( BO.front( ) , 1.01 ); BOOST_CHECK_EQUAL( BO.back( ) , 1.20 ); BOOST_CHECK_CLOSE(1.15 , table0.evaluate( "BO" , 250*1e5 ) , 1e-6); BOOST_CHECK_CLOSE( 1.15 , pvtoTable.evaluate( "BO" , 1e-3 , 250*1e5 ) , 1e-6 ); BOOST_CHECK_CLOSE( 1.15 , pvtoTable.evaluate( "BO" , 0.0 , 250*1e5 ) , 1e-6 ); }
void check_SgofTable(ParserPtr parser) { DeckPtr deck = parser->parseString(parserData); Opm::SgofTable sgofTable(deck->getKeyword("SGOF")); BOOST_CHECK_EQUAL(10U, sgofTable.getSgColumn().size()); BOOST_CHECK_EQUAL(0.1, sgofTable.getSgColumn()[0]); BOOST_CHECK_EQUAL(0.0, sgofTable.getKrgColumn()[0]); BOOST_CHECK_EQUAL(1.0, sgofTable.getKrogColumn()[0]); BOOST_CHECK_EQUAL(0.0, sgofTable.getPcogColumn()[0]); }
virtual int run(int, char*[]) { // // Create a proxy for the root directory // Ice::ObjectPrx base = communicator()->stringToProxy("RootDir:default -p 10000"); // // Down-cast the proxy to a Directory proxy. // DirectoryPrx rootDir = DirectoryPrx::checkedCast(base); if(!rootDir) { throw "Invalid proxy"; } ParserPtr p = new Parser(rootDir); return p->parse(); }
void check_parser(ParserPtr parser) { DeckPtr deck = parser->parseString(parserData); DeckKeywordConstPtr kw1 = deck->getKeyword("SGOF"); BOOST_CHECK_EQUAL(1U , kw1->size()); DeckRecordConstPtr record0 = kw1->getRecord(0); BOOST_CHECK_EQUAL(1U , record0->size()); DeckItemConstPtr item0 = record0->getItem(0); BOOST_CHECK_EQUAL(10U * 4, item0->size()); }
void Client::interrupted() { Lock sync(*this); if(_parser) // If there's an interactive parser, notify the parser. { _parser->interrupt(); } else { // // Otherwise, destroy the communicator. // assert(_communicator); try { _communicator->destroy(); } catch(const Exception&) { } } }
int main(int argc, char** argv) { ros::init(argc, argv, "parser_test"); ros::NodeHandle nh; try { std::string name = "personal_robot"; RobotPtr robot = RobotPtr(new Robot(name)); ParserPtr parser = ParserPtr(new Parser()); std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/pr2.yaml"; parser->load(path, robot); ros::MultiThreadedSpinner spinner; TfPublisherPtr tf_publisher = TfPublisherPtr(new TfPublisher()); const std::string mnp_name1 = "left_mnp"; const std::string mnp_name2 = "right_mnp"; unsigned long cnt = 0; ros::Rate r(10.0); while(ros::ok()) { Eigen::VectorXd q = Eigen::VectorXd::Zero(robot->getDOF()); ManipulatorPtr left_mnp = robot->getManipulator(mnp_name1); double goal = sin(2.0 * M_PI * 0.2 * cnt * 0.1); ++cnt; //std::cout << M_PI / 4.0 * goal << std::endl; q = Eigen::VectorXd::Constant(q.rows(), 0.0); q[10] = goal; robot->update(q); //robot->update(mnp_name2, q); robot->computeJacobian(mnp_name1); robot->computeJacobian(mnp_name2); robot->computeMassMatrix(mnp_name1); robot->computeMassMatrix(mnp_name2); M1 = robot->getMassMatrix(mnp_name1); M2 = robot->getMassMatrix(mnp_name2); J1 = robot->getJacobian(mnp_name1, "gripper_l_link"); J2 = robot->getJacobian(mnp_name2, "gripper_r_link"); calc(); tf_publisher->publish(robot, false); r.sleep(); } } catch(ahl_utils::Exception& e) { ROS_ERROR_STREAM(e.what()); } return 0; }
int main(int argc, char** argv) { ros::init(argc, argv, "parser_test"); ros::NodeHandle nh; try { std::string name = "youbot"; RobotPtr robot = RobotPtr(new Robot(name)); ParserPtr parser = ParserPtr(new Parser()); std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/youbot.yaml"; parser->load(path, robot); robot->getMobility()->print(); ros::MultiThreadedSpinner spinner; TfPublisherPtr tf_publisher = TfPublisherPtr(new TfPublisher()); const std::string mnp_name = "mnp"; unsigned long cnt = 0; const double period = 0.001; ros::Rate r(1 / period); ahl_filter::DifferentiatorPtr differentiator = ahl_filter::DifferentiatorPtr(new ahl_filter::PseudoDifferentiator(period, 1.0)); Eigen::VectorXd q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 0.0); Eigen::VectorXd dq = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 0.0); differentiator->init(q, dq); Eigen::VectorXd pre_q = q; //std::ofstream ofs1; //ofs1.open("result1.hpp"); //std::ofstream ofs2; //ofs2.open("result2.hpp"); //std::ofstream ofs3; //ofs3.open("result3.hpp"); while(ros::ok()) { q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 1.0); double coeff = 1.0 * sin(2.0 * M_PI * 0.1 * cnt * period); ++cnt; q = coeff * q; //q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), M_PI / 2.0); q.coeffRef(0) = 0.0; q.coeffRef(1) = 0.0; q.coeffRef(2) = 0.0; robot->update(mnp_name, q); robot->computeBasicJacobian(mnp_name); robot->computeMassMatrix(mnp_name); differentiator->apply(q); Eigen::VectorXd dq1 = robot->getJointVelocity(mnp_name); Eigen::VectorXd dq2; differentiator->copyDerivativeValueTo(dq2); std::cout << "p : " << q.transpose() << std::endl; std::cout << "pre_p : " << pre_q.transpose() << std::endl; Eigen::VectorXd dq3 = (q - pre_q) / period; pre_q = q; //std::cout << "dq1 : " << dq1.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl; //std::cout << "dq2 : " << dq2.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl; //std::cout << "dq3 : " << dq3.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl; //std::cout << dq << std::endl << std::endl; //std::cout << cos(2.0 * M_PI * 0.1 * cnt * 0.1) << std::endl; tf_publisher->publish(robot, false); /* ofs1 << cnt * period << " "; ofs2 << cnt * period << " "; ofs3 << cnt * period << " "; for(unsigned int i = 0; i < dq.rows() - 3; ++i) { ofs1 << dq1[i + 3] << " "; ofs2 << dq2[i + 3] << " "; ofs3 << dq3[i + 3] << " "; } ofs1 << std::endl; ofs2 << std::endl; ofs3 << std::endl; */ r.sleep(); } } catch(ahl_robot::Exception& e) { ROS_ERROR_STREAM(e.what()); } return 0; }
static void check_parser(ParserPtr parser) { DeckPtr deck = parser->parseString(pvtgData, ParseMode()); DeckKeywordConstPtr kw1 = deck->getKeyword("PVTG" , 0); BOOST_CHECK_EQUAL(5U , kw1->size()); DeckRecordConstPtr record0 = kw1->getRecord(0); DeckRecordConstPtr record1 = kw1->getRecord(1); DeckRecordConstPtr record2 = kw1->getRecord(2); DeckRecordConstPtr record3 = kw1->getRecord(3); DeckRecordConstPtr record4 = kw1->getRecord(4); DeckItemConstPtr item0_0 = record0->getItem("GAS_PRESSURE"); DeckItemConstPtr item0_1 = record0->getItem("DATA"); BOOST_CHECK_EQUAL(1U , item0_0->size()); BOOST_CHECK_EQUAL(9U , item0_1->size()); BOOST_CHECK_EQUAL(2U , record0->size()); DeckItemConstPtr item1_0 = record1->getItem("GAS_PRESSURE"); DeckItemConstPtr item1_1 = record1->getItem("DATA"); BOOST_CHECK_EQUAL(1U , item1_0->size()); BOOST_CHECK_EQUAL(9U , item1_1->size()); BOOST_CHECK_EQUAL(2U , record1->size()); DeckItemConstPtr item2_0 = record2->getItem("GAS_PRESSURE"); DeckItemConstPtr item2_1 = record2->getItem("DATA"); BOOST_CHECK( item2_0->defaultApplied(0)); BOOST_CHECK_EQUAL(0U , item2_1->size()); BOOST_CHECK_EQUAL(2U , record2->size()); DeckItemConstPtr item3_0 = record3->getItem("GAS_PRESSURE"); DeckItemConstPtr item3_1 = record3->getItem("DATA"); BOOST_CHECK( !item3_1->defaultApplied(0)); BOOST_CHECK( item3_1->defaultApplied(1)); BOOST_CHECK( !item3_1->defaultApplied(2)); BOOST_CHECK( !item3_1->defaultApplied(3)); BOOST_CHECK( item3_1->defaultApplied(4)); BOOST_CHECK( !item3_1->defaultApplied(5)); BOOST_CHECK_EQUAL(1U , item3_0->size()); BOOST_CHECK_EQUAL(9U , item3_1->size()); BOOST_CHECK_EQUAL(2U , record3->size()); DeckItemConstPtr item4_0 = record4->getItem("GAS_PRESSURE"); DeckItemConstPtr item4_1 = record4->getItem("DATA"); BOOST_CHECK_EQUAL(1U , item4_0->size()); BOOST_CHECK_EQUAL(9U , item4_1->size()); BOOST_CHECK_EQUAL(2U , record4->size()); { Opm::PvtgTable pvtgTable; pvtgTable.initFORUNITTESTONLY(kw1, 0); const auto &outerTable = *pvtgTable.getOuterTable(); const auto &innerTable0 = *pvtgTable.getInnerTable(0); BOOST_CHECK_EQUAL(2U, outerTable.numRows()); BOOST_CHECK_EQUAL(4U, outerTable.numColumns()); BOOST_CHECK_EQUAL(3U, innerTable0.numRows()); BOOST_CHECK_EQUAL(3U, innerTable0.numColumns()); BOOST_CHECK_EQUAL(20.0e5, outerTable.getPressureColumn()[0]); BOOST_CHECK_EQUAL(0.00002448, outerTable.getOilSolubilityColumn()[0]); BOOST_CHECK_EQUAL(outerTable.getOilSolubilityColumn()[0], innerTable0.getOilSolubilityColumn()[0]); BOOST_CHECK_EQUAL(0.061895, outerTable.getGasFormationFactorColumn()[0]); BOOST_CHECK_EQUAL(outerTable.getGasFormationFactorColumn()[0], innerTable0.getGasFormationFactorColumn()[0]); BOOST_CHECK_EQUAL(1.299e-5, outerTable.getGasViscosityColumn()[0]); BOOST_CHECK_EQUAL(outerTable.getGasViscosityColumn()[0], innerTable0.getGasViscosityColumn()[0]); } }
virtual void initRobot(const std::string& name, const std::string& yaml) { robot_ = std::make_shared<Robot>(name); ParserPtr parser = std::make_shared<Parser>(); parser->load(yaml, robot_); }