1
$\begingroup$

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.

$\endgroup$

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.