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

edit

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)