Issue: Frames computed from Pinocchio does not match ground truth values of gazebo.

[rns0869]

Hello, I read in a robot_description file in my node using (which is a ros parameter inside the launch file) , where I want the dynamics of panda Robot in my node.

Note: when I run command: rosrun xacro xacro panda_robot.xacro > output.urdf
the output.urdf does includes an urdf, which makes me think the urdf is fine and has no issues( I could be wrong, I am not sure), plus the robot spawns inside gazebo, which makes me think that the urdf is okay.

So in my launch file first i get the robot_description file and then I load the robot_description in an empty gazebo world, then I run a robot_state_publisher gui (which publishes joint angles on a topic which my node subscribes to) and finally I run my node which subscribes to the joint angles published by robot_state_publisher gui and publishes them on the topic ros_controller_command, which is the topic that the joint state controller accepts (I am using joint_state_controller from ros_control package).

What I want to do in my node: I want to read the robot description

further I subscribe to the joint values published by robot_state_publisher gui, use these joint values as desired values and implement a forwards kinematics using desired joint values (I want to see whether the frames computed from Pinocchio matches the one from gazebo) and also publish the desired joint values to the topic that the joint state controller subscribes to.

Then publish the tf of link_8 which I get from Pinocchio robot.data.oMf[''], named as my_robot_1_link8 and check whether the published tf for link8 matches with the real tf of link8, named robot_1_link8 (robot_1_link8 is ground truth position that is in gazebo or being shown in rviz).

Note: I noticed something that the robot is not spawned at all 0 position (robot_1_joint4 is -1.57 radians, but somehow, the robot_state_publisher gui shows all joints are 0 in the beginning including robot_1_joint4).

What I noticed is that when all joint values are 0 in robot_state_publisher gui then both the tf, my_robot_1_link8 and robot_1_link8 coincide), but when I move the joint values the frames are not in the same position and my_robot_1_link8 (the one which Pinocchio computes) moves weirdly.

Here is a video as well:

Current.webm

I don't know why this is happening, Can you please help me?

[jorisv]

Hello @rns0869,

I have never used Gazebo so take what I say with a grain of salt.
Gazebo run a physics engine and when we send a joint configuration (q) target to Gazebo a servoing algorithm will run to try to follow this target. If the servoing is not well tuned, a joint error can occur.
You can try to remove gravity in gazebo to check if the result is the same.

Also in your video, when you move the joint 2 configuration target to 1.43 rad (near to 90°) the frame computed by pinocchio seem to be at 90° from the robot joint 2. So it look correct.