Ask Your Question
0

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

asked 2020-02-04 16:27:20 -0500

mkb_10062949 gravatar image

updated 2020-02-05 08:04:43 -0500

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 flag offensive close merge delete

Comments

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?

gvdhoorn gravatar image gvdhoorn  ( 2020-02-05 08:59:13 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-02-26 20:15:13 -0500

fvd gravatar image

updated 2020-02-26 20:20:56 -0500

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)
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

2 followers

Stats

Asked: 2020-02-04 16:27:20 -0500

Seen: 38 times

Last updated: Feb 26