Reference Frame for Global Link Transform

[Leo Keselman]

Hello All,

I need to extract the Jacobian and the end-effector pose from a manipulator in the same frame. How should I do this? 

My guess was to do 

const Eigen::Affine3d &base_state = kinematic_state->getGlobalLinkTransform(kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().front()));

          

Eigen::Vector3d reference_point_position = base_state.translation();

Eigen::MatrixXd jacobian;

kinematic_state->getJacobian(joint_model_group, kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),

                          reference_point_position,

                          jacobian);

but I realize I don't actually know the reference frame for the getGlobalLinkTransform. I assumed it was the base of the manipulator but I appear to be wrong.

Thanks for the help!

Leo

[Ioan Sucan]

Leo,

I am not sure I understand correctly what you are trying to do. All operations on a RobotState are performed in the model frame, which you can get using RobotModel->getModelFrame(). So if you call getJacobian(), it will be in the same reference frame as the pose reported by getGlobalLinkTransform(). The reference point you pass to the getJacobian function is just an offset in the link's reference frame. You can leave it as 0,0,0.

ioan

[Leo Keselman]

Ioan,

Thanks for the response. It is possible my problem is elsewhere. Just to be sure though... if my model base has some rotation, the Jacobian calculation will take this into account? 

I have this in my urdf for the fixed joints:

  <joint name="joint0" type="fixed">

    <parent link="base_footprint"/>

    <child link="L0"/>

    <origin xyz="0 0 0" rpy="-1.57 1.57 0" />

  </joint>

This shouldn't cause any problem?

Thanks

Leo

[Ioan Sucan]

That rotation will be considered, but I need to double check this. I added an optimization for fixed joints that may bypass that. Is it possible for you to test whether changing the urdf values for rpy above changes FK in otherwise constant circumstances?

Ioan

[Ioan Sucan]

And I looked at the code now -- to me it seems fine. (Fixed joint should work)

Ioan

[Leo Keselman]

Yes both the Jacobian and FK change when I chage the fixed joint rotation. My problem must be elsewhere.

Thanks!

[Leo Keselman]

Ioan,

I realized that in fact the Jacobian returned by RobotState does not take into account rpy fields of joints (whether revolute or fixed). What should I do to fix this?

Thanks

Leo

[Sachin Chitta]

Hi Leo,

Could you please point me to some code to test this?

Thanks,
Sachin

[Leo Keselman]

Hello Sachin,

Sure. I'm attaching the urdf I'm using here. I guess this wouldn't actually be useful without the link models so I'll attach them as well. In the urdf, the models are specified by their absolute position on my computer so you may want to change that to ./link.dae or whatever. I guess it would be easiest for you to make your own moveit_config with the setup_assistant. 

To test, I am simply looking at the Jacobian provided by the Robot_State class exactly as in this tutorial: http://moveit.ros.org/wiki/Kinematics/C%2B%2B_API

You should see that the Jacobian(at least the linear part) will stay the same despite changing the rpy value of joint0 ( which should be set to [1.57 1.57 0] to get the manipulator to stand up properly).

Let me know if you want me to be more specific or provide more files.

Thank you

Leo

[Sachin Chitta]

Hey Leo,

I just pushed a change to moveit_core (hydro-devel) that should fix this - could you please try and let me know. The issue was with chains that had a fixed joint in the beginning (like your arm did) - this was not properly handled when finding the root joint of the chain.

Best Regards,
Sachin

 

[Leo Keselman]

Yes it works properly! Thanks for your help

Leo