How to move two UR5 end effectors two a same target POSE

Hello, I am trying to coordinate two UR5 to bring them to same target POSE. The robot1 and robot2 are facing each other. Now both of UR5's are connected to the "world" frame. I have given a target position and orientation for the robot-1 end effector. The robot-1 successfully does the path planning using MOVEIT and comes to the goal position with the desired orientation. Now for the robot-2, I understand that if I want to move the robot-2 to go near the same target I can give the same position but I am not sure how to deal with the orientation. How shall I create a representation from the orientation of the first robot so that I can move the second robot to the same position with a mirrored sort of orientation (because the robot2 is facing the robot1) so that it can grab the item from the first robot?

Note: The robots are facing each other

edit retag close merge delete

1

Quick comment: take tgt pose, rotate by appropriate amount of degrees (over one of the axis, or could need rotations over multiple axes, depending on how it's oriented), then transform pose from robot1_base to robot2_base?

Sort by » oldest newest most voted

You seem to be asking how to manipulate quaternions to change orientations/rotations of poses. The easiest way to do this in ROS is to use the TF quaternion object (or the TF2 version).

You can create a TF quaternion from the geometry_msgs orientation, multiply two rotations, and then extract the geometry_msgs Quaternion.

Assuming that your PoseStamped object is called p, in C++ use:

tf::Quaternion q_old(p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w);
tf::Quaternion q_new, q_rotate;
q_rotate.setRPY(0, 0, M_PI);
q_new = q_old*q_rotate;
tf::quaternionTFToMsg(q_new, p.pose.orientation);

In Python, import tf.transformations and do:

q_old = [p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w]
q_rotate = tf.transformations.quaternion_from_euler(0, 0, pi)
q_new = tf.transformations.quaternion_multiply(q_old, q_rotate)
p.pose.orientation = geometry_msgs.msg.Quaternion(*q_new)
more