Example of CartesianControlClient is not working properly

[rsantos88]

Errors found: The maximum number of iterations is exceeded and movj(): invKin failed.
Terminal 1:

teo@teo-oliver:~/repos/kinematics-dynamics/example/cpp/exampleCartesianControlClient$ cd build/
teo@teo-oliver:~/repos/kinematics-dynamics/example/cpp/exampleCartesianControlClient/build$ ./exampleCartesianControlClient 
yarp: Port /CartesianControlExample/rpc:c active at tcp://2.2.2.100:10034/
yarp: Port /CartesianControlExample/command:o active at tcp://2.2.2.100:10035/
yarp: Port /CartesianControlExample/state:i active at tcp://2.2.2.100:10036/
yarp: Sending output from /CartesianControlExample/rpc:c to /teoSim/rightArm/CartesianControl/rpc_transform:s using tcp
yarp: Sending output from /CartesianControlExample/command:o to /teoSim/rightArm/CartesianControl/command:i using udp
yarp: Receiving input from /teoSim/rightArm/CartesianControl/state:o to /CartesianControlExample/state:i using udp
[success] DeviceDriverImpl.cpp:60 open(): Connected to remote.
[INFO]created device <CartesianControlClient>. See C++ class roboticslab::CartesianControlClient for documentation.
[success] acquired interface
[warning] ICartesianControlImpl.cpp:150 stat(): FK stream timeout, sending RPC request.
Controller status (forward kinematics): 0.000000 -0.346920 -0.221806 0.000000 1.000000 -0.000000 90.000000 
Position 1: poss (0 0 0 90 0 0 0)
Position 2: move forward along axis X
Position 3: move right along axis Y
Position 4: rotate 10 degrees about axis Y
Position 5: rotate 30 degrees about axis Y
yarp: Removing output from /CartesianControlExample/rpc:c to /teoSim/rightArm/CartesianControl/rpc_transform:s
yarp: Removing output from /CartesianControlExample/command:o to /teoSim/rightArm/CartesianControl/command:i
yarp: Removing input from /teoSim/rightArm/CartesianControl/state:o to /CartesianControlExample/state:i

Terminal 2:

yarp: Receiving input from /CartesianControlExample/rpc:c to /teoSim/rightArm/CartesianControl/rpc_transform:s using tcp
yarp: Receiving input from /CartesianControlExample/command:o to /teoSim/rightArm/CartesianControl/command:i using udp
yarp: Sending output from /teoSim/rightArm/CartesianControl/state:o to /CartesianControlExample/state:i using udp
[debug] RpcResponder.cpp:46 respond(): Got: [stat]
[debug] RpcResponder.cpp:46 respond(): Got: [movj] 0.390926 -0.346663 0.166873 -0.004334 0.70944 0.704752 0.353119
[info] ICartesianControlImpl.cpp:85 movj(): dist[0]: 0.249984
[info] ICartesianControlImpl.cpp:89 movj():  -->candidate: 0.024998
[info] ICartesianControlImpl.cpp:85 movj(): dist[1]: 0.249856
[info] ICartesianControlImpl.cpp:85 movj(): dist[2]: 0.249958
[info] ICartesianControlImpl.cpp:85 movj(): dist[3]: 89.750126
[info] ICartesianControlImpl.cpp:89 movj():  -->candidate: 8.975013
[info] ICartesianControlImpl.cpp:85 movj(): dist[4]: 0.249956
[info] ICartesianControlImpl.cpp:85 movj(): dist[5]: 0.249539
[info] ICartesianControlImpl.cpp:92 movj(): max_time[final]: 8.975013
[info] ICartesianControlImpl.cpp:106 movj(): vmo[0]: 0.027853
[info] ICartesianControlImpl.cpp:106 movj(): vmo[1]: 0.027839
[info] ICartesianControlImpl.cpp:106 movj(): vmo[2]: 0.027850
[info] ICartesianControlImpl.cpp:106 movj(): vmo[3]: 10.000000
[info] ICartesianControlImpl.cpp:106 movj(): vmo[4]: 0.027850
[info] ICartesianControlImpl.cpp:106 movj(): vmo[5]: 0.027804
[success] ICartesianControlImpl.cpp:138 movj(): Waiting
[debug] RpcResponder.cpp:46 respond(): Got: [movj] 0.5 -0.346663 0.166873 -0.004334 0.70944 0.704752 0.353119
[info] ICartesianControlImpl.cpp:85 movj(): dist[0]: 19.800194
[info] ICartesianControlImpl.cpp:89 movj():  -->candidate: 1.980019
[info] ICartesianControlImpl.cpp:85 movj(): dist[1]: 0.249088
[info] ICartesianControlImpl.cpp:85 movj(): dist[2]: 0.391816
[info] ICartesianControlImpl.cpp:85 movj(): dist[3]: 64.675673
[info] ICartesianControlImpl.cpp:89 movj():  -->candidate: 6.467567
[info] ICartesianControlImpl.cpp:85 movj(): dist[4]: 0.367804
[info] ICartesianControlImpl.cpp:85 movj(): dist[5]: 5.275263
[info] ICartesianControlImpl.cpp:92 movj(): max_time[final]: 6.467567
[info] ICartesianControlImpl.cpp:106 movj(): vmo[0]: 3.061459
[info] ICartesianControlImpl.cpp:106 movj(): vmo[1]: 0.038513
[info] ICartesianControlImpl.cpp:106 movj(): vmo[2]: 0.060582
[info] ICartesianControlImpl.cpp:106 movj(): vmo[3]: 10.000000
[info] ICartesianControlImpl.cpp:106 movj(): vmo[4]: 0.056869
[info] ICartesianControlImpl.cpp:106 movj(): vmo[5]: 0.815649
[success] ICartesianControlImpl.cpp:138 movj(): Waiting
[debug] RpcResponder.cpp:46 respond(): Got: [movj] 0.5 -0.5 0.166873 -0.004334 0.70944 0.704752 0.353119
[error] ICartesianSolverImpl.cpp:139 invKin(): -5: The maximum number of iterations is exceeded
[error] ICartesianControlImpl.cpp:77 movj(): invKin failed.
[debug] RpcResponder.cpp:46 respond(): Got: [movj] 0.5 -0.5 0.166873 0.0 1.0 0.0 10.0
[error] ICartesianSolverImpl.cpp:139 invKin(): -5: The maximum number of iterations is exceeded
[error] ICartesianControlImpl.cpp:77 movj(): invKin failed.
[debug] RpcResponder.cpp:46 respond(): Got: [movj] 0.5 -0.5 0.166873 0.0 1.0 0.0 30.0
[error] ICartesianSolverImpl.cpp:139 invKin(): -5: The maximum number of iterations is exceeded
[error] ICartesianControlImpl.cpp:77 movj(): invKin failed.
yarp: Removing input from /CartesianControlExample/rpc:c to /teoSim/rightArm/CartesianControl/rpc_transform:s
yarp: Removing input from /CartesianControlExample/command:o to /teoSim/rightArm/CartesianControl/command:i
yarp: Removing output from /teoSim/rightArm/CartesianControl/state:o to /CartesianControlExample/state:i

[jgvictores]

Teo-specific example, blocked by all issues related to unifying kinematic model, namely roboticslab-uc3m/teo-main#23, roboticslab-uc3m/teo-main#15, and future derived issues.

[rsantos88]

Example working at 1ab71af.

[rsantos88]

Merge done at 2a2954d.

[rsantos88]

Just for the record, some pointers to misplaced comments: :-)

• Mismatch in install of share/models teo-grasp#2 (comment)
• Mismatch in install of share/models teo-grasp#2 (comment)

[rsantos88]

@jgvictores sorry for the forgetfulness...
I'll move the issue here

[rsantos88]

Some notes of BasicCartesianControl:

• connecting with it using yarp rpc /teoSim/rightArm/CartesianControl/rpc:s and doing stat, we can see the first 3 position parameters (x,y,z) and other 3 for orientation (but when I did the example, it worked with 4 parameters, isn't it?: 3 for axis and 1 for rotation)
• When we take it to the position (0 0 0 90 0 0) the result of stat according to the example should be:
  0.390926 -0.346663 0.166873 -0.004334 0.70944 0.704752 0.353119, but it shows:

>>stat
Response: [ccnc] 0.389496 -0.34692 0.16769 0.0 0.0 0.0

• so, movj works with six parameters and the representation of the orientation is... diferent?
• we can move the TCP with the desired orientation, sending the value in that three parameters, thinking that the rotation is applied on the choosen coordinate axis. For example:

>>movj 0.39 -0.34692 0.16769 0.0 -0.2 0.0
Response: [ok]

It works well but we don't know the units (degrees or radians?)

[rsantos88]

@PeterBowman said:

If you want to read axis-angle data, launch the server device with --angleRepr axisAngle. Similarly, start the client device with --transform to connect it with /portname/rpc_transform:s (instead of the usual /portname/rpc:s, which falls back to standard KDL twist representation, that is, 3xtranslation + 3xrotation in scaled axis-angle notation).

Please read /issues/113.

It works well but we don't know the units (degrees or radians?)

There was a major documentation overhaul several months ago, check http://robots.uc3m.es/dox-kinematics-dynamics/.

[jgvictores]

Just to sum up, major merge was 2a2954d and most relevant line is

kinematics-dynamics/examples/cpp/exampleCartesianControlClient/exampleCartesianControlClient.cpp

Line 54 in 2a2954d

options.put("transform", 1); // Was yarp::os::Value::getNullValue()

[jgvictores]

...for this particular use case, and for the time being.

[rsantos88]

I want to ask something about this exampleCartesianControlClient.cpp
The result of executing these lines in cpp example:

    iCartesianControl->stat(status, vector);
    printf("Controller status (forward kinematics): ");
    for (int i = 0; i < vector.size(); i++)
    {
        printf("%f ", vector[i]);
    }
printf("\n");

are this values:

Controller status (forward kinematics): 0.000000 -0.346920 -0.221806 0.000000 1.570796 -0.000000

I can't see the same result when we have the robot in a initial position and we do stat using yarp rpc /teoSim/rightArm/CartesianControl/rpc_transform:s:

Response: [ccnc] 3.25149848407618e-17 -0.34692 -0.221806 1.53080849893419e-16 1.0 -3.06161699786838e-17 90.0

... but it's the same using yarp rpc /teoSim/rightArm/CartesianControl/rpc:s

Response: [ccnc] 3.25149848407618e-17 -0.34692 -0.221806 2.40458836715224e-16 1.5707963267949 -4.80917673430447e-17

so, we are getting the "compressed" representation with six values (three of position and three of orientation) in the example. We want to work with the four values of orientation like here

Is options.put("transform", 1); working well when we call the stat function?

[jgvictores]

Related with WIP at #145, essentially options.put("transform", 1); was always a hack. As such, it affects RPC, and example broke when stat started using streaming rather than RPC.

Options:

1. Wait for WIP at Make solver implementations more aware of their internal state #145, which can take a while (and block this issue).
2. Change the example to use server-client 3-pos and 3-ori.
3. Change the example to use KinRepresentation as in https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/f1f4ec97a50ae98831d0003821059ac881fa468c/tests/testKinRepresentation.cpp

[PeterBowman]

@rsantos88 should be fixed at da0a4ac, RPC is enforced now if users pass on the --transform option.

[PeterBowman]

essentially options.put("transform", 1); was always a hack

This is arguable. The main hack now is the --angleRepr option still provided to the cartesian server, which I left for backwards compatibility as a result of #113. In fact, two interfaces have to be considered: yarp rpc and the C++ API. Proper handling of angle representations in user code should be of user's concern only, thus several conversion methods have been provided along with a unified and properly documented interface. However, old behavior has been preserved by means of a new RPC port, <prefix>/rpc_transform:s, which can be accessed and queried via yarp rpc. Since I found the lack of a C++ API counterpart quite counterintuitive (and achieving this was rather cheap while sparing the need to rewrite the example app), the client device was extended with a new --transform option in order to mimic such behavior. Again, it is the user's responsibility to properly instantiate the client part (with or without said option) as it is to pass on the adequate (if any) --angleRepr to the server part. There are other ideas I'd like to expand on later (e.g. let both client and server perform a hand-shake on the desired representation during the configuration phase of the former), but keep in mind that both device options should be treated alongside each other.

TL;DR: (bugs aside) please use conversion methods, don't rely on transformations unless both client and server devices are properly configured. See #113 (comment).

[PeterBowman]

Tested and works, thanks for reporting this!