[william.stewart]

Hello,

I have been working on a python program for a Staubli TX2-90 robot. I am attempting to adjust the angle of the tool. I first set the tool pose relative to the robot flange, then I move the robot to the home position using predefined joints. Then I retrieve the position of the tool, and adjust the relevant angle (saving it as a new position). I convert back to a pose, then use the MoveJ command to move the robot.

The code seems to work fine, however, in the line where I adjust the angle, if I adjust rx or rz (+20 in my code) they produce rotation about the same axis. Regardless of whether I change the rx or rz angle, it changes the purple value (in attached screenshot). It seems like a gimbal lock problem, but in the roboDK GUI, I am able to use the dial to rotate about each axis correctly (light blue/purple/yellow angles in the figure below). Furthermore, I don't get any errors related to that.

I have included the code below. Thanks in advance for any insights.

Code:

# Set the tool pose relative to the robot flange
0610年tool_position = [0, 0, -20, 0]
tool_pose = xyzrpw_2_pose(tool_position)
robot.setPoseTool(tool_pose)

# Move to the home position and save the pose
robot.MoveJ([90,-45,-90,0,65,0])
Home_pose = robot.Pose()

# Adjust the angle
Home_position = pose_2_xyzrpw(Home_pose)
x,y,z,rx,ry,rz = Home_position
new_position = [x,y,z,rx,ry,rz+20] # Here I change the angle

# Move to new position
new_pose = xyzrpw_2_pose(new_position)
robot.MoveJ(new_pose)

[william.stewart]

I think I have narrowed down the culprit a bit. It seems to be a problem with xyzrpw_2_pose(). Regardless of whether I change the r or w input, it produces almost the same pose (one term is -0 in one pose and in the other pose it is 0).My question is why when given different inputs, does xyzrpw_2_pose() give the same output?

Code:

# get the current robot position
home_pose = robot.Pose()
home_position = pose_2_xyzrpw(home_pose)
x,y,z,rx,ry,rz = home_position

print(' ')
print('First position: ')
# adjust one angle (rx) and move to the new position
first_position = [x,y,z,rx+20,ry,rz]
print(first_position)
first_pose = xyzrpw_2_pose(first_position)
robot.MoveJ(first_pose)
print(first_pose)

print(' ')
print('Second position: ')
# adjust a different angle (rz) and move to the new position
second_position = [x,y,z,rx,ry,rz+20]
print(second_position)
second_pose = xyzrpw_2_pose(second_position)
robot.MoveJ(second_pose)
print(second_pose)

This produces the following output:

Code:

First position:
[-50.00000000000013, -1218.2225247665606, 242.834301761225, 20.0, -90.0, 90.0]
Pose(-50.000, -1218.223, 242.834, 90.000, 20.000, 90.000):
[[ 0.000, -0.940, 0.342, -50.000 ],
[ 0.000, -0.342, -0.940, -1218.223 ],
[ 1.000, 0.000, 0.000, 242.834 ],
[ 0, 0, 0, 1 ]]


Second position:
[-50.00000000000013, -1218.2225247665606, 242.834301761225, 0.0, -90.0, 110.0]
Pose(-50.000, -1218.223, 242.834, 90.000, 20.000, 90.000):
[[ -0.000, -0.940, 0.342, -50.000 ],
[ 0.000, -0.342, -0.940, -1218.223 ],
[ 1.000, 0.000, 0.000, 242.834 ],
[ 0, 0, 0, 1 ]]

[Maarten]

Hi William,

What the angular values of [90,20,90] in the "Pose(,,,,,)" line mean following both your angular inputs of [20,-90,90] and [0,-90,110] is unclear to me too.

The 4x4 homogenous transformation matrix below it is however correct: a roll-pitch-yaw of [20,-90,90] yields the same orientation as [0,-90,110]. Why? Because the three different subsequent axial rotations in this case add up to the same orientation.

这个4 x4函数xyzrpw_2_pose输出matrix thus seems correct to me. When calling "print(some_pose)" however, with "some_pose" being a 4x4 matrix output from xyzrpw_2_pose, not only this 4x4 matrix is printed. It seems some other function adds the misunderstood "Pose(,,,,,)" line before it, but I'm not savvy enough to see which.

Maarten

[william.stewart]

Hi Maarten!

Thanks for your reply.

The 4x4 homogenous transformation matrix below it is however correct: a roll-pitch-yaw of [20,-90,90] yields the same orientation as [0,-90,110]. Why? Because the three different subsequent axial rotations in this case add up to the same orientation.

You might indeed be correct. In this case, any thoughts on how to differentiate the between the two?

这个4 x4函数xyzrpw_2_pose输出matrix thus seems correct to me. When calling "print(some_pose)" however, with "some_pose" being a 4x4 matrix output from xyzrpw_2_pose, not only this 4x4 matrix is printed. It seems some other function adds the misunderstood "Pose(,,,,,)" line before it, but I'm not savvy enough to see which.

I'm not very familiar with poses. I thought that maybe it was not just the 4x4 matrix, but included the position in it.