search

roboticslab-uc3m / kinematics-dynamics

Kinematics and dynamics solvers and controllers.
https://robots.uc3m.es/kinematics-dynamics/
GNU Lesser General Public License v2.1
19 stars 12 forks source link

BasicCartesianControl::movv should follow a velocity-driven trajectory #166

Closed PeterBowman closed 5 years ago

PeterBowman commented 5 years ago

This implementation of ICartesianControl::movv is not keeping track of a reference during its execution, i.e. no control loop is closed as it is the case in movl:

https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/24c0699702a42907f52456ad12152d9fd3f84470/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp#L199-L249

There was an advise concerning its experimental status which had been prematurely removed (my fault) from this function, we just learned that real TEO is not capable of maintaining a linear path when control is performed through this RPC command. In contrast to what we see on the simulator, an arm of the real-life TEO tends to bend down due to its own weight. This was somehow expected, but I wasn't aware of such big differences between reality and simulation. Also, everything is fine on AMOR, but in this case the vendor API takes care of everything under the hood.

Ideally, this implementation should build a new trajectory with simple velocity interpolation (just keep it steady per user's input). Thinking of splines (see KDL's built-in velocity profiles), a first-degree path interpolation should do; I recall such a solution in ASIBOT's former cartesian controller.

PeterBowman commented 5 years ago

Ongoing work on trajectory generation: https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/134.

PeterBowman commented 5 years ago

So far I have improved our KDL wrapper for trajectory generation, now accepting rectangular velocity profiles, and movv is mostly equivalent to movl (using velocity- instead of position-based inputs): https://github.com/roboticslab-uc3m/kinematics-dynamics/compare/518e269...7035634. Sadly, we learned that TEO does not compensate its weight neither with movl nor movv, on the long run. The forearm tends to bend down while executing the trajectory. Things get worse if the start position is a partly extended arm: initially, the arm drops a few centimeters down, tries to get back on track and hits an angular velocity limit on some joint while doing that.

@jgvictores has proposed three solutions:

  1. Ideally, we should take a closer look at the low-level control, i.e. iPOS (motor driver). Any robotic arm is essentially a mechanism that moves an excentric load. It's hard/impossible to tune a linear PID that is supposed to respond adequately regardless of the manipulator pose at any time. Since the iPOS may run small programs (TML language), perhaps we could make them aware of other iPOS from the same kinematic chain so that they can always tune themselves accordingly. Comms could be centralized via a Mbed device (?).

  2. Implement torque control (so far we adopted velocity control). Joint are expected to behave similarly to gcmp, i.e. they may not follow an exact linear path.

  3. Implement https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/124#issuecomment-373703327. Then, tune Kp and Kd with care.

PeterBowman commented 5 years ago

TODO:

PeterBowman commented 5 years ago

I'm probably going to put my efforts on solving https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/119 in the following days. Further progress on real TEO is blocked by the lack of a truly working cartesian controller.

PeterBowman commented 5 years ago

As stated above, we know that TEO is unable to perform a linear trajectory. See https://github.com/roboticslab-uc3m/teo-bimanipulation/issues/3#issuecomment-435919430 for an in-depth explanation.

AMOR, on the other hand, performs nicely thanks to its built-in controller (vendor amor-api + our AmorCartesianController YARP wrapper device), which takes care of closing a control loop under the hood. On the other hand, I've observed that BasicCartesianControl's MOVV command performs far better than TEO did, with a barely noticeable deviation of 0.5~1 cm when AMOR extends its arm while holding a 0.5 kg payload. Even though this is a good result, the new implementation of MOVV as proposed here clearly outperforms the old one, managing to keep the TCP at the expected height (z coordinate) along the whole path.

jgvictores commented 5 years ago

Slightly related: "Create a generator-enabled trajectory #168"