Question

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

Asked by mkb_10062949 on 2020-02-04 17:27:20 UTC

Answer 1

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)

Asked by fvd on 2020-02-26 21:15:13 UTC

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

Comments

Answers

Comments