Getting the iCub's hand following smooth Cartesian trajectories through given way-points

[pattacini]

Rayat Goyal posted the following question.

I am using the Cartesian Interface to move iCub's hand on a certain trajectory as described in the tutorial.
The relevant methods I have to use are the following:

icart->goToPoseSync(xd,od);
icart->waitMotionDone();

I have now several 3D points, e.g:

(0.0,0.1,0.2)
(0.1,0.2,0.3)
(0.0,0.4,0.3)
(0.5,0.2,0.3)
(0.1,0.2,0.3)

that I am providing to the Cartesian controller within a loop. What happens is that the hand moves towards the first point and then comes to a stop, starts again and moves to the second point and so on so forth.
My task is to make the hand move smoothly through all those points without
any stop in between.

• My first intuition was to use just icart->goToPose(xd,od), which however
  does not ensure that iCub touches all the points.
• My second intuition was to use events callback, so that I can start over moving the arm before it comes to a complete stop. Unfortunately, also this approach results in jerky motion.
  I tried setting various trajectory times along with, but it didn't help out.

Any idea on how to achieve smooth motion using the Cartesian Interface while ensuring that the hand reaches all the points?

[pattacini]

@stephane-lallee replied:

I might be wrong, but I guess you have to calculate the waypoints yourself so that they form a "smooth trajectory". If you send to the controller some jerky trajectory then it will be jerky.

Let's assume you have 4 points which form a rectangle, then the hand effector will move more or less along the sides of this triangle. If instead you want the hand to pass by all the corners but describing a circle then you have to calculate manually some additional waypoints...

Although I may not have understood completely your issue.

[pattacini]

Rayat Goyal replied:

The problem I am having is I want the arm to keep moving and not to stop on the points in between.
Let's take your case of a rectangle: what I want is the velocity to be constant when the arm starts moving until the rectangle is complete. What happens instead is: it first starts moving and then stops at one point; then, it starts moving again and then stops at the second point.

I think I used the word "smooth" wrong in the context.

[pattacini]

My suggestion is to provide the Cartesian controller (from within a thread) with a virtual point that follows the trajectory (i.e. path + timing) you want to achieve.
Then, you should play with the point-to-point trajectory time of the controller in order to tune the tracking performance.

For instance, the rectangle has 4 edges, hence there will be 4 different states of the thread; in each state the path law is linear with a constant velocity. Once one state is complete, then you immediately switch to the next state without letting the controller stop.

The instant position of that virtual point should be passed to the controller in streaming, i.e. with the goToPose() method (non-sync).

Basically, the example in the tutorial with the circular trajectory already gives you a code template to stick to.

[pattacini]

Rayat Goyal replied:

That is a nice solution, but the problem is I don't have the trajectory points beforehand.
In fact, I am getting them in real-time, so that I can't really define trajectories.
I am using yarp::os::BufferedPort to buffer the points.

What I am doing is waiting until the trajectory is 70% complete and then providing the second point, so that the arm does not lose the velocity and continues towards the new target.
But, still, the arm keeps on stopping before starting to move to the new point.

Here is the code logic behind the code:

virtual void cartesianEventCallback()
{
    goahead=true;
}

public:
    CtrlThread(const double period) : RateThread(int(period*1000.0))
    {
        // raise at event at 70% of trajectory .
        cartesianEventParameters.type="motion-ongoing";
        cartesianEventParameters.motionOngoingCheckPoint=0.7;
    }

//...

while (//new data point given every-time in this loop )
{
    icart->goToPose(xd,od);
    goahead=false;
    std::cout<<"waiting";
    while (goahead==false)
        std::cout<<".";
}

Any idea why this is not working, or any other alternative solutions?

[pattacini]

The technique I proposed is still viable in a dynamic fashion.

Assume that your input is received with a period P_i; this means that each P_i seconds you get a fresh new point x(i).

What the thread should accomplish is the following:

1. With a period P_j (< P_i) it generates a virtual (temporary) target tg(j) that is spatially (linearly) comprised between x(i-1) and x(i) and moves from x(i-1) to x(i) with a given constant speed vel.
2. It sends continuously tg(j) to the Cartesian controller which has been tuned with a proper Ttraj point-to-point maneuver time.

Of course, the final movement won’t stop and will appear to be smooth enough if and only if:

1. The new point x(i+1) is provided well before the point x(i) is reached: the trajectory generator will still attain x(i) prior to producing the path to x(i+1);
2. Ttraj is such that the tracking is properly accomplished and the end-effector follows closely the point tg(j).

So, as you can figure out, there are three dynamics you need to blend and trade off:

1. The incoming input x(i) dynamic;
2. The generation of virtual points tg(j) with the time granularity P_j and speed vel;
3. The Cartesian movement dynamic tuned by Ttraj.

Normally (1) is given and you can play with (2) and (3) relying on the parameters P_j, vel, Traj.

Coming to the use of the callback, you have to bear in mind that the point-to-point movement is designed in such a way that the end-effector will follow a quasi-minimum-jerk trajectory in the operational space meaning that the velocity profile is bell-shaped and approaches zero in the neighborhood of the end of the set-point. Depending on the values assumed by Ttraj and the nearness of x(i+1) and x(i) it might happen that at 70% of the trajectory the hand is almost still; therefore, try to increase Ttraj and/or reduce the percentage.

However, what you will get is simply a sequence of bells (representing the velocity) that are joint together exactly at the 70% of their profile. This necessarily entails an overall stick-slip movement.
On the other hand, if you increase the number of bells (you have one bell for each input) and let them joint together much more tightly, the overall profile will resemble a straight line which is definitely the behavior you want to achieve. In this respect the first method is more suitable than the use of callbacks, at least because it generates an higher number of bells.

One further solution you can explore makes use of the cartesian velocities. In principle you should be able – up to a certain extent – to set directly the speed (translational + rotational) of the end-effector in the operational space. The relevant method is setTaskVelocities(). Thus, basically, the argument of that function should be a vector of norm equal to vel and direction from x(i-1) to x(i); the rotational speed can be set equal to zero to start with. The documentation in the tutorial should shed light on this topic as well. This third approach seems to be much easier but then you’ll lose some degrees of freedom you have with the first method.