Review joint sizes shared between controller and solver

[PeterBowman]

BasicCartesianControl integrates two distinct members related to the number of controlled joints:

kinematics-dynamics/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp

Line 229 in c187abd

int numRobotJoints, numSolverJoints;

• numRobotJoints is the number of joints as regarded by the joint controller (e.g. CanBusControlboard). On TEO, this is 7 (6 actuated rotary joints + 1 "fake" gripper joint).
• numSolverJoints is the number of joints as regarded by the cartesian solver (e.g. KdlSolver). On TEO, this is 6 (the number of non-fixed joints after the kinematic chain is configured).

In the KDL world, the latter is returned by KDL::Chain::getNrOfJoints(). However, it's also important to note that certain solvers work with the number of segments (links), as returned by KDL::Chain::getNrOfSegments().

Since most data travels between API calls inside a container of some sort (std::vector), and its size is not enforced in any way, hard-to-spot errors may arise.

• At the BasicCartesianControl level, a 7-element vector of external forces is instantiated and initialized here:

  kinematics-dynamics/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp

  Lines 244 to 252 in c187abd

  	std::vector< std::vector<double> > fexts;
  	for (int i = 0; i < numRobotJoints - 1; i++) //-- "numRobotJoints-1" is important
  	{
  	std::vector<double> fext(6, 0);
  	fexts.push_back(fext);
  	}
  	fexts.push_back(td);

  The first six force vectors (applied on each link) are null/empty, just the seventh one matters and it should be interpreted as a force exerted on the tool's TCP. Note that there are only six joints in the "standard" chain (gripper aside). KDL sees six joints, but eight links (kinematic chain prepended with H0, and HN appended thereafter). Inverse dynamics are performed on said vector (ref). However, the KDL implementation of ID requires to fill a vector of wrenches, built from the initial vector of external forces (esentially, their meaning is the same):

  kinematics-dynamics/libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp

  Lines 297 to 305 in c187abd

  	KDL::Wrenches wrenches(chain.getNrOfSegments(), KDL::Wrench::Zero());
  	for (int i = 0; i < fexts.size(); i++)
  	{
  	wrenches[i] = KDL::Wrench(
  	KDL::Vector(fexts[i][0], fexts[i][1], fexts[i][2]),
  	KDL::Vector(fexts[i][3], fexts[i][4], fexts[i][5])
  	);
  	}

  The weird part here is that idsolver.CartToJnt(...) (ref) expects a vector of eight external forces, that is, wrenches.size() == 8. As explained earlier, wrenches[0] should correspond to a force exerted on H0 (fixed joint), and wrenches[7] (in the case described here) to a force exerted on HN (another fixed joint). It becomes apparent that the assignment to wrenches[i] inside the loop (ref) should read wrenches[i+1] instead, size checks aside.

  Note: check how exactly KDL treats external forces. Is fexts[6] applied on the tip of the tool's TCP (probably), or perhaps on the COG of the last link?

• Compare:

  kinematics-dynamics/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp

  Line 254 in c187abd

  std::vector<double> t(numRobotJoints);

  ...and:

  kinematics-dynamics/libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp

  Line 323 in c187abd

  t.resize(chain.getNrOfJoints());

  Assuming that numRobotJoints equals to 7 (see previous point), t[6] now probably points to an unhandled (unassigned, perhaps used by another variable) memory block and trying to access it might result in undefined behavior. In fact, t.at(6) will throw an exception, while t[6] always spits out whatever resides at that location in memory.

  There are several places throughout the code where this is relevant.

[PeterBowman]

I plan to merge numRobotJoints and numSolverJoints into one entity that represents motor joints, only. That is, the (until and for now) only additional YARP-handled joint, which corresponds to the LacqueyFetch single-motor gripper, will be exposed via a dedicated YARP port to which BasicCartesianControl would have to connect via a separate remote_controlboard device in case it needs to access actuator commands (#115). See rationale at roboticslab-uc3m/yarp-devices#228 (comment).

[jgvictores]

Two comments related to the iCub:

1. Specifically on the iCub, I recall the hand joints are coupled to the arm. I know we do not have one, but maybe it is not a great idea to make our controller incompatible with its port layout.
2. In YARP's http://www.yarp.it/classyarp_1_1dev_1_1ICartesianControl.html#a425a689b2ab34bf2d13f1a17197a2ccc class, there is a differentiation between controlled joints and blocked joints (joints that are not controlled). Maybe this kind of mapping would be better?

[PeterBowman]

Ad 1:

Did we ever comply with iCub regarding joint or cartesian control? Are we compatible with iCub in the current state of this repo? If so, what should we keep in mind in order to keep being compliant, e.g. are there any written guidelines for that respect?

Ad 2:

We handle blocked (non-actuated) joints in the solver layer, every axis passing through the BCC is actuated (including gripper joints). In our case, the first N joints belong to a kinematic chain, whereas gripper joints always live at the end of this joint list. Doesn't seem the case for the method you link to. If you actually want to discuss randomly inserted blocked joints, in the current state of things, I think this should be treated at the joint controller level (yarp-devices repo).

[PeterBowman]

If you actually want to speak about randomly inserted blocked joints, in the current state of things, I think this should be discussed at the joint controller level (yarp-devices repo).

Just recalled #178.

[PeterBowman]

I plan to merge numRobotJoints and numSolverJoints into one entity that represents motor joints, only.

Done at 83b02a2.

[PeterBowman]

• The first six force vectors (applied on each link) are null/empty, just the seventh one matters and it should be interpreted as a force exerted on the tool's TCP. Note that there are only six joints in the "standard" chain (gripper aside). KDL sees six joints, but eight links (kinematic chain prepended with H0, and HN appended thereafter). Inverse dynamics are performed on said vector (ref).

Considering this, @jgvictores, would you mind if we 🔥 BREAK THE API 🔥 (just a little bit)? I'd suggest to replace

ICartesianSolver::invDyn(const std::vector<double> &q, const std::vector<double> &qdot, const std::vector<double> &qdotdot,
                         const std::vector<std::vector<double>> &fexts, std::vector<double> &t);

with

ICartesianSolver::invDyn(const std::vector<double> &q, const std::vector<double> &qdot, const std::vector<double> &qdotdot,
                         const std::vector<double> &ftip, std::vector<double> &t);

That is, expect const std::vector<double> &ftip instead of const std::vector<std::vector<double>> &fexts. While fexts is a vector of external wrenches applied on each segment/link, ftip would refer to its last element: external forces applied on the tip.

Why? Because we can't query the number of chain/tree segments as it is required by the current API. Currently, this is hardcoded as I explained in the first comment, and maybe even wrong or buggy. In case we load a kinematics model with more or less segments, the loop in PeriodicThreadImpl.cpp could segfault. Therefore:

• either we add ICartesianSolver::getNumSegments() (or getNumLinks) and use this information to instantiate fexts, or
• we modify the ICartesianSolver API and let it fill the vector of wrenches with zeros conveniently.

The latter alternative assumes that we won't ever want to provide non-zero values for external forces applied on other segments (besides the tip). Keep in mind the signature of ICartesianControl::forc doesn't allow to do so anyway (the input vector is ftip regardless of the underlying solver API).

[jgvictores]

Let's go for it!

[PeterBowman]

Important finding: the diagram represented in the wiki does not depict the actual behavior of KDL solvers accurately enough.

The Fcog is, in fact, referred to the tip frame of the segment. In other words, the "reference frame" stated in the documentation of RigidBodyInertia is the tip frame, not the local base/root frame.

Further read: orocos/orocos_kinematics_dynamics#170 and https://www.orocos.org/forum/orocos/orocos-users/problem-dynamic-model-and-inverse-dynamic-solver-kdl.

[PeterBowman]

Done at be7289c.

[PeterBowman]

The interpretation of external/exerted forces is a source of confusion.

• int KDL::ChainIdSolver::CartToJnt(const KDL::JntArray &q, const KDL::JntArray &q_dot, const KDL::JntArray &q_dotdot, const KDL::Wrenches &f_ext, KDL::JntArray &torques): here, f_ext is a vector of external wrenches applied on each segment

• kinematics-dynamics/libraries/YarpPlugins/ICartesianSolver.h

  Lines 202 to 203 in f9523fc

  	virtual bool invDyn(const std::vector<double> &q, const std::vector<double> &qdot, const std::vector<double> &qdotdot,
  	const std::vector<double> &ftip, std::vector<double> &t, reference_frame frame = BASE_FRAME) = 0;

  here, f_tip is a single wrench applied on the TCP

• kinematics-dynamics/libraries/YarpPlugins/ICartesianControl.h

  Line 263 in f9523fc

  virtual bool forc(const std::vector<double> &fd) = 0;

  here, fd is a single wrench exerted by the TCP

The last one requires that sign inversion is performed when passing data between controller and solver. This is done at f9523fc.