BasicCartesianControl::movv should follow a velocity-driven trajectory

[PeterBowman]

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:

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

Lines 199 to 249 in 24c0699

	void roboticslab::BasicCartesianControl::handleMovv()
	{
	//-- Obtain current joint position
	std::vector<double> currentQ(numRobotJoints);
	if (!iEncoders->getEncoders(currentQ.data()))
	{
	CD_WARNING("getEncoders failed, not updating control this iteration.\n");
	return;
	}
	//-- Compute joint velocity commands and send to robot.
	std::vector<double> commandQdot;
	if (!iCartesianSolver->diffInvKin(currentQ, xdotd, commandQdot, referenceFrame))
	{
	CD_WARNING("diffInvKin failed, not updating control this iteration.\n");
	return;
	}
	CD_DEBUG_NO_HEADER("[MOVV] ");
	for (int i = 0; i < 6; i++)
	{
	CD_DEBUG_NO_HEADER("%f ", xdotd[i]);
	}
	CD_DEBUG_NO_HEADER("-> ");
	for (int i = 0; i < numRobotJoints; i++)
	{
	CD_DEBUG_NO_HEADER("%f ", commandQdot[i]);
	}
	CD_DEBUG_NO_HEADER("[deg/s]\n");
	for (int i = 0; i < commandQdot.size(); i++)
	{
	if (std::abs(commandQdot[i]) > maxJointVelocity)
	{
	CD_ERROR("diffInvKin too dangerous, STOP!!!\n");
	stopControl();
	return;
	}
	}
	if (!iVelocityControl->velocityMove(commandQdot.data()))
	{
	CD_WARNING("velocityMove failed, not updating control this iteration.\n");
	}
	}

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]

Ongoing work on trajectory generation: #134.

[PeterBowman]

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): 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 Future of BasicCartesianControl and outlines for creating new controllers #124 (comment). Then, tune Kp and Kd with care.

[PeterBowman]

TODO:

• [ ] Regression: MOVV does not support input twists expressed in the TCP frame. my bad, it actually did work
• (c6decc7) Avoid race conditions induced through successive calls to MOVV. The ICartesianTrajectory instance is reused and reconfigured while still being referenced by an active CMC loop on a separate thread.

[PeterBowman]

I'm probably going to put my efforts on solving #119 in the following days. Further progress on real TEO is blocked by the lack of a truly working cartesian controller.

[PeterBowman]

As stated above, we know that TEO is unable to perform a linear trajectory. See roboticslab-uc3m/teo-bimanipulation#3 (comment) 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]

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