Lets say I have 2d poses as such (angles in degrees):

world_to_robot = {0, 4, 45} robot_to_object = {4, -4, -45}

From a graph it would be easy to see that:

world_to_object = {4, 8, theta}, what goes in for theta? atan2(8, 4)? What if the object has a world heading?

Ultimately this will need done in a format that is implemented in code and usable with any list of poses. So I would have as such for a c++ implementation,

Eigen::Matrix<float, 3, 1> world_to_robot;
Eigen::Matrix<float, 3, 1> world_to_object;
Eigen::Matrix<float, 3, 1> robot_to_object;

// My omniscient poses below from my fake scenario
world_to_robot << 0, 4, M_PI/2;
robot_to_object << 4, -4, -M_PI/4;
wold_to_object << 4, 8, some_theta_rad;   // place holder for whatever this ends up being

Eigen::Matrix<float, 3, 3> rotationM;
rotationM << cos(M_PI/2), -sin(M_PI/2), 0,
             sin(M_PI/2,   cos(M_PI/2), 4,
                  0,             0,     1;

Eigen::Matrix<float, 3, 1> robot_to_objectM;
robot_to_objectM << 4, -4, 1;

Eigen::Matrix<float, 3, 1> calculated_world_to_object = rotationM*robot_to_objectM;

This will give me my expected world_to_object x and y pose for object wrt the global frame, but what would some_theta_rad be?

Then lets say I have the scenario where I have world_to_object and robot_to_object. Also assume I know theta for world_to_object because we identified the object based on a signature and there is an absolute pose. I want to calculate world_to_robot. Let's use the same poses as above.

I know that to get world_to_robot, I need to do a transform consisting of world_to_object * object_to_robot. I do not have object_to_robot. I need the inverse of the robot_to_object, which is not square. Here is what I have done in this scenario:

Eigen::Matrix<float, 3, 3> rotationM;
rotationM << cos(theta_rad), -sin(theta_rad), 4,
             sin(M_PI/2,     cos(theta_rad),  8,
                  0,           0,             1;    // What is theta_rad? Just the world_to_object theta?

Eigen::Matrix<float, 3, 1> robot_to_objectM;
robot_to_objectM << -4, 4, 1;               // Note: I inverted the signs for the translation

Eigen::Matrix<float, 3, 1> calculated_world_to_robot = rotationM*robot_to_objectM;

This seems to give the correct new x and y pose if I use world_to_robot heading for theta. However there could be a scenario where this is not known ahead of time. So how would I correctly get my theta value? Also, I know the new robot pose needs to be M_PI/2, but how do I get this from my last piece of code segment. Typically I would start doing a bunch of "atan2(y,x)", but am missing the key "theta's" in the code implementations above.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.