I’m trying to understand how to use pose_sub together with speedL and trying to imitate the pose_sub functionality in Python.
I’ve written a small program which has 4 variables containing 4 poses.
var_1 is the base pose, from where all the movements start.
var_2 represents a position change from var_1.
var_3 represents an orientation change from var_1
var_4 represents a pose change from var_1
First I do te movements using MoveL commands, and then I try to reproduce them with speedL commands.
It seems odd to me that in order to imitate the MoveL commands and achieve the same final poses with pose_sub and speedL, rotations (coming from pose_sub) have to change the sign while translations (coming from pose_sub) keep the same sign.
Why is this?
Besides, a second question is, how is the orientation subtraction computed?
I’ve tried this using Python:
- From var_1 and var_3, convert Rx, Ry and Rz from axis angle representation to quaternions (also tried using roll, pitch and yaw).
- In quaternion form, compute de rotation difference.
- Take this orientation different back to an axes angle representation.
I found the solution to the second question: how is the orientation subtraction computed so that it can be used to feed an speedL command?
In pseudo-code, it’d be like this:
- From var_1 and var_3, convert Rx, Ry and Rz from axis angle representation to quaternions (q1 and q3)
- In quaternion form, compute de rotation difference as: q_diff_orientation= quternion_multiplication(quaternion_inverse(q3), q1)
- Take this q_diff_orientation back to an axes angle representation to feed speedL.
I still find intriguing why the difference of sign when using translations or rotations.
pose_sub is basically created so the following expression becomes true
pose_add(pose_sub(p_1, p_2), p_2) == p_1
What is your use case for the speedl functionality?
Thanks for the explanation.
The high level task I’m trying to accomplish is:
- I record the X,Y,Z,rx,ry,rz information of a trajectory performed using RTDE.
- Then, I want to resproduce it. While repoducing, there’ll be small differences in the path that will appear due to the movement of the work piece (this is why I cannot record joint positions). I detect the need to slightly change the path using the force sensor.
- This reproduction is also controlled using RTDE.
So, first, I though I would perform a control loop in my computer like:
- Read actual_pose
- Compute goal_pose-actual_pose (this is why I wanted to learn to reproduce pose_sub in python)
- And send speedL commands
The robot controller would basically read the inconming information and execute speedL commands.
I know I could also try a similar control loop using servoj(get_inverse(goal_pose-actual_pose)), adjusting each loop how much I want to advance.
I’m trying to figure out which one fits better in my applitcation.
I started with speedL because I find it more intuitive.