How to access the positions[] in a trajectory_msgs/JointTrajectoryPoint[] message?

[Joris Beuls]

Hello

I have planned a path using MoveIt!, now i want to access the points (positions and quaternions) in the path. A code snippet i use is displayed beneath. The program crashes when it enters the FOR loop. Does anyone know how i can easily access the points from a planned path. The goal is to send them over TCP to another pc.

moveit::planning_interface::MoveGroup group("epson_arm");
moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan);

if (success == true)
    {
        group.execute(my_plan);
        msg = my_plan.trajectory_; // get the trajectory from the path [1]

        std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory_points;
        trajectory_points = msg.joint_trajectory.points;  //Can be seen in [2]

        std::vector<int>::size_type vectorSize = trajectory_points.size();  //Check if there is something in the array
                                                                                                             
        ROS_INFO("The size of the vectorSize = %i", vectorSize);

        std::vector<float> position;

                for (unsigned i=0; i<vectorSize; i++)
                {
                    position[i] = msg.joint_trajectory.points[i].positions[0];
                    ROS_INFO("Floatvalue for position = %f", position[i]);
                }
    }

The error i get is:
[ INFO] [1432813934.122067037]: Loading robot model 'epsonURDF_gripper'...
[ INFO] [1432813934.122121138]: No root joint specified. Assuming fixed joint
[ INFO] [1432813936.813170547]: Ready to take MoveGroup commands for group epson_arm.
[ INFO] [1432813940.419978115]: The size of the vectorSize = 10
[motion_planning_epson-1] process has died [pid 27236, exit code -11

[1] http://docs.ros.org/hydro/api/moveit_ros_planning_interface/html/structmoveit_1_1planning__interface_1_1MoveGroup_1_1Plan.html#ad56843467e53094cbcaac2024ab032c5
[2] http://docs.ros.org/hydro/api/moveit_msgs/html/msg/RobotTrajectory.html

Best regards
Joris

[G.A. vd. Hoorn - 3ME]

On 28-5-2015 14:01, Joris Beuls wrote:
> Hello
[..]
> std::vector<float> position;
>
> for(unsigned i = 0; i < vectorSize; i++)

> {
> position[i] = msg.joint_trajectory.points[i].positions[0];
> ROS_INFO("Floatvalue for position = %f", position[i]);
> }

[..]

This is not really moveit related, but:

your 'position' vector is zero-length. You'll need to reserve some space
using std::vector::resize(..) or use std::vector::push_back(..).


Gijs

[Joris Beuls]

Ty this resolved the issue, i thought a vector changed its size dynamically.

Sorry for the trouble.

[G.A. vd. Hoorn - 3ME]

On 28-5-2015 14:18, Joris Beuls wrote:
> Ty this resolved the issue, i thought a vector changed its size dynamically.

it does, but not if you use the subscript / index operator ('[n]') like
you did. Then you have to first give the vector a size. It's essentially
like indexing an array.

If you use std::vector::push_back(..), std::vector will automatically
grow its internal store to accommodate new elements.

[Joris Beuls]

it does, but not if you use the subscript / index operator ('[n]') like
you did. Then you have to first give the vector a size. It's essentially
like indexing an array.

If you use std::vector::push_back(..), std::vector will automatically
grow its internal store to accommodate new elements.

This works like a charm now:

std::vector<int>::size_type size1 = msg.joint_trajectory.points.size();
        ROS_INFO("The amount of points = %i", size1);

        std::vector<float> position;
        //position.resize(vector1);
        int k = 0;
                for (unsigned i=0; i<size1; i++)
                {
                    std::vector<int>::size_type size2 = msg.joint_trajectory.points[i].positions.size();
                    for (unsigned j=0; j<size2; j++)
                    {
                        position.push_back(msg.joint_trajectory.points[i].positions[j]); // positions[j]was eerst [0]
                        k++;
                        ROS_INFO("Positions for point = %i", i);
                        ROS_INFO("Floatvalue for position = %g", position[k]);
                    }
                }

The result is:
[ INFO] [1432817006.450464893]: Positions for point = 28
[ INFO] [1432817006.450532289]: Floatvalue for position = 1.83943e+25
[ INFO] [1432817006.450596145]: Positions for point = 28
[ INFO] [1432817006.450664772]: Floatvalue for position = 1.21219e+25
[ INFO] [1432817006.450728993]: Positions for point = 28
[ INFO] [1432817006.450797004]: Floatvalue for position = 6.85117e+22
[ INFO] [1432817006.450864830]: Positions for point = 28
[ INFO] [1432817006.450937107]: Floatvalue for position = 1.12895e+27
[ INFO] [1432817006.451006986]: Positions for point = 28
[ INFO] [1432817006.451079420]: Floatvalue for position = 7.33872e+28
[ INFO] [1432817006.451149090]: Positions for point = 28
[ INFO] [1432817006.451222508]: Floatvalue for position = 1.74094e+25

These points have a rather ridiculously high value, How can i know what value this has? (like meters, milimeters) since [3] does not tell what values it has.

[3] http://docs.ros.org/hydro/api/trajectory_msgs/html/msg/JointTrajectoryPoint.html
 

[G.A. vd. Hoorn - 3ME]

As you can see in [4], float64 is mapped onto double in c++, not float.

And as you can find in [5], all angles in ROS (and certainly in a
JointTrajectoryPoint, are in radians.


Gijs

PS: why do you not just copy the entire positions vector in its
entirety, but element-wise?


> [3] http://docs.ros.org/hydro/api/trajectory_msgs/html/msg/JointTrajectoryPoint.html

[4] http://wiki.ros.org/msg
[5] http://www.ros.org/reps/rep-0103.html

[Joris Beuls]

Because i want to understand what i am doing and what values i am working with before i start to use them. When i understand i will read it in it's entirety.

It could be that i am just misunderstanding ROS but since this is the first time i use these messages i am easily confused. The documentation from [3] didn’t specify how many values where in the positions array. That is why i wanted to check. Now it gives 6 values per point, this doesn't tell me if it is (x,y,z, rotation x, rotation y, rotation z) or something else.
Then when i read the values it gives me huge numbers, changing my float array to double array (like in [4] is said) gave me even larger numbers.

Is there something basic about looking for this kind of information and how to read messages properly that i am missing?

Op donderdag 28 mei 2015 14:57:42 UTC+2 schreef gavanderhoorn:

[Dave Hershberger]

There already exists a function to print an entire ROS message, in the "<<" operator.

You can do:

std::cout << "my message is:" << std::endl;
std::cout << msg << std::endl;

and you will get a nicely-formatted printout of the entire message with all trajectory points, etc.

This is implemented on all message types, so if you don't want the whole thing you can just do a sub-message, like:

std::cout << msg.joint_trajectory << std::endl;

That sort of thing is sometimes easier than writing a bunch of new code to walk through the messages.

Also, the reason you are seeing giant numbers in your output is an indexing problem.  You do:

positions.push_back(point);

and then you do

k++;

and then

print(positions[k]);

After the first push_back() the array length is 1, you do k++ (from 0 to 1) and then you print position[1].  But position[1] has not been set.  position[0] has been set by the push_back().

So just move "k++" after your print statement and you'll be fine.

Dave

[Dave Hershberger]

Also, to understand the JointTrajectory message, you should run (at the shell prompt):

rosmsg show JointTrajectory

The result is:

[trajectory_msgs/JointTrajectory]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string[] joint_names
trajectory_msgs/JointTrajectoryPoint[] points
  float64[] positions
  float64[] velocities
  float64[] accelerations
  float64[] effort
  duration time_from_start

So each JointTrajectoryPoint by itself does not actually contain the information about what the position numbers etc actually mean.  That information is one level up, in the joint_names property of the JointTrajectory message.

The output of a planner is a sequence of robot joint values, not x,y,z values.  So you'll have something like:

joint_names: ['shoulder_yaw', 'shoulder_lift', 'elbow', 'wrist1', 'wrist2', 'wrist3']
point[0]:
  positions: [1.1, 0.4, 1.2, 0.3, -0.3, 0.0]
point[1]:
  positions: [1.2, 0.4, 1.2, 0.3, -0.3, 0.0]
point[2]:
  positions: [1.3, 0.4, 1.2, 0.3, -0.3, 0.0]

The order of values in the "positions", "velocities", "accelerations", and "effort" arrays correspond to the order of the joint_names.

The fact that there are 6 values in each position array comes from the fact that the planning group you're using for your robot has 6 joints.

Dave

[Joris Beuls]

Thank you very much, this was really helpful. I made a very stupid mistake with the k++. Now i understand the message format. I will keep the rosmsg show function in mind for my future work. I would like to ask one more thing.

I am using a Epson C3 robot and for this robot there are no drivers available. To use my robot i would like points and rotations. I tried the "MultiDOFJointTrajectoryPoint" message from "trajectory_msgs", this gives me an empty arry. Therefore i presume that the points from the path are not saved in this type of msg.

Do you have any suggestion on how to convert the rotations from "JointTrajectoryPoint" to points and rotations?

Best regards
Joris

MultiDOFJointTrajectoryPoint.msg

Op donderdag 28 mei 2015 18:43:27 UTC+2 schreef Dave Hershberger:

[G.A. vd. Hoorn - 3ME]

On 29-5-2015 10:15, Joris Beuls wrote:
[..]

> I am using a Epson C3 robot and for this robot there are no drivers
> available. To use my robot i would like points and rotations. I tried the
> "MultiDOFJointTrajectoryPoint" message from "trajectory_msgs", this gives
> me an empty arry. Therefore i presume that the points from the path are not
> saved in this type of msg.
>
> Do you have any suggestion on how to convert the rotations from
> "JointTrajectoryPoint" to points and rotations?

Perhaps more of a question than an answer, but the Epson controller also
supports joint moves.

JointTrajectoryPoints map very nicely onto joint moves on industrial
controllers (so almost no conversion needed) and you also avoid all
sorts of nastiness with singularities. Do you have a particular reason
for wanting to work with in Cartesian space?

If you must (because of a requirement to perform linear moves between
waypoints fi), most industrial controllers allow you to use their FK
systems to convert joint positions into Cartesian positions. Perhaps you
could use that on the Epson controller.


Gijs

[Dave Hershberger]

In moveit we generally think of the contents of JointTrajectoryPoint as
corresponding roughly to "robot state", which is always in terms of
joint values.

The "MultiDOFJointTrajectoryPoint" kind of tricks you because it looks
like what you want, but it is not. The purpose of that is for parts of
robots where the control happens directly in cartesian space. In the
past we've used that for the mobile base of the PR2, or I could imagine
it being useful for a quadcopter. It is rare for a robot to directly be
controlled in cartesian space at the lowest level, but sometimes the
software interface to them makes that easier. In any case, for a 6-axis
articulated arm like the Epson C3, we wouldn't typically use it.

So what we generally do is compute our desired poses in cartesian space,
then use moveit::core::RobotState::setFromIK() to solve the inverse
kinematics to get a set of joint values. Then we can use path planning
to figure out how to get from current state to that destination state.

Or if you want the end-effector to move straight in cartesian space, use
RobotState::computeCartesianPath() to essentially do a sequence of IK
calls all along a line.

In the end you'll get a sequence of RobotStates corresponding
(hopefully) to the motion you want.

In the reverse direction, if you are receiving joint values and you want
to know where in cartesian space the end-effector is, you need to put
the joint values into a RobotState instance, then call
RobotState::update() and RobotState::getGlobalLinkTransform().

Hope this helps,
Dave