ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

how to rotate a point(x,y) from current orientation to a given orientation

asked 2013-08-21 02:03:49 -0500

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

I saw many api's to convert between euler angles and quaternions but could not find an api which actually performs rotation. Basically i want to rotate a point from current orientation to new orientation.

Could some one please suggest how can i do it in ros groovy ??


I tried as below :

    geometry_msgs::Pose pose1, pose2, pose3;
// point P1 of triangle.
pose1.position.x = cones[0].x1 * map_resolution + map_origin_x ; 
pose1.position.y = cones[0].y1 * map_resolution + map_origin_y;
pose1.position.z = 0.0;
//some assumed orientation
pose1.orientation.x = 0.0;  
pose1.orientation.y = 0.0;
pose1.orientation.z = 1.0;
pose1.orientation.w = yaw; 

// point P2 of triangle.
pose2.position.x = cones[0].x2 * map_resolution + map_origin_x;
pose2.position.y = cones[0].y2 * map_resolution + map_origin_y;
pose2.position.z = 0.0;
//some assumed orientation
pose2.orientation.x = 0.0;  
pose2.orientation.y = 0.0;
pose2.orientation.z = 1.0;
pose2.orientation.w = yaw;

// point P3 of triangle.
pose3.position.x = cones[0].x4 * map_resolution + map_origin_x;
pose3.position.y = cones[0].y4 * map_resolution + map_origin_y;
pose3.position.z = 0.0;
//some assumed orientation
pose3.orientation.x = 0.0;  
pose3.orientation.y = 0.0;
pose3.orientation.z = 1.0;
pose3.orientation.w = yaw;

tf::Transform tf_pose1, tf_pose2, tf_pose3, tf_pose4, tf_pose5, tf_pose6, tf_pose7, tf_pose8, tf_pose9, tf_pose_origin;

// Original created Points:
tf_pose1 = tf::Transform::getIdentity();
tf::poseMsgToTF(pose1, tf_pose1);
br.sendTransform(tf::StampedTransform(tf_pose1, ros::Time::now(), "map", frame_id1));

tf_pose2 = tf::Transform::getIdentity();
tf::poseMsgToTF(pose2, tf_pose2);
br.sendTransform(tf::StampedTransform(tf_pose2, ros::Time::now(), "map", frame_id2));

tf_pose3 = tf::Transform::getIdentity();
tf::poseMsgToTF(pose3, tf_pose3);
br.sendTransform(tf::StampedTransform(tf_pose3, ros::Time::now(), "map", frame_id3));

// Actual given Pose
tf_pose_origin = tf::Transform::getIdentity();
tf::poseMsgToTF(poseArray.poses[0], tf_pose_origin);
br.sendTransform(tf::StampedTransform(tf_pose_origin, ros::Time::now(), "map", frame_id_origin));

// Rotated Points:

// Assign P1 to same as origin since its position and orientation is same as of given pose.
tf_pose4 = tf_pose_origin;
br.sendTransform(tf::StampedTransform(tf_pose4, ros::Time::now(), "map", frame_id4));

// rotate P2
tf_pose5 = tf_pose2.inverseTimes(tf_pose_origin);
br.sendTransform(tf::StampedTransform(tf_pose5, ros::Time::now(), "map", frame_id5));

// rotate P3
tf_pose6 = tf_pose3.inverseTimes(tf_pose_origin);
br.sendTransform(tf::StampedTransform(tf_pose6, ros::Time::now(), "map", frame_id6));

// Another try reversing the order of multiplication:

// Assign P1 to same as origin since its position and orientation is same as of given pose.
tf_pose7 = tf_pose_origin;
br.sendTransform(tf::StampedTransform(tf_pose7, ros::Time::now(), "map", frame_id7));

// rotate P2
tf_pose8 = tf_pose_origin.inverseTimes(tf_pose2);
br.sendTransform(tf::StampedTransform(tf_pose8, ros::Time::now(), "map", frame_id8));

// rotate P3
tf_pose9 = tf_pose_origin.inverseTimes(tf_pose3);
br.sendTransform(tf::StampedTransform(tf_pose9, ros::Time::now(), "map", frame_id9));

Even though the position of P2 and P3 changed they were not as expected. The triangle in total was not in the required orientation :( Any idea ??

Or atleast any idea how to create a triangle given a pose and height(assume distance between P2 and P3 is equal to distance between P1 and midpoint of P2 and P3)


class Cone { public: float x1, y1, x2, y2, x3, y3, x4, y4; float radius; float height; float angle; // not ... (more)

edit retag flag offensive close merge delete


Maybe a different question: What is your input to which goal? Do you want global/map coordinates for your cones? It looks like you're using global inputs in the cones[0]. It's quite straight-forward to only define a cone robot-relative and then transform that to map coords (although just the inverse

dornhege gravatar image dornhege  ( 2013-08-21 23:04:14 -0500 )edit

I have updated some more code and details.

kk gravatar image kk  ( 2013-08-22 01:54:13 -0500 )edit

3 Answers

Sort by ยป oldest newest most voted

answered 2013-08-24 05:42:57 -0500

kk gravatar image

I got it working. Instead of creating points in global frame, i created with respect to the given pose frame. And then looked up between the global and local frames to find the exact points.

edit flag offensive delete link more

answered 2017-02-10 07:11:32 -0500

Wiew gravatar image

updated 2017-02-10 07:13:18 -0500

In my research i used a DCM Matrix /Matrix of direct cosine/. This matrix provides (2D or 3D rotation) of point. This method is very simply, but you need matrix calculations. On wikipedia you are able to find how to transform data. In our paper the example of matixies and calculations are shown.

Blockquote Blockquote

edit flag offensive delete link more

answered 2013-08-21 06:14:01 -0500

dornhege gravatar image

Depends how you have your point and orientation.

If it is tf data types, you can just make a transform out of the orientation and transform the point with that. geometry_msgs can be easily transformed to tf with helper functions.

If it is just a 2d x,y and theta, I'd probably just do it by hand.

edit flag offensive delete link more


Thank you for the comment. Its is in geometry_msgs:Pose type.. plz check this for more details of my query.

kk gravatar image kk  ( 2013-08-21 06:19:21 -0500 )edit

In that case, just convert them to tf data types. Check the API, the functions are called something like: `tf::poseMsgToTF(msg, tfPose);`. You can then use intuitive math like `p2.inverseTimes(p1)` to get relative poses.

dornhege gravatar image dornhege  ( 2013-08-21 06:46:03 -0500 )edit

I tried but not working correctly. I have updated the my question with some code snippets that i tried.

kk gravatar image kk  ( 2013-08-21 13:04:12 -0500 )edit

Question Tools



Asked: 2013-08-21 02:03:49 -0500

Seen: 3,900 times

Last updated: Feb 10 '17