PeterBowman
commented
4 years ago 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 (https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/115). See rationale at https://github.com/roboticslab-uc3m/yarp-devices/issues/228#issuecomment-532200863.
jgvictores
BasicCartesianControl integrates two distinct members related to the number of controlled joints:
https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/c187abdc0c9b2284d36df0f22cf64ecb238642b2/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp#L229
numRobotJointsis 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).numSolverJointsis 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 byKDL::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:
https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/c187abdc0c9b2284d36df0f22cf64ecb238642b2/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp#L244-L252
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):
https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/c187abdc0c9b2284d36df0f22cf64ecb238642b2/libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp#L297-L305
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), andwrenches[7](in the case described here) to a force exerted on HN (another fixed joint). It becomes apparent that the assignment towrenches[i]inside the loop (ref) should readwrenches[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:
https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/c187abdc0c9b2284d36df0f22cf64ecb238642b2/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp#L254
...and:
https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/c187abdc0c9b2284d36df0f22cf64ecb238642b2/libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp#L323
Assuming that
numRobotJointsequals to7(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, whilet[6]always spits out whatever resides at that location in memory.There are several places throughout the code where this is relevant.