ICartesianControl - using setTaskVelocities(x,o)

[vicentepedro]

Hi all,

I'm trying to define Task Velocities to the iCub arms using the Cartesian interface (setTaskVelocities(.) )

I found some documentation here.

However, it's not clear for me how to fine-tune the task velocities along with the trajectory time to achieve a continuous motion of the arm. (e.g, a straight line with a constant velocity).

The only information I found was:

executing high task-space velocity the trajectory time should be tuned towards lower values, whereas it should be increased to track low task-space reference velocity.

Maybe @pattacini is the best person to help me on this. 😃

/cc @gsaponaro

[pattacini]

Hi @vicentepedro
I'm aiming to provide a sort of "enlighting" example on this. In the meantime, I would suggest you read #30 since it contains already some heads-up on the tuning.

[pattacini]

Hi @vicentepedro,

I've prepared this Gist to explore the use of the yarp::dev::ICartesianControl::setTaskVelocities() method.

When the user calls such a service, the Cartesian controller integrates internally the commanded velocity, ending up with a moving target (i.e. the red ball in the simulations below) whose position and orientation are fed back into the normal tracking pipeline.

Therefore, as for a usual position tracking, the resulting behavior is essentially regulated by the ratio between the magnitude of the task velocity V you aim to achieve (i.e. how fast the target moves) and the point-to-point trajectory time Ttraj, which in turn accounts for the reactivity of the Cartesian controller (i.e. lower Ttraj makes the robot move faster).

In this context, if the target moves slower than how much the robot can react, we'll have intermittent movements where the robot will decelerate in the proximity of the target to then accelerate soon afterward when the relative distance starts over increasing.

Gist Example

For the same magnitude of the task velocity equal to 7 cm/s we can have two main results:

Results with too reactive controller

$ ./test-settaskvelocities --V 0.07 --Ttraj 0.1

The outcome shown below is a sort of stick-slip movement of the robot, meaning that Ttraj is too low.

Results with a balanced controller

$ ./test-settaskvelocities --V 0.07 --Ttraj 0.4

The movement visible below is way smoother instead. The hand is lagging a bit behind the target; however, we are not interested in this particular distance, but rather in the fact that the hand is actually moving roughly at V.

Clearly, with increasing values of Ttraj, we will reach a point where the robot will be no longer able to track the target consistently.

Advantage of Cartesian controller over traditional methods

By inverting the well-known differential kinematic map relating the joint velocities to the task velocities through the Jacobian

$$ \dot{x}=\mathbf{J} \dot{q} $$

one may think to address the problem more easily.

Let's try it out:

$ ./test-settaskvelocities --V 0.07 --method pseudo-inverse

You'll get the poor movements reported below. The main reason is that, unfortunately, this approach must be extended in order to take the joint bounds and the singular configurations into account. Further, the robot moves quite slowly also because the simulator, in this case, does not implement a perfect integration of the joint velocities, meaning that the low-level PID controllers do not guarantee zero-dynamics.

Concluding remarks

Similarly to the case of position task velocities, the Cartesian controller also allows tracking a target frame that rotates in the workspace with a specified angular velocity. In this respect, I'd recommend you update your local copy of icub-main/devel in order to get the latest fixes.

Hope this answer clarifies your doubts.

[vicentepedro]

Thanks @pattacini!
Awesome tutorial ;)
I will try to fine-tune my module exploiting these ideas. Thanks for the insight!

[frank-foerster]

Very nice. This function was also puzzling me and I was tempted to play with it, but didn't touch it in the end. Now I will. Many thanks.

[pattacini]

I'm happy it helped out 😃