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_);
}
