Ask Your Question
0

how to: geom calcs in ROS

asked 2013-03-05 01:35:14 -0500

Claudio gravatar image

updated 2013-03-05 02:32:57 -0500

AHornung gravatar image

I'm in the need to do geometry calculations (matrix_by_matrix, vector_by_matrix, scalar_by_vector, and so on).

I looked up the documentation but the vector class isn't documented at all, and doing something like

tf::Vector3 myvect = vect1 + vect2;

results in error, as well as

tf::Vector3 myvect;
myvect.x = 3.5;

or

tf::Vector3 myvect(1,2,3);
myvect = myvect * 2;

So Vector3 appears to be unusable. Why are all those types there if I eventually must use Eigen?

Here is my code:

    tf::StampedTransform transform1, transform2, transform3;
    tf::Vector3 vactual1, vactual2, vactual3, cmd1, cmd2, cmd3, *tmp1, *tmp2, *err;
    tf::Quaternion qactual1,qactual2,qactual3;
    double roll,pitch,yaw, dist12, dist13, dist23, factor;
    while (n.ok())
    {        

        try{
            listener.lookupTransform("world", "/robot_1/base_link", ros::Time(0), transform1);
            listener.lookupTransform("world", "/robot_2/base_link", ros::Time(0), transform2);
            listener.lookupTransform("world", "/robot_3/base_link", ros::Time(0), transform3);
        vactual1 = transform1.getOrigin();
        vactual2 = transform2.getOrigin();
        vactual3 = transform3.getOrigin();
        qactual1 = transform1.getRotation();
        qactual2 = transform2.getRotation();
        qactual3 = transform3.getRotation();
        tf::Matrix3x3(qactual1).getRPY(roll, pitch, yaw);
        dist12 = (powf(vactual2.x()-vactual1.x(),2)+powf(vactual2.y()-vactual1.y(),2));
        dist13 = (powf(vactual3.x()-vactual1.x(),2)+powf(vactual3.y()-vactual1.y(),2));
        dist23 = (powf(vactual3.x()-vactual2.x(),2)+powf(vactual3.y()-vactual2.y(),2));

    //compute error correction for node 1
    err = new tf::Vector3(vactual2.x() - vactual1.x(),vactual2.y() - vactual1.y(), 0);
    factor = - 0.5 * (dist12 - 1.0);
    tmp1 = new tf::Vector3(factor * err->x(), factor * err->y(), 0);
    err = new tf::Vector3(vactual3.x() - vactual1.x(), vactual3.y() - vactual1.y(), 0);
    factor = - 0.5 * (dist13 - 1.0);
    tmp2 = new tf::Vector3(factor * err->x(), factor * err->y(), 0);
    cmd1 = *tmp1 + *tmp2;

    //compute error correction for node 2
    err = new tf::Vector3(vactual1.x() - vactual2.x(), vactual1.y() - vactual2.y(), 0);
    factor = - 0.5 * (dist12 - 1.0);
    tmp1 = new tf::Vector3(factor * err->x(), factor * err->y(), 0);
    err = new tf::Vector3(vactual3.x() - vactual2.x(), vactual3.y() - vactual2.y(), 0);
    factor = - 0.5 * (dist23 - 1.0);
    tmp2 = new tf::Vector3(factor * err->x(), factor * err->y(), 0);
    cmd2 = *tmp1 + *tmp2;

    //compute error correction for node 3
    err = new tf::Vector3(vactual2.x() - vactual3.x(), vactual2.y() - vactual3.y(), 0);
    factor = - 0.5 * (dist23 - 1.0); 
    tmp1 = new tf::Vector3(factor * err->x(), factor * err->y(), 0);
    err = new tf::Vector3(vactual1.x() - vactual3.x(), vactual1.y() - vactual3.y(), 0);
    factor = - 0.5 * (dist13 - 1.0);
    tmp2 = new tf::Vector3(factor * err->x(), factor * err->y(), 0);
    cmd3 = *tmp1 + *tmp2;

    }
    catch (tf::TransformException ex){
            ROS_ERROR("%s",ex.what());
        }


    std::cout << "Robot 1 -> X: " << vactual1.x() << "  Y: " << vactual1.y() << "  T: " << yaw << std::endl;
    std::cout << "Formation : [" << dist12 << " , " << dist13 << " , " << dist23 << "]" << std::endl;
        ros::spinOnce();
        r.sleep();
    }

As is evident from the above I had to find workarounds for many of the operations: scalar_by_vector wasn't accepted, and not knowing how to set a member I had to move math operations inside instantiation instructions.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2013-03-05 05:07:44 -0500

tfoote gravatar image

If you want a generic linear math library Eigen is the recommended library.

The tf/bullet LinearMath library is designed for a limited purpose and works well for that purpose. Feel free to use it or not as you see fit. Just because it does not have operator overloading does not mean that it's broken. It's a design choice made by the developers.

edit flag offensive delete link more

Comments

Shouldn't the operator overloading work? My currently installed header defines for example: TFSIMD_FORCE_INLINE Vector3 operator*(const Vector3&amp; v, const tfScalar&amp; s)

dornhege gravatar image dornhege  ( 2013-03-05 05:29:28 -0500 )edit

Please, what does TFSIMD_FORCE_INLINE mean?

BluPlana gravatar image BluPlana  ( 2017-01-17 02:12:39 -0500 )edit
1

@BluPlana this is an old resolved question. If you have a question of your own please ask it separately.

tfoote gravatar image tfoote  ( 2017-01-17 02:36:45 -0500 )edit
0

answered 2013-03-05 01:52:05 -0500

dornhege gravatar image

updated 2013-03-05 01:53:00 -0500

Yes, it seems the Vector3 doc is broken. I don't really know what the best place to file a ticket for that would be (maybe rosdoc_lite?). I currently use the Vector3.h in /opt/ros/fuerte/stacks/geometry/tf/include/tf/LinearMath as documentation.

For setting values, you use vec.setX(...).

All the other things should actually work. Multiplying transforms definitely work as I'm using that all the time. What errors do you get?

edit flag offensive delete link more

Comments

I'll edit the original question with code and errors. Ok done. BTW I don't know where to put the bug report, should I still use kforge? The geometry stack apparently hasn't been moved to github yet.

Claudio gravatar image Claudio  ( 2013-03-05 02:09:21 -0500 )edit

According to Tully's reply on ros-users this should go to geometry, so I opened a ticket for the missing doc: https://code.ros.org/trac/ros-pkg/ticket/5617

dornhege gravatar image dornhege  ( 2013-03-05 05:14:58 -0500 )edit

For your actual code: It seems like you want relative movement/transforms between the three that you are getting. You can do that directly by using tf13 = transform1.inverseTimes(transform2). The distance would then be tf13.getOrigin().length(). There is a slight overhead also computing rotation

dornhege gravatar image dornhege  ( 2013-03-05 05:33:23 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2013-03-05 01:35:14 -0500

Seen: 180 times

Last updated: Mar 05 '13