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