Watch joint configuration in CMC thread

[PeterBowman]

Expanding on #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:

kinematics-dynamics/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp

Lines 11 to 33 in c187abd

	void roboticslab::BasicCartesianControl::run()
	{
	switch (getCurrentState())
	{
	case VOCAB_CC_MOVJ_CONTROLLING:
	handleMovj();
	break;
	case VOCAB_CC_MOVL_CONTROLLING:
	handleMovl();
	break;
	case VOCAB_CC_MOVV_CONTROLLING:
	handleMovv();
	break;
	case VOCAB_CC_GCMP_CONTROLLING:
	handleGcmp();
	break;
	case VOCAB_CC_FORC_CONTROLLING:
	handleForc();
	break;
	default:
	break;
	}
	}

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 roboticslab-uc3m/yarp-devices#189.

Edit: see #161 (comment).

[PeterBowman]

Marking as high priority per #164.

[PeterBowman]

Context: roboticslab-uc3m/teo-bimanipulation#3 (comment).

[rsantos88]

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]

I've committed ba17795, 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]

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: 6544a92. 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]

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 roboticslab-uc3m/teo-main#15 and should end up somewhere in https://github.com/roboticslab-uc3m/developer-manual but sure! If it works, go for it!!

[PeterBowman]

OK, done at d2afc5e. 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]

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]

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

Done at 406ea26.

[PeterBowman]

• Joint limits done (see referenced commits in the previous comments).
• Speed/acceleration checks: not doing, retitled accordingly. These parameters (actually, only the velocities) are obtained from internal solver calculations and ultimately originate from user commands (in cartesian space). For that reason, I don't think we should query the robot for its current instantaneous velocity (this is indeed blocked by Implement instantaneous speed/acceleration getters yarp-devices#189). Rather than that, let's fetch robot velocity limits and check commanded joint velocities against them before sending anything to the robot. This approach has been followed in 84739d0.

[PeterBowman]

(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.