Moveit getJacobian returns wrong jacobian

asked 2019-05-20 04:54:24 -0600

os gravatar image

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 test_cart is different than the pose given to the setFromIK method, (test_cart!=pose) what am I misssing? I use ros kinetic

edit retag flag offensive close merge delete

Comments

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

dhindhimathai gravatar image dhindhimathai  ( 2019-05-20 06:46:41 -0600 )edit

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

os gravatar image os  ( 2019-05-20 07:04:20 -0600 )edit

@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.

dhindhimathai gravatar image dhindhimathai  ( 2019-05-23 04:34:25 -0600 )edit

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

dhindhimathai gravatar image dhindhimathai  ( 2019-05-23 04:36:50 -0600 )edit

@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?

os gravatar image os  ( 2019-05-23 06:36:19 -0600 )edit