Ask Your Question

# How to align PR2's gripper with object's orinetation using rotation?

I am working on a translation and rotation project. So, the idea is an object will be in two different positions and orientations. Let's assume the orientation of a long rectangular object was initially horizontal, then it was rotated to make it vertical. So, what I need to do, I will take the PR2's gripper in a good orientation aligned with the object when it is in horizontal orientation. I would save the relative orientation. When the object is in vertical orientation, I will apply a rotation using the saved relative orientation to the old orientation of the gripper to get a new aligned orientation of the gripper. I have tried something similar:

  #Object in first orientation
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.82
p.pose.position.y = 0.30
p.pose.position.z = 1.20
p.pose.orientation.x=0.0
p.pose.orientation.y=0.0
p.pose.orientation.z=0.0
p.pose.orientation.w=1.0
scene.add_box("object1", p, (0.1, 0.5, 0.1))
x1=p.pose.position.x
y1=p.pose.position.y
z1=p.pose.position.z

rospy.sleep(2)

quaternion = (p.pose.orientation.x,p.pose.orientation.y,p.pose.orientation.z,p.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
roll1=euler[0]
pitch1=euler[1]
yaw1=euler[2]

#Gripper pose1
pose_source = geometry_msgs.msg.Pose()
pose_source.orientation.x = 0.5
pose_source.orientation.y = 0.5
pose_source.orientation.z = 0.5
pose_source.orientation.w = 0.5
pose_source.position.x = 0.68
pose_source.position.y = -0.01
pose_source.position.z = 1.1
#group.set_planning_time(10);

X=pose_source.position.x
Y=pose_source.position.y
Z=pose_source.position.z

quaternion = (pose_source.orientation.x,pose_source.orientation.y,pose_source.orientation.z,pose_source.orientation.w)
euler_G = tf.transformations.euler_from_quaternion(quaternion)

#Object second pose
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.75
p.pose.position.y = -0.1
p.pose.position.z = 0.75
p.pose.orientation.x=0.7071068
p.pose.orientation.y=0.0
p.pose.orientation.z=0.0
p.pose.orientation.w=0.7071068
scene.add_box("object1", p, (0.1, 0.5, 0.1))
x2=p.pose.position.x
y2=p.pose.position.y
z2=p.pose.position.z

rospy.sleep(2)

dx=x1-x2
dy=y1-y2
dz=z1-z2

quaternion = (p.pose.orientation.x,p.pose.orientation.y,p.pose.orientation.z,p.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
roll2=euler[0]
pitch2=euler[1]
yaw2=euler[2]

droll= (roll1-roll2)
dpitch= (pitch1-pitch2)
dyaw= (yaw1-yaw2)

Pose=np.array([0.0,0.0,0.0])
Pose[0]=droll+euler_G[0]
Pose[1]=dpitch+euler_G[1]
Pose[2]=dyaw+euler_G[2]

quaternion = tf.transformations.quaternion_from_euler(Pose[0], Pose[1], Pose[2])

qx = quaternion[0]
qy = quaternion[1]
qz = quaternion[2]
qw = quaternion[3]


qx, qy, qz, qw are my final pose. There is no way to set RPY in moveit as orientation so I used quaternion and euler transformation and vice versa. Ligically ...

edit retag close merge delete

## Comments

This would be easier if you posted images of what you expected and what is happening instead of "it is not working".

( 2019-03-19 06:34:52 -0500 )edit

## 1 Answer

Sort by » oldest newest most voted

1) To start with, you can't combine rotations by subtracting the Euler angles. You need to apply the inverse rotations in order.

2) It is unclear what you mean by "relative orientation". Relative to what? In which coordinate system is it defined?

3) For cylindrical objects, you may want to consider some form of systematic grasp candidate generation, like this. If you define the orientation and position in your object's coordinate system, it should not be particularly difficult to try a number of them out and see if the planning succeeds.

For general questions about rotation, I suggest publishing markers like this or this coordinate system to Rviz so you can visually understand what is happening.

more

## Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

## Stats

Asked: 2019-03-02 20:06:05 -0500

Seen: 38 times

Last updated: Mar 19