I am trying to extract the Jacobian with that code:
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelConstPtr kinematic_model = robot_model_loader.getModel();
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("manipulator");
Eigen::MatrixXd jacobian;
Eigen::Vector3d reference_point_position(0.0,0.0,0.0);
geometry_msgs::Pose pose;
pose.orientation.x=0;
pose.orientation.y=0;
pose.orientation.z=0;
pose.orientation.w=1;
pose.position.x=0.249;
pose.position.y=0.0;
pose.position.z=0.68;
pose.position.x=0.74;
pose.position.y=0.0;
pose.position.z=0.855;
kinematic_state->setFromIK(joint_model_group, pose, 10, 0.01);
kinematic_state->getJacobian(joint_model_group,
kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
reference_point_position, jacobian);
Eigen::MatrixXd joint_test(6,1);
double a1=*kinematic_state->getJointPositions("joint_a1");
double a2=*(kinematic_state->getJointPositions("joint_a2"));
double a3=(*kinematic_state->getJointPositions("joint_a3"));
double a4=*kinematic_state->getJointPositions("joint_a4");
double a5=(*kinematic_state->getJointPositions("joint_a5"));
double a6=*kinematic_state->getJointPositions("joint_a6");
joint_test.col(0).tail(6)<
↧