End-effector pose

Hellow,
Thanks for providing a good simulation. I got a problem when I was controlling the end-effector. In the main.py file, I added the following code to get the pose of the end-effector.

 pos = p.getLinkState(robot.id, robot.eef_id)
 start_eur = p.getEulerFromQuaternion(pos[5])
 print(start_eur)

But the returned start_eur variable is inconsistent with the input of env.read_debug_parameter().
For example

start_eur = (3.141490980302897, 0.6957960404397654, -1.57088815227758)
[roll, pitch, yaw] in env.read_debug_parameter() is [0.0, 2.446, 1.571]

This looks like there is a missing transformation matrix between the pose of the end-effector and the Orientation [R P Y].
How to make the end-effector move to the specified pose？

I would appreciate it if you could share some suggestions or point me in the right direction. Thank you.

Hi @Gary3410 ,

Thanks for your interest. I don't know the root cause here. Perhaps it is due to this line

pybullet_ur5_robotiq/urdf/ur5_robotiq_85.urdf

Line 547 in 8e1256b

But, since the angles all have a delta, $3.14$, or $pi$, I think it's relatively easy to fix, if you just want a working solution. You can simple gather multiple pairs of values, and see if you can get that linear transform between the real RPY and the reported RPY.

Thanks for your reply.
I did some other attempts and found that there is no problem with the control of the end-effector.

real_RPY = [0.0, 2.446, 1.571]
reported_RPY = [3.141490980302897, 0.6957960404397654, -1.57088815227758]
real_Quaternion  = p.getQuaternionFromEuler(real_RPY)
reported_Quaternion = p.getQuaternionFromEuler(reported_RPY)
# real_Quaternion = (0.6647267386875654, -0.6647632689796785, -0.24109134420405115, -0.2410458936367279)
# reported_Quaternion = (-0.6648372109085479, 0.664701815170704, 0.24102555796676586, 0.24097647251738866)

The real_RPY and the reported_RPY can be considered equivalent when converted to Quaternion.
When converting from Quaternion to Euler angles.

r = R.from_quat(real_Quaternion) 
euler0 = r.as_euler('xyz', degrees=False)
# euler0 = [3.14149098, 0.69579604, -1.57088815]
euler1 = r.as_euler('yxz', degrees=False)
# euler1 = [2.44579661e+00, 7.80386893e-05, 1.57076967e+00]

Therefore, the real_RPY and the reported_RPY are two representations of the same pose. The difference is only reflected in the movement path of the end-effector.
Perhaps directly using the Quaternion to control the end-effector can avoid the inconsistency of the path.
Can this be easily implemented in your project?

Hi @Gary3410 ,

You are right! I stick with RPY in the GUI because it's intuitive. Actually, the robot is controlled by quat, as we can see here

pybullet_ur5_robotiq/robot.py

Line 115 in 8e1256b

orn = p.getQuaternionFromEuler((roll, pitch, yaw))

.

Therefore,

Can this be easily implemented in your project?

Yes! Exactly!