Test stability via ZMP while standing on one foot

[PeterBowman]

We want to have a different take on roboticslab-uc3m/teo-main#51 starting with much simpler experiments. For starters, achieving single-foot stability with ZMP compensation is our current goal (see these videos from 2019). This task may supersede #1.

Ankle FT sensor is a must. Then, I'd consider using the IMU to somehow overcome slack errors: https://github.com/roboticslab-uc3m/teo-hardware-issues/issues/63.

cc @munozyanez @Gerson-Martin @jmgarciah

[PeterBowman]

Regarding some design decisions:

• I believe firmware-level velocity control will not work at all, considering all the robot's weight in the first place.
• CSP mode can map both to (absolute) position commands and a position integrator based on velocity commands. I do like velocities better as a more intuitive approach, but they require a differential IK solver. Jacobians are going to pose a problem due to convergence issues whenever the leg reaches its fully extended configuration (knee angle close to zero).
• MOVI-based control has the inconvenience described in Spnav controller does not dismiss out-of-limit poses (MOVI) kinematics-dynamics#186, (i.e. unchecked limits). Perhaps it can be mitigated somehow in this case. On the other hand, our screw theory IK solver implements a configuration selector for legs that takes care of choosing human-like directions of motion (knees bent forward).
• Since initially the desired ZMP pose could diverge notably from the actual pose, I'd rather start motion gradually. Velocity commands (discarded, see previous points) would have required an additional acceleration limiter. Let's see how to solve this with position-based trajectories.
• There could be infinite ZMP configurations along a line in 3D space. I'd pick the one which achieves the lowest step (largest difference between base and TCP in the Z direction) within limits. This will require several IK/reachability checks per iteration.

[PeterBowman]

Oops, we did it again. TEO's right leg is kinda broken after today's tests. See https://github.com/roboticslab-uc3m/teo-hardware-issues/issues/65 and https://github.com/roboticslab-uc3m/teo-hardware-issues/issues/66.

See also roboticslab-uc3m/teo-gazebo-models#7 regarding the introduction of FT sensors in the simulator.

[PeterBowman]

See also: https://github.com/roboticslab-uc3m/force-torque-balance.