Foot trajectory planning unstable

[catachiii]

Dear TA,

I managed to finish exercise 3, but when I tried to increase turning speed, the planned foot trajectory is very unstable, which results in the unstable motion of the feet.

When I only changed the turning speed to 0.1, the robot would start to "dancing". In the picture below, we can clearly see that there are some trajectories in the circle (stable trajectory) or even out of the circle. And this also results in the instability of IK.

Is this supposed to be solved by us, or we can just leave it?

[breadli428]

Same problem here, not sure it is a technical problem or out of instability.

[eastskykang]

This looks a bit suspicious, it shouldn't be the case. As you can see from the example video, trajectory planner should be stable enough to make turning motion: https://youtu.be/NwVNkggKhC0

[breadli428]

I used Newton-Gauss and did not yet implement line search and regularization. Does this result in the instability?

This is how it behaves for now: https://youtu.be/gSHkTksCPE8

[eastskykang]

From the screenshot you posted here, it could be the case that your forward kinematics implementation does not update position of ee correctlyl if the base orientation is not 0, 0, 0.

The base orientation (qIndex 3 (yaw), 4 (pitch), 5 (roll)) components of generalized coordinate are euler angles. One thing you should keep in mind, is that we use Y(yaw) X(pitch) Z(roll) euler angles, since forward direction of the robot is z axis, and sideways is x axis and world-up is y axis.

Thus, rotational matrix of the base R should be

R = Ry(yaw) * Rx(pitch) * Rz(roll)

hope this helps you to identify the problem.

[catachiii]

Problem identified.

Both my FK and Locomotion plan have some mistakes. It is not a technical issue. Thanks, Dongho.