Ex3: Forward direction not updating

[leopoldsedev]

I am currently doing exercise 3 ("Trajectory Planning (baseline)"). As I understand it, in bFrameReferenceMotionPlan::generate() we are supposed to use robot->forward to get the forward direction of the robot. However, after the robot turns, this vector remains at its original value. Should this vector not change if the heading changes? I could of course update it in bFrameReferenceMotionPlan::generate() myself, but I am not sure if that is what I am supposed to do or if I am misunderstanding the purpose of the robot->forward vector. Maybe it would be possible to clarify this?

Thanks!

[eastskykang]

@leopoldsedev hello,

robot->forward vector is a vector expressed in robot's bframe, not world frame. Thus, you have to take into account robot's orientation to correctly generate a future trajectory.

Quaternion heading = getRotationQuaternion(headingAngle, V3D(0, 1, 0));

at the top of the while loop block, computes the orientation of the base. You can use this quaternion variable.

Note. do not update robot->forward. This is deliberately chosen to be expressed in the bframe.

[iqiac]

Sorry, I'm unfamiliar with quaternions. How are we supposed to use this to change the moving direction of the robot? Is any comments in the code or theory on the slides? My robot also moves only in one direction but it can "turn" resp. spin, although the legs start to get fancy if I spin it.

[eastskykang]

@iqiac I cannot say too much details because it could be directly related to the task. Please take a look this diagram. Your task is incrementing position and heading angle accordingly.

Quaternion is nothing but a rotation representation. You can consider it as the same thing (but differently parameterized) with a rotational matrix R. Here Quaternion heading = getRotationQuaternion(headingAngle, V3D(0, 1, 0)); is the (future) orientation of the robot's base with respect to the world frame. More formally, this can be expressed as R_WB

Thus, you can get forward direction vector of the robot expressed in the world frame v_W by v_W = R_WB v_B

In cpp, this can be simply implemented as

V3D forward_W = heading * robot->forward;

Hope this helps.

[eastskykang]

p.s. P3D is a position of a point while V3D is a 3D vector: vector is difference of two P3Ds i.e.

P3D start(0, 10, 2); // this is just an example
V3D vectorFromStartToEnd(1, 3, 2);
P3D end = start + vectorFromStartToEnd; // here end will be P3D(1, 13, 4)

[iqiac]

Ok thanks you, that helped for the turning.