How to transform a vector in rospy?
I couldn't find any tutorials for my situation.
I have a vector (23.7, 0, 0) and I want to transform (only rotation no translation needed) it into another frame which is RPY (0.39, 0, 1.64).
I'm trying with tf_conversions.posemath but couldn't get the correct answer which is nearly (-1.8, -21.8, 9.1). Any ideas?
Code:
current_pose_rpy = tf.transformations.euler_from_quaternion(quaternion) # which is (0.39, 0, 1.64).
tf_val = PyKDL.Frame(PyKDL.Rotation.RPY(*current_pose_rpy), PyKDL.Vector(0, 0, 0))
vel_val = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0), PyKDL.Vector(23.7, 0, 0))
transformed_vel = tf_val * vel_val
Which is giving around what user janindu commented.
But intuitively z component should be present which I'm not getting via this method.
Original problem:
My state estimator gives pose in state estimator frame (starting point as 0, 0, 0) and twist in state estimator frame. I want twist in base_link or drone frame as one opensource MPC controller takes base link twist. So, need to convert global velocity to local drone velocities.
Tried these two solutions but still getting similar output as my code above.
1. https://answers.ros.org/question/1961...
2. https://answers.ros.org/question/1922...
Like when drone is rotated 90 deg left yaw and moving it's own right, we get those global velocity values of (23.7, 0, 0). In this instance, drone maintains constant roll, pitch, yaw which are (0.39, 0, 1.64) derived from pose.pose.orientation of odom message using tf.transformation.euler_from_quaternion(quaternion).
Intuitively, as the drone is yaw left, rolled body right and going body right, that global x velocity should have both y, z components in drone frame. But z component is 0 when tried all these methods.
Am I doing something wrong in rotations?
And if I modify my code like this and transforming individually in each axis it is working but some velocities should be multiplied by -1 but values are correct. I mean I'm getting z component of velocity. I'm really confused and stuck here for two days.
current_pose_rpy = tf.transformations.euler_from_quaternion(quaternion) # which is (0.39, 0, 1.64) .
tf_val = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, current_pose_rpy[2]), PyKDL.Vector(0, 0, 0))
tf_val_2 = PyKDL.Frame(PyKDL.Rotation.RPY(0, current_pose_rpy[1], 0), PyKDL.Vector(0, 0, 0))
tf_val_3 = PyKDL.Frame(PyKDL.Rotation.RPY(current_pose_rpy[0], 0, 0), PyKDL.Vector(0, 0, 0))
vel_val = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0), PyKDL.Vector(23.7, 0, 0))
transformed_vel_val = tf_val * vel_val
transformed_vel_val = tf_val_2 * transformed_vel_val
transformed_vel_val = tf_val_3 * transformed_vel_val
Are you sure (-1.8, -21.8, 9.1) is the correct answer? Your vector is along the X axis so its immune to any rotations about the X axis (roll). You have zero pitch (rotation about Y axis). Yaw (rotation about Z axis can't change the Z component of the vector so the Z component of the answer should be zero. The result I got was
Specifically, what have you done (i.e., code)? Can you please update your question with the code that you're using
Thanks for reply guys.
I've modified the question and added code and more info of what I'm exactly trying to solve. But couldn't figure out how to put a code block.
I'm thinking the issue is my understanding of how Euler angles work. I can not put it in the code but intuitively see that z component should be there. And I'm also confused that transforming roll first and then yaw gives different output than doing yaw first and roll later (in my second code where I transformed individually each rotation). I'm really confused here.
Possibly a silly comment, but are you sure that gimbal lock isn't a factor here?
Are you able to do the calculations in quaternion or rotation matrix form first, then turn them back into Euler angles? And does this give the answer you expect?
I don't think so. One Odom values (ground truth with local velocities) are from the gazebo simulator and another one are from my estimator. Everything is software and everything gives outputs as expected. Except for this transformation.
I tried those two links posted in question where they used quaternions and still the output is the same as mine. But different than what I wanted.