> > - Adds functionality for coupled joints
>
> Which functionality exactly? Do you have some written documentation or
> references about this?

No documentation, but the concept is simple. When I talk about a coupled joint I refer to a joint that its speed is related by a constant to another joint speed. This can be extended to all the joints of the chain, and multiple relationships can be defined with a matrix.

An example of such matrix would be:

C= [ 1.0, 0.0, 0.0]
[ 0.0, 1.0, 0.0]
[ 0.0, 0.0, 1.0]
[ 0.0, 0.0, 1.0]

Vjoints=C*Vind_joints

Vind_joints: speed of independent joints.
Vjoints: speed of all joints.

This is our particular example the DLR-HIT finger. We can only control three values, but four joints move. The last two joints move equally fast (and therefore with the same angle).

The jacobian calculation in chainjnttojacsolver haven't changed at all except that before returning the jacobian I right multiply the jacobian by the coupling matrix to get a reduce jacobian. Or put another way, since the jacobian of the whole chain (including coupled joints) relates the task space speed to joint speeds, a transformation can be applied at the right side to get a relation of task speeds to non-coupled joint speeds (Vind_joints). This is important because when the inverse velocity kinematics will solve for joint speeds, it needs to know that the coupled joint is not indepedent. One way, I thought, to do this is to reduce the size of the jacobian so that the vik solver thinks the number of joints is reduced, then the vik solver will give a joint velocity vector corresponding to the Vind_joints. Since Vind_joints is what can be controlled on the manipulator the solution is ready to be used on the robot. (Optionally one could multiply Vind_joints by C to obtain al
l the joint speeds, in our example this will mean that the last two joints will always have the same speed)

So the only change to the jacobian solver is to right-multiply the jacobian by the coupling matrix and return this as the new jacobian.
This is optional by an argument (bool coupled), and will return to the old behavior when this argument is false.

Chainiksolvervel_wdls calculates the resulting speeds using the reduced jacobian and multiplies them with C to obtain all the joint speeds, this is what is returned. This behavior could be changed (by changing the code) to return only the non-coupled speeds.

> > - Adds more access to the locked joints (previous functionality).
>
> What is "more access", and why is it needed?

Well, only public access through a method in the jacobian solver to the locked joints, and also adding the locked joints datastructure not only to the jacobian solver but also to the chain. This is because I decided to put the coupling matrix as part of the chain (from a object oriented programming point of view looked more appropiate to me, the coupling matrix is very related to the manipulator kinematic chain), I considered that the locked joints should also be part of the chain, and then I can check that when I set the coupling matrix it has the proper size (comparing to locked joints size and the rows of the coupling matrix). There are more reasons about this decision I can mention later in another email. (Basically I make use of the locked joints to calculate torque/forces with torque sensors that are in the middle of the links by making a bigger chain that includes the sensors as locked joints)

Actually my opinion is that the locked joints should be part of the chain. (Is more a characteristic of the chain than of a particular solver, the solver should decide if it gets used or not). The jacobian needs to know the quantity of independent joints, and this now depends not only of the locked joints but also of the coupled matrix rank, and this is accessed from the chain. Also the vik solver takes this information from the chain (including the coupling matrix).

> > - Adds the corresponding python bindings
> > - The joint scaling is now accessible.
> >
> > The chain class is modified to include the number of independent joints
> > and the number of unlockedjoints. This is used to generate a Jacobian
> > only for the independent joints.
>
> There is a trade-off between two approaches, I think:
> - one keeps the current solver, but you give it a new chain that has new
> segments for each locked joint
> - one makes a new solver, as you do, that goes over the total chain but
> neglects the locked joints
> The latter has some consequences:
> - an extra runtime cost
> - an extra bookkeeping complexity
> - and, last but not least, an extra solver to maintain.
> Could you provide strong motivation why it's a good idea to pay the price
> of your approach?

I didn't really change the solver almost at all, the only thing that changes in the jacobian solver is the right multiplication by the coupling matrix (which only happens if bool coupled is True) and the left multiplication with the coupling matrix in the vik solver (this could be also optional). So this extra runtime can be avoided by not having coupling matrix (identity) and setting coupled to false when calculating the jacobian. Maybe one or two more ifs will be the extra load. The bookkeeping complexity could be avoided by completely moving the locked joints to the chain and removing it from the jacobian solver, but I prefered to keep this as before to not break other unexpected things (it's possible that, with current code, some solvers expect the locked joints in the jacobian solver and not the chain). For more in this look at the code.

> > Chainjnttojacsolver is also modified to take into account locked joints
> > and coupled joints.
>
> If I understand correctly, a "locked joint" basically means the joint is
> always motionless, and results in two segments that are acting now as one
> rigid segment?

The part of the locked joints in there was a false statement on my part. To correct myself: this is functionality previously included in kdl. I only included access to the locked_joints data structure and also move it to chain instead of only in the jacobian solver. And yes, that's how I used the locked joints. I used them for the trick with the torque sensors on the finger, because they are not located in the same joints that they advertised, but rather somewhere else in the contiguous link. I consider the sensors "joints" but locked in the new bigger chain. In this way I can compute torque/force transformations (to the finger tip for example) taking into account the whole kinematic chain state.

Federico Ruiz.
IAS. TU-Muenchen.