Ask Your Question
0

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

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

Tawfiq Chowdhury gravatar image

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 ... (more)

edit retag flag offensive 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".

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

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-03-19 06:43:41 -0500

fvd gravatar image

updated 2019-03-19 06:46:31 -0500

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.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

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

Seen: 38 times

Last updated: Mar 19