Robotics StackExchange | Archived questions

Moveit getJacobian returns wrong jacobian

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)<<a1,a2,a3,a4,a5,a6;
auto test_cart=jacobian*joint_test;

The problem is that the testcart is different than the pose given to the setFromIK method, (testcart!=pose) what am I misssing? I use ros kinetic

Asked by os on 2019-05-20 04:54:24 UTC

Comments

Why are you trying to find the pose of the tip by multiplying the Jacobian with the joint positions vector?

Asked by dhindhimathai on 2019-05-20 06:46:41 UTC

I want to convert cartesian velocities to joint space and from joint space to cartesian. The position conversion was a sanity check

Asked by os on 2019-05-20 07:04:20 UTC

@os Let me understand, you are doing the following: 1) creating a cartesian pose -> pose 2) setting the kinematic state of the robot from that pose 3) getting the jacobian 4) multiplying the jacobian by the joint state to get test_cart. Then it's obvious that pose won't be the same as test_cart.

I suspect you wanted to use a geometry_msgs::Twist instead of Pose.

Asked by dhindhimathai on 2019-05-23 04:34:25 UTC

@os And may be you wanted to use some sort of kinematic inversion to find the joint velocities from the twist.

Asked by dhindhimathai on 2019-05-23 04:36:50 UTC

@dhindhimathai Why it's obvious that pose won't be the same as test_cart. According to theory x=J*q. whats the difference between Twis and Pose?

Asked by os on 2019-05-23 06:36:19 UTC

Answers