Squats: Generate trajectories and perform on sim and real

[jgvictores]

• Generate trajectories
• Perform on sim
• Perform on real

Edited from: Perform squats using TrajectoryLib of https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/134, preferably from Python (related: https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/168)

[jgvictores]

Inspiration:

• https://github.com/roboticslab-uc3m/tools/blob/85122db9c6b464d4805463f52be83077615131de/examples/cpp/examplePlaybackThread/examplePlaybackThread.cpp
• https://github.com/roboticslab-uc3m/kinematics-dynamics/tree/0fa54caf91f3ac3b6d83a260910c9408f9ee292a/example/cpp/exampleCartesianControlClient

[jgvictores]

Will develop Joint Space trajectory as part of https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/134.

[jgvictores]

Blocked by https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/134

[jgvictores]

Not blocked by https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/134, may not require joint space trajectories in TrajectoryLib

[PeterBowman]

TEO has learned to perform small up-down (squat, Z axis) and left-right (balance, Y axis) linear displacements of its center of mass. Proof:

Full recipe:

• Tested on the old PT mode, temporally restored in the old-pt branch at yarp-devices.
• Had to disable (remove) absolute encoders in launchLocomotion.ini (thanks @rsantos88 for the tip) due to constant driver failures, apparently related to saturation of CAN comms.
• Might need to increase maximum velocities on real robot. Simulated is fine with default 10 degrees/s, real was tested with 30/50 (left/right leg), but not sure if needed.
• App available at kin-dyn/.../exampleSquatBalance (https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/c9f49a05577d2e302e4fee2e8e18b34af28d1cd4), see doxygen for usage notes.
• Simulated TEO can travel within [0,-0.45] in Z axis and [-0.145,0.145] in Y axis wrt. to its initial home pose. Tested with [0,-0.45] and [-0.1,0.1] on real TEO, see above YouTube clip.
• Improvements on kinematics-dynamics (merged into develop branch):
  • A corner case linked to the PadenKahanThree and PardosGotorThree subproblems has been fixed: https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/b98ac6f84e3d4dcf4de5a80f60765d42d3108380.
  • A new IK configuration strategy selector, --invKinStrategy humanoidGait, has been added: https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/0be892c1cbdfeb154e49fbd39af99b1629d2a3b4. This selector picks the only configuration (out of 8 possible solutions) that entails a human-like pose, i.e. it prevents from rotating the knee backwards and so on.
  • Old PT mode implementation at yarp-devices is not aware of the YARP's remote variables interface, hence it shouldn't make BasicCartesianControl fail on initial configuration: https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/eaed8504aa7e07cc3941688dc85fc87830c49a24.
• Important: comment out the joint velocity check (ref), not working well along with the old PT mode (the robot does not move until the buffer is full).

Update (2021): the squat app was moved to https://github.com/roboticslab-uc3m/gait-experiments. The old pt mode was fully replaced by CSP: https://github.com/roboticslab-uc3m/yarp-devices/issues/254.

[smcdiaz]

Great! The first step to make a first step

[PeterBowman]

As a first approach, I think we are done. See https://github.com/roboticslab-uc3m/teo-main/issues/33#issuecomment-504042205 (2019) and https://github.com/roboticslab-uc3m/teo-blender-models/issues/2 (2021).