Working to obtain a good movement in the Cartesian space

[rsantos88]

At this moment, we've a bimanipulation demostration that works doing this things:

1. Movement of both arms, following the points recorded in joint coordinates in order to reach the tray avoiding hitting the table
2. Once the tray is grasped, it makes movements in cartesian coordinates using the KDL library: ICartesianControl. He moves the tray 5 cm in all of the three axis, interpolating to 20 points each path of each limb.

This is a video of the result, working with teoSim (openrave):

[rsantos88]

Things to improve first:

• I don't know if I can use something like TwoLimbCartesianControl or something similar, to control and moving at the same time the two arms, with one TCP (the trail). From what I've seen, it looks like it's in development, isn't it? At the moment, my application is alternating each arm, moving one after the other...
• Try to get a softer and faster movement using the solver. I use movj because I thing it's faster to reach the points than movl... but a linar movement would be in this case the most correct form.

Any suggestion is welcome :smiley:

[PeterBowman]

I don't know if I can use something like TwoLimbCartesianControl or something similar, to control and moving at the same time the two arms, with one TCP (the trail). From what I've seen, it looks like it's in development, isn't it?

The old two-limb cartesian controller will be superseded by a brand new trajectory generator, see https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/134. Sadly, the old code is unmaintained and will probably break at current develop.

Try to get a softer and faster movement using the solver.

You can't achieve this with profiled paths such as movj and movl. I mean profiled since the current generator assumes point-to-point motion with starting and ending zero speed while generating its own velocity profile, that is, your robot will stop at every waypoint as seen in the video. You definitely need to use streaming commands: either twist or pose (docs). Even better: the long-planned position-controlled movi command (instant movement, https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/119) will bypass any low-level trajectory generator by forwarding IK-computed joint values to the yarp::dev::IPositionDirect implementation at the motor drivers.

[rsantos88]

Thanks for your aportations @PeterBowman I have thought another procedure to achieve this goal to obtain a movement of both limbs at the same time and a smooth displacement. Using ICartesianSolver, I want to:

1. calculate the forward kinematic (joint space -> cartesian space)
2. do the the corresponding operations (interpolating a path)
3. transform this points in joint space calculating the inverse kinematic
4. Move the arms in joint space

My problem is that I can't calculate the inverse kinematics with the solver... (The maximum number of iterations is exceeded) In this example, I am only trying to get the IK from the FW:

[success] Acquired rightArmIEncoders interface
[success] Encoder values of right-arm: (31.282951 -14.042175 -10.790863 52.636204 -81.528992 88.488579 )
 -> FK of right-arm: (-0.313215 0.032358 0.257216 1.216977 -1.195232 -0.792099 )
[error] ICartesianSolverImpl.cpp:155 invKin(): -5: The maximum number of iterations is exceeded
[ERROR] invKin failed.
-> IK of right-arm: (0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 )

Any way to configure the solver to increase the number of iterations or some possible solution? Thanks!!

[jgvictores]

Any way to configure the solver to increase the number of iterations or some possible solution?

https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/da0a4ac2aefe94a607418adbabc9108e8251d70c/libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp#L211-L212

(defaults: https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/da0a4ac2aefe94a607418adbabc9108e8251d70c/libraries/YarpPlugins/KdlSolver/KdlSolver.hpp#L19-L28 )

[PeterBowman]

You could increase it at the cost of spending more time in each waypoint, and perhaps not even reaching a convergent solution. That's actually the main motivation behind introducing a fast IK solver. I was tempted to try the solution described in https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/119#issuecomment-351735714, which should be easier to implement.

[PeterBowman]

The old two-limb cartesian controller will be superseded by a brand new trajectory generator

Clarification: I meant here that current BasicTwoLimbCartesianControl and TwoLimbCartesianControlServer controller/server devices are unmaintaned and may undergo notable changes, perhaps even get reworked from scratch.

[jgvictores]

@rsantos88 BTW, since you are directly using the solver, you're essentially developing a controller. Therefore, your device or program should be in charge of getting the joint limits from the robot to configure the solver. Here's how it is done in BasicCartesianController: https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/da0a4ac2aefe94a607418adbabc9108e8251d70c/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp#L73-L93

[rsantos88]

Added kinematic limits (right and left arms): https://github.com/roboticslab-uc3m/teo-configuration-files/commit/3c7b1b1053d8c65aabca543dbd881e1bcf1e4b34 I've tried to calculate IK of right-arm with a more normal position far away from the limits using the values of FW calculated before...

[success] Acquired rightArmIEncoders interface
[success] Encoder values of right-arm: (-22.917389 2.636204 31.810192 15.905096 -22.302277 22.671352 )
 [success] Acquired leftArmIEncoders interface
[success] Encoder values of left-arm: (22.917389 -2.636204 -31.810192 -15.905096 22.302277 -22.671352 )
 -> FK of right-arm: (-0.118218 -0.380733 -0.184653 -0.011106 1.326649 -0.202441 )
 -> FK of left-arm: (-0.118218 0.380733 -0.184653 0.011106 1.326649 0.202441 )
 calculating IK...
[error] ICartesianSolverImpl.cpp:155 invKin(): -5: The maximum number of iterations is exceeded
[ERROR] invKin failed.
-> IK of right-arm: (0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 )

The values of the KdlSolver are:

#define DEFAULT_KINEMATICS "none.ini"  // string
#define DEFAULT_NUM_LINKS 1  // int

#define DEFAULT_EPSILON 0.005     // Precision tolerance
#define DEFAULT_DURATION 20     // For Trajectory
#define DEFAULT_MAXVEL 7.5      // unit/s
#define DEFAULT_MAXACC 0.2      // unit/s^2

#define DEFAULT_EPS 1e-9
#define DEFAULT_MAXITER 10000000 //max value: 100000000

I've tried to put a bigger value of DEFAULT_EPS like 1e-6 but it doesn't work...

[jgvictores]

1. You don't have to change the code and recompile, just pass the parameters as --commandLineArg value (check()ed here)
2. 1e-6 is still pretty small, try 1e-3.

[jgvictores]

((edited second comment))

[jgvictores]

roboticslab-uc3m/teo-configuration-files@3c7b1b1 no... that was not what my comment on https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/da0a4ac2aefe94a607418adbabc9108e8251d70c/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp#L73-L93 was...

[rsantos88]

After doing multiple tests, I have identified the problem... It's simple. The detail in decimals when it calculates the forward kinematic is scarce. This is the result calculating FK of right-arm using fwdKin function:

[success] Acquired rightArmIEncoders interface
[success] Encoder values of right-arm: (31.282900 -14.042100 -10.790800 52.636200 -81.528900 88.488500 )
 [success] Acquired leftArmIEncoders interface
[success] Encoder values of left-arm: (-31.282900 14.042100 10.790800 -52.636200 81.528900 -88.488500 )
 -> FK of right-arm: (0.367654 -0.242313 0.167621 -1.238838 -1.020247 1.369499 )
 -> FK of left-arm: (0.367654 0.242313 0.167621 1.238838 -1.020247 -1.369499 )
 calculating right-arm IK...
[error] ICartesianSolverImpl.cpp:155 invKin(): -5: The maximum number of iterations is exceeded
[ERROR] invKin failed.
-> IK of right-arm: (0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 )

If I use BasicCartesianControl to get FK of the final pose using stat, the result is this:

>>stat
Response: [ccnc] 0.367653642389892 -0.242313427959188 0.167620650405435 -1.23883784251295 -1.02024656675129 1.36949948133935

When I try to do the IK using the inv command of BasicCartesianControl with the last result, it can do it without any error and with the correct values of the encoders

>>inv 0.367653642389892 -0.242313427959188 0.167620650405435 -1.23883784251295 -1.02024656675129 1.36949948133935
Response: 31.2829 -14.0421 -10.7908 52.6362 -81.5289 88.4885

on the other hand:

>>inv 0.367654 -0.242313 0.167621 -1.238838 -1.020247 1.369499
Response: [fail]

and the corresponding message of BasicCartesianControl:

[debug] RpcResponder.cpp:46 respond(): Got: inv 0.367654 -0.242313 0.167621 -1.238838 -1.020247 1.369499
[error] ICartesianSolverImpl.cpp:155 invKin(): -5: The maximum number of iterations is exceeded
[error] ICartesianControlImpl.cpp:47 inv(): invKin failed.

[PeterBowman]

Are you sure you're not truncating the numeric result of ICartesianSolver::fwdKin on stdout? The output of stat is actually what you would get from a fwdKin call.

[rsantos88]

I think not but maybe I'm missing something ... I'll copy the code of my FK function for right-arm:

bool BodyExecution::getRightArmFwdKin(std::vector<double> *currentX)
{

    /** ----- Obtain current joint position ----- **/
    int rightArmAxes;
    rightArmIPositionControl2->getAxes(&rightArmAxes);

    if (!rightArmDevice.view(rightArmIEncoders) ) { // connecting our device with "IEncoders" interface
        printf("[warning] Problems acquiring rightArmIEncoders interface\n");
        return false;
    } else printf("[success] Acquired rightArmIEncoders interface\n");

    std::vector<double> currentQ(rightArmAxes);
    if ( ! rightArmIEncoders->getEncoders( currentQ.data() ) ){
        printf("[ERROR] getEncoders failed\n");
        return false;
    }
    else
    {
        printf("[success] Encoder values of right-arm: (");
        for(int i=0; i<rightArmAxes; i++)
            printf("%f ",currentQ[i]);
        printf(")\n ");
    }

    /** ----- Obtain current cartesian position ---------- **/
    if ( ! rightArmICartesianSolver->fwdKin(currentQ, *currentX) )    {
        printf("[ERROR] Forward Kinematic failed.\n");
        return false;
    }

    return true;
}

[rsantos88]

maybe I'm doing something wrong... because I've also tried to put directly the values obtained of the stat command and see the result of ICartesianSolver::fwdKin but it's not working too..

std::vector<double> vector {0.367653642389892, -0.242313427959188, 0.167620650405435, -1.23883784251295, -1.02024656675129, 1.36949948133935};
if ( ! rightArmICartesianSolver->invKin(vector, desireX, desireQ) )    {
    printf("[ERROR] invKin failed.\n");
}

printf("-> IK of right-arm: (");
for(int i=0; i<desireX.size(); i++)
    printf("%f ",desireX[i]);
printf(")\n ");

it shows the same error message...

[PeterBowman]

Even if the cartesian-space input values are equal (vector variable), make sure you start with the exact same joint-space guesses (desireQ variable) in both cases, via RPC and via C++ app.

[rsantos88]

Thanks a lot for your help. The best solution, use LMA solver: inside build directory of kinematics-dynamics, ccmake .. -> USE_LMA ON

[PeterBowman]

inside build directory of kinematics-dynamics, ccmake .. -> USE_LMA ON

Soon, we'll make this simpler so that both algorithms could be loaded (not at once, obviously) without having to recompile the device. See https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/155.

[rsantos88]

At the moment, the demonstration allow us to move the tray using dual arms in the three cartesian axes position moving it in two modalities:

• moving point to point following the path created by interpolation (slower and not conituous movement)
• travel the entire trajectory without waiting (continuous movement)

The result can be seen in the following video with simulation and the real robot. The biggest problem is that not all the joints end at the same time, avoiding a perfect linear movement. Now, I am working to move the arms in velocity mode using differential inverse kinematic. The result is not to bad, regarding the problem discussed above, but as you can see in this video, the position and orientation of the TCPs deviates a lot. Because of that, @jgvictores suggested me adding the error (K*ex) to the equation to correct it see the beautifull scheme. I'm working on it but there are some problems and unfortunetly it's not working...

[PeterBowman]

This is an interesting problem :). Just laying out what I would do, perhaps you can find some of these ideas useful:

• Initialization:
  • identify the tool (tray) and its TCP (centroid)
    • build a 6-element vector describing its absolute position and orientation wrt your base reference frame
  • configure cartesian solvers (one per arm):
    • use ICartesianSolver::appendLink so that both TCPs converge at the same point (the tray's centroid)
  • prepare the trajectory:
    • this could be done offline, that is, generated by a script, saved to a file, and loaded by your application (by yarpdataplayer, PlaybackThread or the like)
    • the outcome of this is a (possibly huge) collection of points - the path followed by the tray's centroid over time
    • each point represents the target pose both manipulators will try to achieve (accounting for the transformation from each arm's original TCP to the tray's TCP)
    • for better visual appearance, I'd suggest a nice circular trajectory performed at constant height - only two out of six pose values will ever change (for stability, you don't want the orientation to vary at any time)
    • apply small time increments between successive points - I'd say anything between 10-50 milliseconds would do
• Control loop:
  • iterate over the collection of points previously described
  • obtain the current robot pose in joint space - IEncoders::getEncoders
  • perform FK - ICartesianSolver::fwdKin
  • compute the difference between the target pose and the current pose - ICartesianSolver::poseDiff(desired, current, output)
  • apply the gain - the aim here is to keep the pose diff low enough between iterations, otherwise the control law will fail
  • apply numerical differentation - transfom the resulting distance to velocity using the same time increment as in the trajectory generation step
  • apply inverse differential kinematics - ICartesianSolver::diffInvKin
  • command robot joints in velocity mode - IVelocityControl::velocityMove

As you can see, this recipe is essentially the same one as described in the "beatiful scheme".

[rsantos88]

Thank you for all of your contributions.

@PeterBowman , I'm sure that I can improve my code (I know you are very purist with this :sweat_smile:), but I think I'm on the right track. I've followed your steps and this is the function that contains all the concoction.
I am quite fond of recording videos to check the results and be able to share them.

The problem that I was finding was of overshoot due to a pretty big gain used to correct the error. I used 0.05 value first, taken as an example of BasicCartesianControl , but @jgvictores suggested me to change the gain decreasing it to a much lower value, specifically I've used 0.05e-12. The result in the simulator was quite satisfactory.

Some curious observation: the CPU must be free of other process running at the same time, because otherwise, the behavior in the movement can vary a lot, possibly because the control does not perform it in the time it should do. Maybe it can be interesting to do the process in other different thread?

After that, I did the same tests on the real robot, but the result was a big disorientation of TCPs. Curiously the result of the movement in both arms is different. Other problem that you can see is that the left arm continues making a trajectory (as if it were falling down bit by bit). The result is quite disappointing :(

You can share your impressions!!

[PeterBowman]

Interesting! 0.05e-12 is quite negligible, which means that the simulated robot will try to follow the velocity reference only. I would actually expect it to behave correctly even with zero gain, is that true? On the other hand: does the real robot's behavior improve with (a bit) higher gains? It could be of help to print the value of leftArmCommandXdot[i] (ref, after performing this instruction) and leftArmDesireVelX[i] (ref), and compare them. If the former is way lower, I'd not be surprised for the robot arms to drift away since no position reference is being kept along the path.

By the way, cmcRateMs actually expresses time in seconds, according to the input parameters. That is, there is no need to add a 1000 factor here, and it might be spoiling you results on simulation (in short, you are actually applying a gain value of 50, not 0.05 - remember that the goal is to keep the correction values low).

Some curious observation: the CPU must be free of other process running at the same time, because otherwise, the behavior in the movement can vary a lot, possibly because the control does not perform it in the time it should do.

There is no delay (yarp::os::Time::delay()) inside this loop. Although each error correction step is performed once per checkpoint, which accounts to once per 100 milliseconds in your app with current data, this blocks is constantly querying encoder values, performing complex calculations and sending velocity commands. I'd put said block inside this check and defer resuming the loop until the next cycle. That is, yarp::os::Time::delay(cmcRateMs*(t+1)+yarp::os::Time::now()-timeStamp) or something equivalent at the very end of the while block. For reference, the period of the control loop in BasicCartesianControl is 50 milliseconds (for a single arm).

Other problem that you can see is that the left arm continues making a trajectory (as if it were falling down bit by bit).

That's presumably the lack of a stop command (this won't work, as you noticed - see https://github.com/roboticslab-uc3m/yarp-devices/issues/120). Try switching to position control mode (ref), but don't forget to get back to velocity before the next iteration.

[rsantos88]

Thank you for all your reports @PeterBowman . I've performed a lot of changes referring to what you have commented:

Interesting! 0.05e-12 is quite negligible, which means that the simulated robot will try to follow the velocity reference only. I would actually expect it to behave correctly even with zero gain, is that true? On the other hand: does the real robot's behavior improve with (a bit) higher gains?

It's true. But I only have tested this last changes in the simulator. The difference are priceless. I need to do some tests on the real robot, but this week Teo is also in great demand.

By the way, cmcRateMs actually expresses time in seconds, according to the input parameters. That is, there is no need to add a 1000 factor here, and it might be spoiling you results on simulation (in short, you are actually applying a gain value of 50, not 0.05 - remember that the goal is to keep the correction values low).

Ok, I've fixed this obviousness here and here

There is no delay (yarp::os::Time::delay()) inside this loop. Although each error correction step is performed once per checkpoint, which accounts to once per 100 milliseconds in your app with current data, this blocks is constantly querying encoder values, performing complex calculations and sending velocity commands. I'd put said block inside this check and defer resuming the loop until the next cycle.

Ok. My mistake was thinking that I need to refresh in each cycle the current position of the joints: rightArmCurrentQ and leftArmCurrentQ but I've noticed that I can put a delay that exactly takes the period of time that we want to make a correction. This was the problem that caused an excessive use of CPU (permanent checkpoint condition check).

For reference, the period of the control loop in BasicCartesianControl is 50 milliseconds (for a single arm).

Following this reference, I've change the period, doubling the value of resolution variable to 20 but I have significantly decreased the speed of movement: 1cm/sec, taking 4 seconds to complete the trajectory of 4 cm displacement. The period of control loop now is 200ms. I can check the results in the real robot with this res value or try to put res=80 to get a period of 50ms (4sec/80). In the simulation, the app works great but only God knows in the real robot.

That's presumably the lack of a stop command (this won't work, as you noticed - see https://github.com/roboticslab-uc3m/yarp-devices/issues/120). Try switching to position control mode (ref), but don't forget to get back to velocity before the next iteration.

Yes! It's the best solution to be sure that the joints stop completely. I have helped myself with this change of mode, implementing the functions configAllDevicesToPosition and configAllDevicesToVelocity.

The result in simulation is very cool! (smooth and without parkinson xD) but I don't know what wonders can happen in the real robot

[PeterBowman]

It's probably better to rename the cmcRateMs variable, too (Ms stands for milliseconds).

I've noticed that I can put a delay that exactly takes the period of time that we want to make a correction.

Just a word of caution: all these calculations take their time and you are currently not accounting for it in yarp::os::Time::delay(cmcRateMs) (check my last comment). Each loop will start a bit later than it's supposed to, and such error adds up with every iteration. It probably won't be of significance if compared with a period of 200 ms, though.

In a rapid recap, I noticed that this demo of short, linearly controlled trajectories - as it currently is - could be entirely implemented with a sequence of movv calls (with no error correction involved at all).

[PeterBowman]

I wrote a script that should help you in obtaining the transformation matrix between the gripper and the tray center, for both arms. Launch TEO on the simulator and move the robot to the desired pose (e.g. via RPC). Then, run this:

import yarp
import kinematics_dynamics
import numpy as np
import scipy.linalg as lin

yarp.Network.init()

options = yarp.Property()
options.put('device','CartesianControlClient')

options.put('cartesianRemote','/teoSim/leftArm/CartesianControl')
options.put('cartesianLocal','/cc/teoSim/leftArm')
ddLeft = yarp.PolyDriver(options)

options.put('cartesianRemote','/teoSim/rightArm/CartesianControl')
options.put('cartesianLocal','/cc/teoSim/rightArm')
ddRight = yarp.PolyDriver(options)

iccLeft = kinematics_dynamics.viewICartesianControl(ddLeft)
iccRight = kinematics_dynamics.viewICartesianControl(ddRight)

xLeft = yarp.DVector()
xRight = yarp.DVector()

iccLeft.stat(xLeft)
iccRight.stat(xRight)

# N: gripper 
H_left_0_N = np.eye(4)
H_right_0_N = np.eye(4)

# https://stackoverflow.com/a/25709323
axis2dcm = lambda(axis): lin.expm3(np.cross(np.eye(3), axis))

H_left_0_N[:3,:3] = axis2dcm(xLeft[3:])
H_left_0_N[:3,3] = xLeft[:3]

H_right_0_N[:3,:3] = axis2dcm(xRight[3:])
H_right_0_N[:3,3] = xRight[:3]

# T: tray
H_left_0_T = np.eye(4)
H_right_0_T = np.eye(4)

H_left_0_T[:3,3] = [xLeft[0], 0, xLeft[2]]
H_right_0_T[:3,3] = [xRight[0], 0, xRight[2]]

H_left_N_T = np.dot(lin.inv(H_left_0_N), H_left_0_T)
H_right_N_T = np.dot(lin.inv(H_right_0_N), H_right_0_T)

# from yarp::math::dcm2axis
def dcm2axis(rot):
    axis = np.array([rot[2,1] - rot[1,2], rot[0,2] - rot[2,0], rot[1,0] - rot[0,1]])
    norm = lin.norm(axis)
    return (1.0 / norm) * axis * np.arctan2(0.5 * norm, 0.5 * (rot.trace() - 1))

twist_left_N_T = np.append(H_left_N_T[:3,3], dcm2axis(H_left_N_T[:3,:3]))
twist_right_N_T = np.append(H_right_N_T[:3,3], dcm2axis(H_right_N_T[:3,:3]))

# update tool coordinates
iccLeft.tool(yarp.DVector(twist_left_N_T))
iccRight.tool(yarp.DVector(twist_right_N_T))

# perform FK
iccLeft.stat(xLeft)
iccRight.stat(xRight)

# sample output (rounded and prettified):
# xLeft = xRight = [0.368 0.0 0.168 0.0 0.0 0.0]

ddLeft.close()
ddRight.close()

yarp.Network.fini()

Tested with:

set poss (-31.2829 14.0421 10.7908 -52.6362 81.5289 -88.4885) # left arm
set poss (31.2829 -14.0421 -10.7908 52.6362 -81.5289 88.4885) # right arm

For the record:

>>> twist_left_N_T
array([ 0.23911314,  0.03862077,  0.00701004, -1.23883784,  1.02024657,
        1.36949948])
>>> twist_right_N_T
array([ 0.23911314, -0.03862077,  0.00701004,  1.23883784,  1.02024657,
       -1.36949948])

Same info in DCM format at https://github.com/roboticslab-uc3m/teo-bimanipulation/issues/3#issuecomment-405298756.

[PeterBowman]

I have tweaked the keyboardController app from kin-dyn for use by TEO: https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/aeddf84e7b25b7aba6be01c7a25e7ee94ea1f1e9. Now, cartesian commands (joint commands are disabled) will move both arms at once, assuming that their TCPs have been properly configured (see previous comment).

[rsantos88]

Thank you so much @PeterBowman !! :smiley::smiley::clap::clap:

[PeterBowman]

Add this to leftArmKinematics.ini:

HN (0.00849934 -0.98679276 -0.16176465 0.23911314   0.12740183 -0.15938355 0.97896152 0.03862077   -0.99181477 -0.02892964 0.12436455 0.00701004   0.0 0.0 0.0 1.0)

...and this to rightArmKinematics.ini:

HN (0.00849934 0.98679276 -0.16176465 0.23911314   -0.12740183 -0.15938355 -0.97896152 -0.03862077   -0.99181477 0.02892964 0.12436455 0.00701004   0.0 0.0 0.0 1.0)

[rsantos88]

I'm sure it will be very useful. Thanks!!

I've upload this changes upload here: https://github.com/roboticslab-uc3m/teo-configuration-files/commit/c8717719294288dcfdc3aaf44509ac6413973ea5 I've been testing all of this changes in your app keyboardController and I realized that when using BasicCartesianControl using differential inverse kinematics, when it's doing a movement, it doesn't take into account the joint limits (or does not cyclically check this limits). So, when the joint reachs its limit, it stops and doesn't continue. On the other hand, the rest of joints that have not reached their limit continue their movement, producing a deviation of the trajectory. There is no warning message in the controller. We must to implement the limits, show a warning message and automatically stop the movement of all the joints. At first we need to modify:

• handleMovv()
• handleMovl()

[rsantos88]

I am going to update the state of this issue:

Thanks to the improvements made in https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/d2afc5e5368ed3cf9b29de91ed56c69cd0d0cc2e the problem with joint limits was solved. Now, when a joint reaches its limit, the kinematic chain stops totally. With the simulator everything works correctly. The next step was to execute movv (using the keyboardcontroller application) and see how clean the trajectory was by moving the arm on a single axis. The result wasn't the expected due to the existence of a force that has not been taken into account when it comes to correct movement in motion: the gravity. As a result of this, the arm tends to fall down, especially when we try to move the TCP on the X axis.

https://youtu.be/Xr_BD3iLT-M

Then, we tried to move the arm using movl because here there is a control loop closed. In the same way, @PeterBowman made an experimental implementation of this control over movv. The result improved when moving it on the Y axis but not on the X axis.

https://youtu.be/DmZu6hYrdwM

On the other hand, @PeterBowman told me the possibility of trying the movement using streaming commands of movement like pose. I tried it in my application. The result was great in the simulator as usual, but the same problem in the real robot.

Therefore, this is the issue. @PeterBowman and @jgvictores are working to solve this problems here: https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/166 and maybe in another opened issues. I keep watching the releases of cartesian controller to be able to advance in a correct movement of the tray for this demo

[rsantos88]

I've done some experiments with IPositionDirect to see the results. In the simulation, the movement is really good and fast behavior, depending on the number of points in which we divide our trajectory. The bash of teo-sim shows some warning messages like this: (with 100 points)

[debug] IControlMode2Impl.cpp:55 setControlModes(): (6)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (0, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (1, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (2, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (3, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (4, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (5, 1685286768)
[WARNING]number of streaming intput messages to be read is  77  and can overflow 
[WARNING]number of streaming intput messages to be read is  73  and can overflow 
[WARNING]number of streaming intput messages to be read is  61  and can overflow 
[WARNING]number of streaming intput messages to be read is  55  and can overflow 
[WARNING]number of streaming intput messages to be read is  44  and can overflow 
[WARNING]number of streaming intput messages to be read is  31  and can overflow 
[debug] IControlMode2Impl.cpp:55 setControlModes(): (6)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (0, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (1, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (2, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (3, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (4, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (5, 1685286768)
[WARNING]number of streaming intput messages to be read is  38  and can overflow 
[WARNING]number of streaming intput messages to be read is  56  and can overflow 
[WARNING]number of streaming intput messages to be read is  49  and can overflow 
[WARNING]number of streaming intput messages to be read is  37  and can overflow 
[WARNING]number of streaming intput messages to be read is  25  and can overflow 
[debug] IControlMode2Impl.cpp:55 setControlModes(): (6)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (0, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (1, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (2, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (3, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (4, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (5, 1685286768)
[WARNING]number of streaming intput messages to be read is  51  and can overflow 
[WARNING]number of streaming intput messages to be read is  38  and can overflow 
[WARNING]number of streaming intput messages to be read is  25  and can overflow 
[debug] IControlMode2Impl.cpp:55 setControlModes(): (6)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (0, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (1, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (2, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (3, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (4, 1685286768)
[debug] IControlMode2Impl.cpp:44 setControlMode(): (5, 1685286768)
[WARNING]number of streaming intput messages to be read is  68  and can overflow 
[WARNING]number of streaming intput messages to be read is  75  and can overflow 
[WARNING]number of streaming intput messages to be read is  63  and can overflow 
[WARNING]number of streaming intput messages to be read is  51  and can overflow 
[WARNING]number of streaming intput messages to be read is  39  and can overflow 
[WARNING]number of streaming intput messages to be read is  30  and can overflow 

We can achieve a smoother and slower movement, dividing even more the points of the trajectory, for example to 1000: -> Video: PositionDirect simulation (100 points) -> Video: PositionDirect simulation (1000 points)

I've tried to check what happen in the real robot. At the moment, when the movement is going to be executed, the arm hits a small blow, the drivers' LEDs can be seen in red and the arm suddenly falls and it loses the control. I'm going to leave the out messages of the launch in case it's useful: launchManipulation-positionDirect-tests.zip

[rsantos88]

[success] IPositionDirectRawImpl.cpp:36 setPositionRaw(): Sent to canId 17: pos -11.040409, time 50, ic 14.

[success] IPositionDirectRawImpl.cpp:54 setPositionRaw(): Sent "startPT". 1f 0. canId(17) via(200).

[warning] ICanBusSharerImpl.cpp:258 interpretMessage(): pt buffer full! canId: 17.

[warning] ICanBusSharerImpl.cpp:264 interpretMessage(): pt buffer empty. canId: 17.

[error] ICanBusSharerImpl.cpp:947 interpretMessage(): Got EMERGENCY from iPOS. 20 32 1 8 0 0 0 0. canId(17) via(80). DC-link under-voltage. canId: 17.

[error] ICanBusSharerImpl.cpp:947 interpretMessage(): Got EMERGENCY from iPOS. 40 23 1 9 20 0 0 0. canId(20) via(80). Short-circuit. canId: 20. (only id 20)

[rsantos88]

Ok, I've been doing different tests in order to check the correct PositionDirect operation and analyze the failures using this code:

#include <stdio.h>
#include <stdlib.h>

#include <vector>

#include <yarp/os/all.h>
#include <yarp/dev/all.h>

using namespace yarp::os;
using namespace yarp::dev;

int main(int argc, char *argv[]) {

    printf("WARNING: requires a running instance of OneCanBusOneWrapper\n");
    Network yarp;
    if (!Network::checkNetwork()) {
        printf("Please start a yarp name server first\n");
        return(-1);
    }
    Property options;
    options.put("device","remote_controlboard"); //remote_controlboard
    options.put("remote","/wrapper0");
    options.put("local","/local");

    PolyDriver dd(options);
    if(!dd.isValid()) {
      printf("RaveBot device not available.\n");
      dd.close();
      Network::fini();
      return 1;
    }

    IPositionDirect *posdir;
    IVelocityControl2 *vel;
    IEncodersTimed *enc;
    IControlMode2 *mode;

    bool ok = true;

    ok &= dd.view(posdir);
    ok &= dd.view(enc);
    ok &= dd.view(mode);

    if (!ok) {
        printf("[warning] Problems acquiring robot interface\n");
        return false;
    } else printf("[success] acquired robot interface\n");

    mode->setControlMode(0, VOCAB_CM_POSITION_DIRECT);

    printf("test positionDirect\n");

    for(float f=0.01; f<40; f+=0.5)
    {
        printf("moving position direct: (%f)\n",f);
        posdir->setPosition(0,f);
        yarp::os::Time::delay(0.05);
    }

    return 0;
}

In it I try to configure the Ipos to POSITION_DIRECT and send discreted positions (0.5 degree increments) with a frequency of 50ms.

On the other hand there is an instance of oneCanBusOneWrapper with the following configuration of the inifile, for the elbow of the right arm

/// oneCanBusOneWrapper.ini ///

[devCan0]
device CanBusControlboard
canDevice /dev/pcan2
canBusType CanBusPeak
canBitrate 1000000
types TechnosoftIpos
ids   18
maxs  83.5
mins -32.9
trs   160
refAccelerations  10.0
refSpeeds  10.0
encoderPulsess  4096

[wrapper0]
device controlboardwrapper2
name /wrapper0
period 60
joints 1
networks (devCan0)
devCan0 0 0 0 0

It works great!! Apparently we have to take a count this line (and the comment) to get it to work properly with the real robot. if we don't take into account the delay, the buffer will be full with the respective error: pt buffer full! @jgvictores Is there any reason for this limitation?

[jgvictores]

PT Mode performs at a fixed rate at driver level. This is great, because it's real-time right next to the motor, so network latencies will not affect performance of set of a pre-defined joint-space targets (positions). I'm not justifying how it's implemented, but providing the reason why they actually did it as it is. Naïve options:

1. First receive (e.g. via CAN-bus) all the trajectory, then execute each target at the exact time given the fixed period. The issue with this is: how much memory should we reserve for this? What happens if somebody wants to run a trajectory with thousands or millions of intermediate targets?
2. Receive the next target (e.g. via CAN-bus), execute it at exactly the planned time given the fixed period, repeat. The issue with this is: what happens if a target arrives late?

None of these options is the implemented solution. The iPos implementation is an intermediate solution, essentially a FIFO memory with 8 buffer positions (would have to check the iPos manual for the specific correct value). So, you start filling it in, once it is initially full you start running, and then continue feeding it targets (e.g. via CAN-bus) at the rate established by the fixed period.

• If you feed it too slow, the buffer will empty before time and movement will stop.
• If you feed it too fast, the buffer will get full (the pt buffer full! message you were seeing).

Hence, best to feed it at the most precise rate possible. Take into account that a PeriodicThread (YARP's old RateThread) will be more precise than adding a fixed delay at the end of your loop.

In the current implementation, this is set when we instance the CanBusControlboard class and may be modified via --ptModeMs. I guess you'll be asking if there is a minimum threshold. The answer is yes, and this minimum should be estimated by the time consumed by CAN-bus communications to feed all the individual drivers per period.

[jgvictores]

this line (and the comment)

The only restriction on increment/time is velocity. The current line and comment says Don't move more than 1 degree in 50 ms which maps out to 20 deg/s, which is a conservative measure. TEO's current configuration allows up to 1000 deg/s, which is extremely dangerous.

[smcdiaz]

Please, refer to CanOpen Programming Manual (P091.063.CANopen.iPOS.UM), chapter 9.2. (P091.063.CANopen.iPOS.UM.pdf).

[PeterBowman]

From 9.2.8:

If the motion is absolute, set in 2079h the actual position of the drive (read from object 6063h)

In our implementation, 0 is set as the initial joint pose upon requesting PT mode (IPositionDirect in YARP parlance). I'm not sure we ever touch bit 6 of 6060h (control word), which toggles relative-absolute positioning if set/unset. I guess the default is 0 and that's why this configuration step is not reflected in the PT absolute movement example, section 9.3. The whole process is replicated in our code and no attempts are made to change this mode whatsoever. In either case, I guess that any setPosition() issued away from the home position will wreak havoc because of misinterpreting the target position it is commanded to, i.e. the driver assumes it starts at 0 degrees instead of fetching the actual encoder values.

Sorry if I wasn't paying sufficient attention, @rsantos88, but to reproduce today's failed attemps:

1. Start launchManipulation, move all joints in TEO's right arm to home.
2. Move the elbow away from home (say, 45 degrees), still in IPositionControl mode.
3. Switch to IPositionDirect mode.
4. Issue setPosition to, say, 46 degrees.
5. TEO bends his elbow (no pun intended) way too fast, sometimes in the wrong direction (depending on the starting non-home pose and the final target).

Is that close to the observed behavior?

[rsantos88]

Is that close to the observed behavior?

Yes, totally like that. I'll share the testing code here:

#include <stdio.h>
#include <stdlib.h>

#include <vector>

#include <yarp/os/all.h>
#include <yarp/dev/all.h>

#define JOINT 3

using namespace yarp::os;
using namespace yarp::dev;

int main(int argc, char *argv[]) {

    printf("WARNING: requires a running instance of launchManipulation\n");
    Network yarp;
    if (!Network::checkNetwork()) {
        printf("Please start a yarp name server first\n");
        return(-1);
    }
    Property options;
    options.put("device","remote_controlboard"); //remote_controlboard
    options.put("remote","/teo/rightArm"); 
    options.put("local","/local");

    PolyDriver rightArm(options);
    if(!rightArm.isValid()) {
      printf("RightArm device not available.\n");
      rightArm.close();
      Network::fini();
      return 1;
    }

    IPositionDirect *posdir;
    IPositionControl2 *pos;
    IVelocityControl2 *vel;    
    IEncoders *enc;
    IControlMode2 *mode;

    bool ok = true;

    ok &= rightArm.view(posdir);
    ok &= rightArm.view(pos);
    ok &= rightArm.view(enc);
    ok &= rightArm.view(mode);

    if (!ok) {
        printf("[warning] Problems acquiring robot interface\n");
        return false;
    } else printf("[success] acquired robot interface\n");

    /** Axes number **/
    int numJoints;

    printf("-- testing POSITION MODE --\n");
    pos->getAxes(&numJoints);
    std::vector<int> positionMode(numJoints,VOCAB_CM_POSITION);
    if(! mode->setControlModes(positionMode.data())){
        printf("[warning] Problems setting position control: POSITION \n");
        return false;
    }

    printf("moving joint 0 to 10 degrees...\n");
    pos->positionMove(JOINT, 10);
    getchar();

    printf("-- testing POSITION DIRECT --\n");
    posdir->getAxes(&numJoints);
    std::vector<int> positionDirectMode(numJoints,VOCAB_CM_POSITION_DIRECT);
    if(! mode->setControlModes(positionDirectMode.data())){
        printf("[warning] Problems setting position control: POSITION_DIRECT \n");
        return false;
    }

    double encValue;
    if ( ! enc->getEncoder(JOINT,&encValue) ){
        printf("[ERROR] Failed getEncoders of right-arm\n");
        return false;
    }

    printf("Current ENC value: %f\n", encValue);

    getchar();

    // vector with the degrees
    std::vector<double> jointPosition(numJoints); // vector de 6 joints

    while(encValue<=20)
    {
        encValue+=0.1;
        jointPosition[JOINT] = encValue;

        printf("-> Joint position of right-arm: (");
        for(int i=0; i<jointPosition.size(); i++)
            printf("%f ",jointPosition[i]);
        printf(")\n ");

        std::vector<double> jointPath(&jointPosition[0], &jointPosition[0]+7); //teoSim (+6) teo (+7)
        posdir->setPositions(jointPath.data());
        yarp::os::Time::delay(0.05); //0.05
    }    

    return 0;
}

Note: Don't take into account the use of posdir->setPositions(jointPath.data()); instead of posdir->setPosition(JOINT, encValue); This is because I've been doing other tests before, moving various joints in POSITION_DIRECT at the same time.

[rsantos88]

In our implementation, 0 is set as the initial joint pose upon requesting PT mode (IPositionDirect in YARP parlance). I'm not sure we ever touch bit 6 of 6060h (control word), which toggles relative-absolute positioning if set/unset. I guess the default is 0 and that's why this configuration step is not reflected in the PT absolute movement example, section 9.3. The whole process is replicated in our code and no attempts are made to change this mode whatsoever. In either case, I guess that any setPosition() issued away from the home position will wreak havoc because of misinterpreting the target position it is commanded to, i.e. the driver assumes it starts at 0 degrees instead of fetching the actual encoder values.

This problem has been fixes now here https://github.com/roboticslab-uc3m/yarp-devices/pull/197 You can delete the "testing" branch in the link if you are agree with the change

[PeterBowman]

More issues with the IPositionDirect interface have been identified, see (and expand if needed) https://github.com/roboticslab-uc3m/yarp-devices/issues/198.

[PeterBowman]

Okay, a "good" movement in the cartesian space has been achieved:

This clip shows exampleScrewTheoryTrajectory run on real TEO. I generate a linear path following a trapezoidal velocity profile, much like with a point-to-point CMC-controlled movl command. However, I built my own control loop that forwards all joint poses to the robot through the IPositionDirect interface after an IK step managed with screw theory.

Keep https://github.com/roboticslab-uc3m/yarp-devices/issues/198 in mind when developing applications. For now, my keyboardController app has to be carefully instantiated due to a kinda faulty mode change.

[rsantos88]

I would like to resume the use of this script to obtaining the transformation matrix between the gripper and the tray center, for both arms. The pose I'm leaving to get this data in joint position is:

left-arm:    (-31.283699, 14.04759, 10.804402, -60.000894, 75, -88.479388)
right-arm:  (31.283699, -14.04759, -10.804402, 60.000894, -75, 88.479388)

When I run the python script, the result values calculated are:

[ 0.23194013  0.01027258 -0.00223408 -1.77420782  1.72593331  1.79445854]
[ 0.23194013 -0.01027258 -0.00223408  1.77420782  1.72593331 -1.79445854]

So, I've put it in my code using appendLink function to change the TCP in booth arms, but when I've checked the forward kinematic, It should show the same values for both arms (unifying the TCP). The values are the same but the signs do not match in some cases..

-> Origin Pose of left-arm: (0.365755 -0.000000 0.214345 -0.536150 0.529716 0.552422 )
-> Origin Pose of right-arm: (0.365755 0.000000 0.214345 0.536150 0.529716 -0.552422 )

[PeterBowman]

@rsantos88 I've fixed that script, please check it again.

[rsantos88]

@rsantos88 I've fixed that script, please check it again.

Thank you so much @PeterBowman for fixing the script and for your explanations yesterday :smiley:

[rsantos88]

Due to the problems mentioned in velocity mode https://github.com/roboticslab-uc3m/teo-bimanipulation/issues/3#issuecomment-435919430 , @smcdiaz told me to do a the test of commanding a joint in speed and check if it maintains the position. As you can see, the motor tries to maintain that speed (3 degrees per second) until it slows down as an effect of the force of gravity. If I apply a force in the direction of movement, the joint keeps moving. If I apply a force in the opposite direction, the motor maintains the position:

[PeterBowman]

Our low level controllers have seen several improvements in the last two years, new control modes have been enabled and help can be found in examples and tutorials: https://github.com/roboticslab-uc3m/yarp-devices/issues/254#issuecomment-834710065.

• I don't know if I can use something like TwoLimbCartesianControl or something similar, to control and moving at the same time the two arms, with one TCP (the trail). From what I've seen, it looks like it's in development, isn't it? At the moment, my application is alternating each arm, moving one after the other...

Coordinated dual-arm motion in cartesian space is now possible, see https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/190#issuecomment-856661951.

I presume the original problem has been already solved. If that is not correct, I'd suggest to open a new issue on a single, specific topic.

For the record, this script could be used in future demos: https://github.com/roboticslab-uc3m/teo-bimanipulation/issues/3#issuecomment-404339305.