Introducing a solver for kinematic trees

[PeterBowman]

KdlSolver is a general-purpose solver for kinematic chains. TEO is a tree, though. KDL knows how to treat both chains and trees, offering geometric primitives as well as FK, IK, ID... solvers: wiki1, wiki2, docs.

I have started the implementation of KdlTreeSolver at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/db2c6b7773e7c67ab812f2f957363a04391513cf. It is meant to parse .ini files in a similar manner to the way KdlSolver does, but accounting for multiple interconnected chains. I have also prepared teo-trunk-arms-fetch.ini at https://github.com/roboticslab-uc3m/teo-configuration-files/commit/e57dc470b0fb3ca99dc64cdd5d7f3afeff12d5e6 for testing. It looks like this:

[chain trunk]
hook "root"
endpoint false
numLinks 2

// dh-leftArm.csv (deprecated) with second X axis pointing upwards
link_0 (offset   0.0) (D 0.1932) (A 0.0) (alpha -90.0)
link_1 (offset -90.0) (D    0.0) (A 0.0) (alpha   0.0)

[chain leftArm]
hook "trunk"
endpoint true
numLinks 6

// H_hip_leftArm
H0 (1 0 0 0.305  0 1 0 0  0 0 1 0.34692  0 0 0 1)

// dh-leftArm.csv
link_0 (offset   0.0) (D 0.0     ) (A 0.0      ) (alpha -90.0) (mass 0       ) (cog 0 0 0) (inertia 0 0 0)
link_1 (offset -90.0) (D 0.0     ) (A 0.0      ) (alpha -90.0) (mass 0       ) (cog 0 0 0) (inertia 0 0 0)
link_2 (offset -90.0) (D -0.32901) (A 0.0      ) (alpha -90.0) (mass 1.750625) (cog 0 0 0) (inertia 0 0 0)
link_3 (offset   0.0) (D 0.0     ) (A 0.0      ) (alpha  90.0) (mass 0       ) (cog 0 0 0) (inertia 0 0 0)
link_4 (offset   0.0) (D -0.215  ) (A 0.0      ) (alpha -90.0) (mass 2.396   ) (cog 0 0 0) (inertia 0 0 0)
link_5 (offset -90.0) (D 0.0     ) (A -0.09    ) (alpha   0.0) (mass 0.300   ) (cog 0 0 0) (inertia 0 0 0)

// dh-fetch.csv
HN (0 0 -1 -0.0975  -1 0 0 0    0 1 0 0    0 0 0 1)

[chain rightArm]
hook "trunk"
endpoint true
numLinks 6

// H_hip_rightArm
H0 (1 0 0 0.305  0 1 0 0  0 0 1 -0.34692  0 0 0 1)

// dh-rightArm.csv
link_0 (offset   0.0) (D 0.0     ) (A 0.0      ) (alpha -90.0) (mass 0       ) (cog 0 0 0) (inertia 0 0 0)
link_1 (offset -90.0) (D 0.0     ) (A 0.0      ) (alpha -90.0) (mass 0       ) (cog 0 0 0) (inertia 0 0 0)
link_2 (offset -90.0) (D -0.32901) (A 0.0      ) (alpha -90.0) (mass 1.750625) (cog 0 0 0) (inertia 0 0 0)
link_3 (offset   0.0) (D 0.0     ) (A 0.0      ) (alpha  90.0) (mass 0       ) (cog 0 0 0) (inertia 0 0 0)
link_4 (offset   0.0) (D -0.215  ) (A 0.0      ) (alpha -90.0) (mass 2.396   ) (cog 0 0 0) (inertia 0 0 0)
link_5 (offset -90.0) (D 0.0     ) (A -0.09    ) (alpha   0.0) (mass 0.300   ) (cog 0 0 0) (inertia 0 0 0)

// dh-fetch.csv
HN (0 0 -1 -0.0975  -1 0 0 0    0 1 0 0    0 0 0 1)

At the end we have three simple chains forming a 14-DOF tree (2 joints for the trunk + 6 joints for each arm), with both arms being the endpoints (i.e. their last HN are the TCPs of this tree).

So far this solver should cope well with BasicCartesianControl as long as its properly instantiated so that it connects to the three remote joint controllers. The remotecontrolboardremapper is used instead of remote_controlboard (in fact, the former wraps several instances of the latter), here is the full command (cf. https://github.com/roboticslab-uc3m/teo-configuration-files/issues/28):

yarpdev --device BasicCartesianControl \
        --name /bcc \
        --robot remotecontrolboardremapper \
        --localPortPrefix /local/bcc \
        --axesNames "(AxialTrunk FrontalTrunk \
                      FrontalLeftShoulder SagittalLeftShoulder AxialLeftShoulder FrontalLeftElbow AxialLeftWrist FrontalLeftWrist \
                      FrontalRightShoulder SagittalRightShoulder AxialRightShoulder FrontalRightElbow AxialRightWrist FrontalRightWrist)" \
        --remoteControlBoards "(/teoSim/trunk /teoSim/leftArm /teoSim/rightArm)" \
        --solver KdlTreeSolver \
        --ik nrjl \
        --from teo-trunk-arms-fetch.ini \
        --eps 0.001 \
        --maxIter 1000

So far it should work with cartesian inv, movj, movi, twist and gcmp commands. Others (such as movl and movv) are not prepared for dealing with multiple TCPs, hence BasicCartesianControl requires a little more work. The stat command is the easiest way to check the tree is (or should be) well-formed.

Problem: IK only seems to work for the current position. If we try to request any other pose, max iterations are hit (tested with up to --maxIter 50000) by the Newton-Raphson solver (--ik nrjl). The other available solver (--ik online) returns NaNs.

[PeterBowman]

Also tested (successfully) with a fixed trunk configuration:

yarpdev --device BasicCartesianControl \
        --name /bcc \
        --robot remotecontrolboardremapper \
        --localPortPrefix /local/bcc \
        --axesNames "(FrontalLeftShoulder SagittalLeftShoulder AxialLeftShoulder FrontalLeftElbow AxialLeftWrist FrontalLeftWrist \
                      FrontalRightShoulder SagittalRightShoulder AxialRightShoulder FrontalRightElbow AxialRightWrist FrontalRightWrist)" \
        --remoteControlBoards "(/teoSim/leftArm /teoSim/rightArm)" \
        --solver KdlTreeSolver \
        --ik nrjl \
        --from teo-fixedTrunk-arms-fetch.ini \
        --eps 0.001 \
        --maxIter 1000

[chain leftArm]
hook "root"
endpoint true
numLinks 6

// dh-root-leftArm.csv
H0 (0 1 0 0    0 0 1 0.3469    1 0 0 0.4982    0 0 0 1)

// dh-leftArm.csv
link_0 (offset   0.0) (D 0.0     ) (A 0.0      ) (alpha -90.0) (mass 0       ) (cog 0 0 0) (inertia 0 0 0)
link_1 (offset -90.0) (D 0.0     ) (A 0.0      ) (alpha -90.0) (mass 0       ) (cog 0 0 0) (inertia 0 0 0)
link_2 (offset -90.0) (D -0.32901) (A 0.0      ) (alpha -90.0) (mass 1.750625) (cog 0 0 0) (inertia 0 0 0)
link_3 (offset   0.0) (D 0.0     ) (A 0.0      ) (alpha  90.0) (mass 0       ) (cog 0 0 0) (inertia 0 0 0)
link_4 (offset   0.0) (D -0.215  ) (A 0.0      ) (alpha -90.0) (mass 2.396   ) (cog 0 0 0) (inertia 0 0 0)
link_5 (offset -90.0) (D 0.0     ) (A -0.09    ) (alpha   0.0) (mass 0.300   ) (cog 0 0 0) (inertia 0 0 0)

// dh-fetch.csv
HN (0 0 -1 -0.0975  -1 0 0 0    0 1 0 0    0 0 0 1)

[chain rightArm]
hook "root"
endpoint true
numLinks 6

// dh-root-rightArm.csv
H0 (0 1 0 0    0 0 1 -0.3469    1 0 0 0.4982    0 0 0 1)

// dh-rightArm.csv
link_0 (offset   0.0) (D 0.0     ) (A 0.0      ) (alpha -90.0) (mass 0       ) (cog 0 0 0) (inertia 0 0 0)
link_1 (offset -90.0) (D 0.0     ) (A 0.0      ) (alpha -90.0) (mass 0       ) (cog 0 0 0) (inertia 0 0 0)
link_2 (offset -90.0) (D -0.32901) (A 0.0      ) (alpha -90.0) (mass 1.750625) (cog 0 0 0) (inertia 0 0 0)
link_3 (offset   0.0) (D 0.0     ) (A 0.0      ) (alpha  90.0) (mass 0       ) (cog 0 0 0) (inertia 0 0 0)
link_4 (offset   0.0) (D -0.215  ) (A 0.0      ) (alpha -90.0) (mass 2.396   ) (cog 0 0 0) (inertia 0 0 0)
link_5 (offset -90.0) (D 0.0     ) (A -0.09    ) (alpha   0.0) (mass 0.300   ) (cog 0 0 0) (inertia 0 0 0)

// dh-fetch.csv
HN (0 0 -1 -0.0975  -1 0 0 0    0 1 0 0    0 0 0 1)

[PeterBowman]

Still testing the fixed trunk config, I noticed that --ik online is best suited for small increments. If commanded to travel a longer distance (e.g. via movj), the TCPs stop earlier. It doesn't sound surprising, though, since this algorithm is derived from Newton-Raphson using only one iteration (docs).

[PeterBowman]

Problem: IK only seems to work for the current position. If we try to request any other pose, max iterations are hit (tested with up to --maxIter 50000) by the Newton-Raphson solver (--ik nrjl). The other available solver (--ik online) returns NaNs.

This has all to do with a missing lambda parameter (and division by zero), added and defaulted at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/7e27671a3be91e11b9df6cf22c4b9878823c90f1. Now it works nicely, see this test with Newton-Raphson solver and movj:

https://user-images.githubusercontent.com/9977198/107262352-bad67480-6a40-11eb-8af3-53a575c41129.mp4

[PeterBowman]

Commit https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/4f49c39a2506c66c175560a9b24d2dc8979db4fb adds support for passing matrices via new weightJS and weightTS parameters related to joint space and cartesian space weight coefficients, respectively. These must be square, symmetric and positive definite, defaulting to the identity matrix. Documented as:

• setWeightJS: joint space weighting symmetric matrix (...) being used as a weight for the norm of the joint space speed it HAS TO BE symmetric and positive definite. We can actually deal with matrices containing a symmetric and positive definite block and 0s otherwise. Taking a diagonal matrix as an example, a 0 on the diagonal means that the corresponding joints will not contribute to the motion of the system. On the other hand, the bigger the value, the most the corresponding joint will contribute to the overall motion. The obtained solution q_dot will actually minimize the weighted norm `sqrt(q_dot'(M_q^-2)q_dot)`. In the special case we deal with, it does not make sense to invert M_q but what is important is the physical meaning of all this : a joint that has a zero weight in M_q will not contribute to the motion of the system and this is equivalent to saying that it gets an infinite weight in the norm computation.
• setWeightTS: task space weighting symmetric matrix (...) being used as a weight for the norm of the error (in terms of task space speed) it HAS TO BE symmetric and positive definite. We can actually deal with matrices containing a symmetric and positive definite block and 0s otherwise. Taking a diagonal matrix as an example, a 0 on the diagonal means that the corresponding task coordinate will not be taken into account (ie the corresponding error can be really big). If the rank of the jacobian is equal to the number of task space coordinates which do not have a 0 weight in M_x, the weighting will actually not impact the results (ie there is an exact solution to the velocity inverse kinematics problem). In cases without an exact solution, the bigger the value, the most the corresponding task coordinate will be taken into account (ie the more the corresponding error will be reduced). The obtained solution will minimize the weighted norm `sqrt(|x_dot-Jq_dot|'(M_x^2)|x_dot-Jq_dot|)`.

In other words, taking an identity matrix and setting the n-th diagonal element to zero means that the n-th joint (in the joint space weighting matrix) will stay fixed. This can be useful for restraining motion in the trunk joints.

Use these parameters in the .ini file as follows (weightJS: number of joints, weightTS: number of TCPs * 6) and adjust accordingly:

weightJS ( 1 0 0 0 0 0 0 0 0 0 0 0 0 0 \
           0 1 0 0 0 0 0 0 0 0 0 0 0 0 \
           0 0 1 0 0 0 0 0 0 0 0 0 0 0 \
           0 0 0 1 0 0 0 0 0 0 0 0 0 0 \
           0 0 0 0 1 0 0 0 0 0 0 0 0 0 \
           0 0 0 0 0 1 0 0 0 0 0 0 0 0 \
           0 0 0 0 0 0 1 0 0 0 0 0 0 0 \
           0 0 0 0 0 0 0 1 0 0 0 0 0 0 \
           0 0 0 0 0 0 0 0 1 0 0 0 0 0 \
           0 0 0 0 0 0 0 0 0 1 0 0 0 0 \
           0 0 0 0 0 0 0 0 0 0 1 0 0 0 \
           0 0 0 0 0 0 0 0 0 0 0 1 0 0 \
           0 0 0 0 0 0 0 0 0 0 0 0 1 0 \
           0 0 0 0 0 0 0 0 0 0 0 0 0 1 )

weightTS ( 1 0 0 0 0 0 0 0 0 0 0 0 \
           0 1 0 0 0 0 0 0 0 0 0 0 \
           0 0 1 0 0 0 0 0 0 0 0 0 \
           0 0 0 1 0 0 0 0 0 0 0 0 \
           0 0 0 0 1 0 0 0 0 0 0 0 \
           0 0 0 0 0 1 0 0 0 0 0 0 \
           0 0 0 0 0 0 1 0 0 0 0 0 \
           0 0 0 0 0 0 0 1 0 0 0 0 \
           0 0 0 0 0 0 0 0 1 0 0 0 \
           0 0 0 0 0 0 0 0 0 1 0 0 \
           0 0 0 0 0 0 0 0 0 0 1 0 \
           0 0 0 0 0 0 0 0 0 0 0 1 )

[PeterBowman]

Now also available for linear paths (movl/movv commands):

https://user-images.githubusercontent.com/9977198/107791327-19a73100-6d54-11eb-86fe-a63b8db492d6.mp4

[PeterBowman]

Merged at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/1a314444223bb9268702cb0b760ac7401de29575. Caveats:

• Not implemented: appendLink, restoreOriginalChain (tool command). Actually feasible, but kinda clumsy since most tree solvers don't implement KDL::SolverI::updateInternalDataStructures yet (example), thus new instances would need to be re-created via constructor.
• Not working: invDyn for external forces (forc command) because of https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/162.

[PeterBowman]

Thanks to KDL we can define open-loop chains following a tree structure. However, it has no support for closed-loop structures, i.e. graphs. Commit https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/8ce09b8c78acee3448bb22ac21f7d1f704223cf2 is a workaround that assumes two or more TCPs can be merged in such a way that kinematics are solved for each tree endpoint independently, but only a single input is required.

We have already exploited a similar trick in the past for the tray-balance demo (cc @rsantos88). In that scenario, two cartesian controllers took care of managing a single TEO's arm each, and we manipulated the .ini conveniently so that the last HN transformations corresponded to the same point in space when TEO was holding the tray. This point was a "virtual" TCP assumed to be located in the center of the tray, same orientation as TEO root. We built a single 6-element coordinate in the cartesian space on each iteration and sent it twice, thus commanding both arms separately.

With the new KdlTreeSolver we could just repeat this procedure and send a single 12-element command to just one cartesian server, where both TCPs are combined in a 6+6 manner. This last commit, however, adds a mergeWith parameter that can be added to a tree leaf to signify that the endpoint (TCP) of that leaf is to be merged with another TCP in the same tree. In this manner, you can send a 6-element vector (coordinates in cartesian space), which then are reconstructed by the solver (6+6), treated separately and linked to the corresponding TCPs. It's just a shorthand, yet it aims to simplify things a bit by reducing duplication.

The following demo showcases a keyboard-controlled simulation scenario where bot TEO's arms are partially extended (elbows rotated -90º) and both TCPs merged in the middle point. Note that motion is nicely coordinated, specially when orientation changes (the virtual mid-point does not translate).

Sequence: translX, translY, translZ, rotX, rotY, rotZ.

https://user-images.githubusercontent.com/9977198/121172699-4083f880-c858-11eb-856a-443b6619102a.mp4

Commands and configuration file:

yarpdev --device BasicCartesianControl \
        --name /bcc \
        --robot remotecontrolboardremapper \
        --localPortPrefix /local/bcc \
        --axesNames "(FrontalLeftShoulder SagittalLeftShoulder AxialLeftShoulder FrontalLeftElbow AxialLeftWrist FrontalLeftWrist \
                      FrontalRightShoulder SagittalRightShoulder AxialRightShoulder FrontalRightElbow AxialRightWrist FrontalRightWrist)" \
        --remoteControlBoards "(/teoSim/leftArm /teoSim/rightArm)" \
        --solver KdlTreeSolver \
        --ik nrjl \
        --from teo-fixedTrunk-arms-fetch-merged.ini \
        --eps 0.001 \
        --maxIter 1000

keyboardController --remoteCartesian /bcc --skipRCB (don't send joint commands)

teo-fixedTrunk-arms-fetch-merged.ini

```ini [chain leftArm] hook "root" endpoint true numLinks 6 // dh-root-leftArm.csv H0 (0 1 0 0 0 0 1 0.3469 1 0 0 0.4982 0 0 0 1) // dh-leftArm.csv link_0 (offset 0.0) (D 0.0) (A 0.0) (alpha -90.0) (mass 0.0) (cog 0 0 0) (inertia 0 0 0) link_1 (offset -90.0) (D 0.0) (A 0.0) (alpha -90.0) (mass 0.0) (cog 0 0 0) (inertia 0 0 0) link_2 (offset -90.0) (D -0.32901) (A 0.0) (alpha -90.0) (mass 1.750625) (cog 0 0 0) (inertia 0 0 0) link_3 (offset 0.0) (D 0.0) (A 0.0) (alpha 90.0) (mass 0.0) (cog 0 0 0) (inertia 0 0 0) link_4 (offset 0.0) (D -0.215) (A 0.0) (alpha -90.0) (mass 2.396) (cog 0 0 0) (inertia 0 0 0) link_5 (offset -90.0) (D 0.0) (A -0.09) (alpha 0.0) (mass 0.3) (cog 0 0 0) (inertia 0 0 0) // dh-fetch.csv + TCP merger HN (-1 0 0 -0.0975 0 0 1 0 0 1 0 -0.34692 0 0 0 1) [chain rightArm] hook "root" endpoint true mergeWith "leftArm" /// NEW numLinks 6 // dh-root-rightArm.csv H0 (0 1 0 0 0 0 1 -0.3469 1 0 0 0.4982 0 0 0 1) // dh-rightArm.csv link_0 (offset 0.0) (D 0.0) (A 0.0) (alpha -90.0) (mass 0.0) (cog 0 0 0) (inertia 0 0 0) link_1 (offset -90.0) (D 0.0) (A 0.0) (alpha -90.0) (mass 0.0) (cog 0 0 0) (inertia 0 0 0) link_2 (offset -90.0) (D -0.32901) (A 0.0) (alpha -90.0) (mass 1.750625) (cog 0 0 0) (inertia 0 0 0) link_3 (offset 0.0) (D 0.0) (A 0.0) (alpha 90.0) (mass 0.0) (cog 0 0 0) (inertia 0 0 0) link_4 (offset 0.0) (D -0.215) (A 0.0) (alpha -90.0) (mass 2.396) (cog 0 0 0) (inertia 0 0 0) link_5 (offset -90.0) (D 0.0) (A -0.09) (alpha 0.0) (mass 0.3) (cog 0 0 0) (inertia 0 0 0) // dh-fetch.csv + TCP merger HN (-1 0 0 -0.0975 0 0 1 0 0 1 0 0.34692 0 0 0 1) ```