UR5 rx,ry,rz reference frame

[Jacco van der Spek]

Hi all,

For my master thesis project at Alten Mechatronics/TU Delft  I'm looking into imitation learning.

Therefore I captured trajectories that I performed using the teach modus of the UR5. This was done by a Matlab driver for the UR5 that was developed at the TU Delft. 

Now I would like to do the learning in the cartesian space and use ROS for the control of the UR5.

However I'm now running into the problem of converting the data that I captured to ROS. Especially I can't figure out setting the quaternion that is needed for the Pose.

The x,y,z values is no problem. I also know that a quaternion can be set by a angle-axis representation which are the rx,ry,rz values given by the UR5.

The problem is on what frame are the rx,ry,rz defined? 

If I use the frame that is used by the UR5 (not the base_link of ROS but the base_link with a rotation because x and y axis are different), I don't get the right quaternions. 

Because I also know the joint positions, I can set the robot to that pose and then do getCurrentPose() to get the quaternions. In this way I can check if I computed the same quaternions by setRotation(vector,angle). 

So does anyone know how to use rx,ry,rz in ROS for setting the quaternion? 

Another solution might be to capture the trajectories using ROS however from what I know teach modus does not work once ROS is connected. 

For clarity the control screen of the UR5:

X = red(towards you), y=green(to the right, direction of wire), z=blue(up)

Does anyone have any input on this? You would be my personal hero if you have anything useful ;)

[Felix Messmer (IPA)]

I am not so sure what you are looking for exactly.
For simply transforming and converting different representations for transformations the tf package has all the functions you need (C++ and Python).
For example in  http://docs.ros.org/hydro/api/tf/html/c++/classtf_1_1Quaternion.html there are setEuler(roll, pitch, yaw) or setRotation(axis, angle) functions.
For transforming coordinate systems, the TransformListener http://mirror.umd.edu/roswiki/doc/diamondback/api/tf/html/c++/classtf_1_1TransformListener.html has various transform* funcitons
In order to convert geometry_msgs representation to tf types you can use http://docs.ros.org/api/tf/html/c++/transform__datatypes_8h.html

Hope that helps

[Jacco van der Spek]

The problem is not in doing the transformations. I know all these packages exist and I can work with them. 

However when I use the ur5_base_link as defined on the control panel and use setRotation(rotation,angle) where rotation is the normalized [rx ry rz] vector and angle is the norm of the [rx ry rz] vector. Then I don't get the right quaternions. 

So my question is, where is this going wrong and how do I get the right quaternions from [rx ry rz]? 

--
You received this message because you are subscribed to a topic in the Google Groups "swri-ros-pkg-dev" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/swri-ros-pkg-dev/WHbKhHjn_AM/unsubscribe.
To unsubscribe from this group and all its topics, send an email to swri-ros-pkg-d...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Jacco van der Spek]

Ok so I solved it.

Here is the procedure you have to do to get from rx,ry,rz of the UR5 (so what is on the screen) to the right pose in ROS.

Pseudo code / Matlab for preparing the right values:

vector = [rx; ry; rz]

angle = norm(vector)

vector = vector/angle

C++ code:

Note that vector(1),vector(2),vector(3),angle are the values from above.

    tf::Quaternion quat;

    tf::Quaternion quat_inv(tfScalar(-0.5),tfScalar(0.5),tfScalar(-0.5),tfScalar(-0.5));

    tf::Vector3 vector(vector(1),vector(2),vector(3));

    tfScalar angle = angle;

    quat.setRotation(vector,angle);

    quat *= quat_inv;

    geometry_msgs::PoseStamped target_pose;

    target_pose.header.frame_id = "/matlab_base_link";

    target_pose.pose.position.x = -0.0759; //0.3

    target_pose.pose.position.y = -0.4954; //-0.5

    target_pose.pose.position.z = 0.2786; //0.55

    target_pose.pose.orientation.x = quat.getX();

    target_pose.pose.orientation.y = quat.getY();

    target_pose.pose.orientation.z = quat.getZ();

    target_pose.pose.orientation.w = quat.getW();

The matlab_base_link is defined as follows (frame as defined on UR5):

    //Matlab base_link

    base_link_tf.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );

    tf::Quaternion q1;

    q1.setRPY(0,0,M_PI);

    base_link_tf.setRotation(q1);

    br.sendTransform(tf::StampedTransform(base_link_tf, ros::Time::now(), "base_link", "matlab_base_link"));

On Friday, 4 July 2014 08:16:58 UTC+2, Jacco van der Spek wrote:

The problem is not in doing the transformations. I know all these packages exist and I can work with them. 

However when I use the ur5_base_link as defined on the control panel and use setRotation(rotation,angle) where rotation is the normalized [rx ry rz] vector and angle is the norm of the [rx ry rz] vector. Then I don't get the right quaternions. 

So my question is, where is this going wrong and how do I get the right quaternions from [rx ry rz]? 

To unsubscribe from this group and all its topics, send an email to swri-ros-pkg-dev+unsubscribe@googlegroups.com.

[Shaun Edwards]

Jacco,

It sounds like you solved your problem, but I thought I might add that we are trying to add frames to the URDF that are the same as those stored in the controller.  This allows us to compare (without any transformations), the positions reported by Rviz and the robot controller itself.  I'm not sure if this addresses your matlab issue, but it may.  

These additional frames are spelled out in this URDF tutorial.  The UR doesn't yet have these frames in it's URDF, but if you add them, it would be great if you could submit them back in a pull request.

Shaun

--
You received this message because you are subscribed to the Google Groups "swri-ros-pkg-dev" group.
To unsubscribe from this group and stop receiving emails from it, send an email to swri-ros-pkg-d...@googlegroups.com.

[Jacco van der Spek]

Shaun,

I can add the base as described in the tutorial. I wasn't able to fix the issue described above with transformations. I might look into this.

Atm I don't have time for both as it is no priority for me now. Maybe I will look into it next month after I handed in my thesis ;)

The transformation for base is:

//Matlab base_link

    base_link_tf.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );

    tf::Quaternion q1;

    q1.setRPY(0,0,M_PI);

    base_link_tf.setRotation(q1);

    br.sendTransform(tf::StampedTransform(base_link_tf, ros::Time::now(), "base_link", "matlab_base_link"));

Where matlab_base_link is the base as defined by the controller. 

To unsubscribe from this group and all its topics, send an email to swri-ros-pkg-d...@googlegroups.com.

For more options, visit https://groups.google.com/d/optout.