[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: ')
#调整一个角(rx)和移动到新的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 ]]