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

Watch joint configuration in CMC thread #161

Closed PeterBowman closed 5 years ago

PeterBowman commented 6 years ago

Expanding on https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/159, status checks could be performed on each thread step to make sure that no joints are out of limits, nor maximum permisible speeds/accelerations are exceeded. I'd put them before the actual handler (e.g. handleMovl) is entered:

https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/c187abdc0c9b2284d36df0f22cf64ecb238642b2/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp#L11-L33

Motivation: starting from point A, I can issue a movl command to reach point B, but joint limits are hit at some point. The controller should either stop and throw an error, or at least warn the user. Now, it just goes on and the final trajectory might be kinda curvy with no warn (sometimes even not noticeable at all, or we notice it too late so that it cannot be stopped nor further corrections be made).

Partially (regarding speeds and accelerations) blocked by https://github.com/roboticslab-uc3m/yarp-devices/issues/189.

Edit: see https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/161#issuecomment-493167169.

PeterBowman commented 5 years ago

Marking as high priority per #164.

PeterBowman commented 5 years ago

Context: https://github.com/roboticslab-uc3m/teo-bimanipulation/issues/3#issuecomment-405940694.

rsantos88 commented 5 years ago

Motivation: starting from point A, I can issue a movl command to reach point B, but joint limits are hit at some point. The controller should either stop and throw an error, or at least warn the user. Now, it just goes on and the final trajectory might be kinda curvy with no warn (sometimes even not noticeable at all, or we notice it too late in order to stop and make corrections)

This is caused because if the joint reachs its limit, it stops and doesn't continue but the rest of joints that have not reached their limit continue their movement, producing a deviation of the trajectory. We need to stop the movement of all kinematic chain when a joint reaches its limit (In reference to roboticslab-uc3m/teo-bimanipulation#3 )

PeterBowman commented 5 years ago

I've committed https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/ba17795348e52da1d5b5eade943700b2cc45d3f6, but this is a naive approach. It won't work on simulation since I'm checking against the limits fetched from the simulator itself, that is, the remote_controlboard client within BCC should never be reported a joint value out of the expected range. It will just stop moving the offending joint and go on with the controlled trajectory, regardless of what we do from the BCC side. On the real robot, limits are parsed from the .ini files. It really depends on how are they handled by the motor driver (Technosoft iPOS), but we can potentially incur in a similar issue.

A solution I can come up with for this is to apply a small epsilon that would make the joint limit check report an out-of-range value a bit earlier that it's actually supposed to.

PeterBowman commented 5 years ago

A solution I can come up with for this is to apply a small epsilon that would make the joint limit check report an out-of-range value a bit earlier that it's actually supposed to.

Here it is: https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/6544a92b20073ecd340558d5c9ad276105f0a59d. The problem is that I couldn't perform any CMC motion from the simulated AMOR's initial pose with epsilon = 0.001 since it starts very closely to the configured limit on certain joints. It did work after decreasing to 0.00001, though. @jgvictores do you think this is a robust solution?

jgvictores commented 5 years ago

A small epsilon that would make the joint limit check report an out-of-range value a bit earlier that it's actually supposed to.

@jgvictores do you think this is a robust solution?

I guess it all boils down to https://github.com/roboticslab-uc3m/teo-main/issues/15 and should end up somewhere in https://github.com/roboticslab-uc3m/developer-manual but sure! If it works, go for it!!

PeterBowman commented 5 years ago

OK, done at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/d2afc5e5368ed3cf9b29de91ed56c69cd0d0cc2e. I'm therefore removing the priority: high label as there is more to be done here and the urgency has been removed.

A word of caution: once having reached a limit, the cartesian controller will not be able to move the joint out of it. Instead, connect to the RPC port and change the robot position via set pos x ....

PeterBowman commented 5 years ago

A word of caution: once having reached a limit, the cartesian controller will not be able to move the joint out of it. Instead, connect to the RPC port and change the robot position via set pos x ....

There should be a way to regain control of the robot once joint limits are hit.

PeterBowman commented 5 years ago

There should be a way to regain control of the robot once joint limits are hit.

Done at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/406ea2696086a70bd36d1d82d09b5f9debc4c869.

PeterBowman commented 5 years ago
PeterBowman commented 5 years ago

(actually, only the velocities)

Probably, I also considered accelerations having https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/163 in mind. Although this is not the common use case, it merits its own case study and further analysis.