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 gettest_cart
. Then it's obvious thatpose
won't be the same astest_cart
.I suspect you wanted to use a
geometry_msgs::Twist
instead ofPose
.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