Abstract: Acceleration constrained hybrid dynamics calculations for a chain, based on Vereshchagin 1989. This class creates an instance of the hybrid dynamics solver. The solver analytically calculates the joint space constraint torques and acceleration in a chain when a constraint force(s) is applied to the chain's endeffector (task space / cartesian space). In the robotics literature, this algorithm is also known under the following names: Acceleration Constrained Hybrid Dynamics (ACHD) and PopovVereshchagin solver. More...
#include <chainhdsolver_vereshchagin.hpp>
Classes  
struct  segment_info 
Public Member Functions  
int  CartToJnt (const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, const JntArray &ff_torques, JntArray &constraint_torques) 
ChainHdSolver_Vereshchagin (const Chain &chain, const Twist &root_acc, const unsigned int nc)  
void  getContraintForceMagnitude (Eigen::VectorXd &nu_) 
void  getTotalTorque (JntArray &total_tau) 
void  getTransformedLinkAcceleration (Twists &x_dotdot) 
virtual void  updateInternalDataStructures () 
~ChainHdSolver_Vereshchagin ()  
Private Types  
typedef std::vector< Frame >  Frames 
typedef Eigen::Matrix< double, 6, 6 >  Matrix6d 
typedef Eigen::Matrix< double, 6, Eigen::Dynamic >  Matrix6Xd 
typedef std::vector< Twist >  Twists 
typedef Eigen::Matrix< double, 6, 1 >  Vector6d 
Private Types inherited from KDL::SolverI  
enum  { E_DEGRADED = +1, E_NOERROR = 0, E_NO_CONVERGE = 1, E_UNDEFINED = 2, E_NOT_UP_TO_DATE = 3, E_SIZE_MISMATCH = 4, E_MAX_ITERATIONS_EXCEEDED = 5, E_OUT_OF_RANGE = 6, E_NOT_IMPLEMENTED = 7, E_SVD_FAILED = 8 } 
Private Member Functions  
void  constraint_calculation (const JntArray &beta) 
void  downwards_sweep (const Jacobian &alfa, const JntArray &ff_torques) 
void  final_upwards_sweep (JntArray &q_dotdot, JntArray &constraint_torques) 
void  initial_upwards_sweep (const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext) 
Private Member Functions inherited from KDL::SolverI  
virtual int  getError () const 
Return the latest error. More...  
SolverI ()  
Initialize latest error to E_NOERROR. More...  
virtual const char *  strError (const int error) const 
virtual  ~SolverI () 
Private Attributes  
Twist  acc_root 
Jacobian  alfa_N 
Jacobian  alfa_N2 
JntArray  beta_N 
const Chain &  chain 
Frame  F_total 
Eigen::MatrixXd  M_0_inverse 
unsigned int  nc 
unsigned int  nj 
unsigned int  ns 
Eigen::VectorXd  nu 
Eigen::VectorXd  nu_sum 
Wrench  qdotdot_sum 
std::vector< segment_info, Eigen::aligned_allocator< segment_info > >  results 
Eigen::VectorXd  Sm 
Eigen::VectorXd  tmpm 
Eigen::VectorXd  total_torques 
Eigen::MatrixXd  Um 
Eigen::MatrixXd  Vm 
Private Attributes inherited from KDL::SolverI  
int  error 
Latest error, initialized to E_NOERROR in constructor. More...  
Abstract: Acceleration constrained hybrid dynamics calculations for a chain, based on Vereshchagin 1989. This class creates an instance of the hybrid dynamics solver. The solver analytically calculates the joint space constraint torques and acceleration in a chain when a constraint force(s) is applied to the chain's endeffector (task space / cartesian space). In the robotics literature, this algorithm is also known under the following names: Acceleration Constrained Hybrid Dynamics (ACHD) and PopovVereshchagin solver.
In 1970', researchers [1], [2] have developed a hybrid dynamics algorithm for evaluating robot behavior based on the input specification that is defined by the Cartesian acceleration constraints, feedforward joint torques and external Cartesian wrenches. The solver is derived from a wellknown principle of mechanics  Gauss' principle of least constraint [6] and provides an analytical (closedform) solution to the hybrid dynamics problem with lineartime, O(n) complexity [3].
In general, the Gauss' principle states that the true motion (acceleration) of a system/body is defined by the minimum of a quadratic function that is subject to linear geometric motion constraints [7], [6]. The result of this Gauss function represents the acceleration energy of a body, which is defined by the product of its mass and the squared distance between its allowed (constrained) acceleration and its free (unconstrained) acceleration [10], [11]. In the case of originally derived PopovVereshchagin algorithm [1], geometric motion constraints are Cartesian acceleration constraints imposed on the robot's endeffector. This domainspecific solver minimizes the acceleration energy by performing computational (outward and inward) sweeps along the robot's kinematic chain [3]. Furthermore, by computing the minimum of Gauss function, the PopovVereshchagin solver resolves the kinematic redundancy of the robot, when a partial motion (task) specification is provided [2]. A necessary condition that enables this type of closedform algorithm (an analytical solution to the abovedescribed optimization problem) defines that the robot’s kinematic chain does not consist of closed loops, i.e. the robot’s kinematic chain must be constructed in a serial or tree structure [11]. However, it is always possible to cut these loops and introduce explicit constrains.
For evaluating robot dynamics, i.e. resolving its constrained motion, the PopovVereshchagin solver is performing three computational sweeps (recursions), along the kinematic chain [3]. More specifically, two sweeps in outward and one sweep in inward direction. In the case of robot dynamics algorithms, the outward sweep refers to a recursion that is covering a kinematic chain from proximal to distal segments, while the inward sweep is covering a kinematic chain from distal to proximal segments [5]. Additionally, after completing the recursion in the second sweep and before starting the recursion in the last sweep, the solver is computing magnitudes of constraint forces, i.e. the Langrage multiplier (noted as nu in the KDL's solver implementation and original solver's publication[2]). More specifically, this operation is performed when the algorithm reaches segment (link) **{0}**, namely the base segment. In this formulation of the solver, the gravity effects are taken into account by setting the baselink's acceleration equal to gravitational acceleration [3].
For more detailed description of the algorithm and its representation, the reader can refer to [3], [5], [11].
For computing solutions to the constrained hybrid dynamics problem, this original derivation of the PopovVereshchagin solver [3] takes into account the following inputs:
The following outlines the abovelisted task interfaces in more detail.
This first type of motion driver can be used for specifying physical constraints such as contacts with environment [3], or artificial (i.e. taskimposed) constraints defined by the operational space task definition for the endeffector (tooltip) segment. Note: the Vereshchagin solver expects that the input Cartesian Acceleration Constraints, i.e. unit constraint forces in alpha parameters, are expressed w.r.t. robot's base frame. However, the acceleration energy setpoints, i.e. beta parameters, are expressed w.r.t. abovedefined unit constraint forces. More specifically, each DOF (element) in beta parameter corresponds to its respective DOF (column) of the unit constraint force matrix (alpha) [11].
To use this interface, a user should define i) the active constraint directions via alpha parameter, which is a 6 x m matrix of spatial unit constraint forces, and ii) acceleration energy setpoints via beta, which is a m x 1 vector. Here, the number of constraints m, or in another words number of spatial unit constraint forces is not required to always be equal to 6, which means that a human programmer can leave some of the degrees of freedom unspecified [2] for this motion driver, and still produce valid joint control commands [5]. For example, if we want to constrain the motion of the endeffector segment in only one direction, namely linear xdirection, we can define the constraint as [3]:
alpha =
1 
0 
0 
0 
0 
0 
beta =  0 
Note that here, the first three rows of matrix alpha represent linear elements and the last three rows represent angular elements, of the spatial unit force defined in Plücker coordinates [4]. By giving zero value to acceleration energy setpoint (beta), we are defining that the endeffector is not allowed to have linear acceleration in x direction. Or in other words, we are restricting the robot from producing any acceleration energy in that specified direction.
Another example includes the specification of constraints in 5 DOFs. We can constrain the motion of robot's endeffector such that it is only allowed to freely move in the linear zdirection, without performing linear motions in x and y and angular motions in x, y and z directions:
alpha =
1  0  0  0  0 
0  1  0  0  0 
0  0  0  0  0 
0  0  1  0  0 
0  0  0  1  0 
0  0  0  0  1 
beta =
0 
0 
0 
0 
0 
For both abovedescribed task examples, the Acceleration Constrained Hybrid Dynamics (ACHD) solver will compute valid control (constraint) joint torques, even though some of the Cartesian DOFs are left unspecified (e.g. in the case of the second example that would be endeffector's zdirection). More specifically: Underconstrained motion specifications are naturally resolved using Gauss' principle of least constraint
[5]. This means that in those directions in which the robot is not constrained by the task definition, its motions will be controlled by the nature. For instance, in the second example, natural resolution of the robot motion would define that the endeffector "**falls**" in the linear z direction due to effects of gravity, with the assumption that gravity forces are acting along $z$direction.
Moreover, the motion specification in the second example is equivalent to:
alpha =
1  0  0  0  0  0 
0  1  0  0  0  0 
0  0  0  0  0  0 
0  0  0  1  0  0 
0  0  0  0  1  0 
0  0  0  0  0  1 
(note that elements in the third column are all zeros, meaning zlinear constraint is deactivated)
beta =
0 
0 
0 
0 
0 
0 
The last example involves the full specification of the desired endeffector motion (in this case, not necessarily zero accelerations), i.e. specification of constraints in all 6 DOFs:
alpha =
1  0  0  0  0  0 
0  1  0  0  0  0 
0  0  1  0  0  0 
0  0  0  1  0  0 
0  0  0  0  1  0 
0  0  0  0  0  1 
beta = alpha^T * X_dotdot_N
Here, N stands for the index of the last robot's segment, endeffector (tooltip). The reader should note that we can directly assign values (magnitudes) of the desired (taskdefined) spatial acceleration 6 x 1 vector X_dotdot_N to the 6 x 1 vector of acceleration energy (beta) [3]. Even though physical dimensions (units) of these two vectors are not the same, the property of matrix alpha (it contains unit vectors), permits that we can assign values of desired accelerations to acceleration energy setpoints, in respective directions. Namely, each column of matrix alpha has the value of 1 in the respective direction in which constraint force works, thus it follows that the value of acceleration energy setpoint is the same as the value of Cartesian acceleration, in the respective direction.
This type of driver can be used for specifying physical (but not artificial, i.e. not taskintroduced) Cartesian wrenches acting on each of the robot's segments [11]. Examples for a physical force on a segment can be: i) a known weight at the robot's gripper, for instance, a grasped cup or ii) a force from a human pushing the robot [5]. Note that the implementation of Vereshchagin solver in KDL expects the provided f_ext is expressed w.r.t. robot's base frame, which is in contrast to the case of KDL's RNE solver.
This type of motion driver can be used for specifying physical (but not artificial, i.e. not taskintroduced) joint torques, for example, spring and/or damperbased torques (e.g. friction effects) in robot's joints [11].
Additional examples on using these input interfaces can be found in "../tests/solvertest.cpp":
This recursive dynamics solver is computing several quantities that represent solutions to both, inverse and forward dynamics problems, or in other words solutions to the constrained hybrid dynamics problem. More specifically, the output interface of the original PopovVereshchagin algorithm consists of [3], [5]:
Furthermore, if necessary, a complete spatial vector of imposed constraint forces can be computed [3], from the following relation: alpha * nu.
The reader should note that this constraint_torque is the necessary control command that a user is supposed to send to robot's joints, to achieve the motion that is computed (resolved) by the PopovVereshchagin solver. More specifically, here constraint_torque represent solution to the Inverse Dynamics (ID) problem. Nevertheless, the reason why a user is not supposed to use the total_torque values as the control commands for robot's joints, is the fact that the torque contributions that represent the difference between total_torque and constraint_torque already exist (act) on robot joints. More specifically, these additional (residual) contributions are produced on the joints by the already existing natural forces that act on the system [11].
On the other side, joint accelerations, namely q_dotdot provide solution to the Forward Dynamics (FD) problem and these quantities can be used for both control (integrate to joint positions/velocities) and simulation purposes [11].
The PopovVereshchagin hybrid dynamics solver enables a user to achieve many types of operational space tasks [11]. In other words, various controllers can be implemented around the aforementioned interfaces of the algorithm. Examples can be controllers for hybrid force/position control, impedance control, etc. However, there are some practical insights about this algorithm that need to taken into account.
The original derivation of this solver, which is considered in this library, prioritizes Cartesian acceleration constrains (specified for the endeffector segment) over other two motion drivers (Cartesian external wrenches and feedforward joint torques) [11]. In practice, this means the following:
Nevertheless, the abovedescribed prioritization can be changed (see [3] & [5] for more details) but those features are not implemented in KDL.
The reader should note that the PopovVereshchagin solver represents an extension to the wellknown forwarddynamics Articulated Body Algorithm (ABA) developed by Featherstone and described in [4] (moreover, Featherstone mentioned Vereshchagin solver in his book [4], page 117). This means that the PopovVereshchagin solver can also be purely used as this Articulated Body Algorithm forwarddynamics algorithm. In that case, it is necessary for the user to deactivate all Cartesian acceleration constraints (it is sufficient to set all elements in alpha matrix to zero) and proceed using other two interfaces as in the case of standard FD solver. More specifically, use f_ext input to define physical external wrenches acting on the robot's body (should be expressed w.r.t. robot's base frame) and ff_torque input to define command torques acting in robot's joints. The resulting robot's motion can be taken from q_dotdot and X_dotdot solver's outputs.
Nevertheless, the PopovVereshchagin solver can also be used for solving more advanced forward dynamics problems, than those solved by ABA [4]. More specifically, if this solver is used in a certain simulation environment for the usecase of simulating robot behaviors, all three interfaces can be exploited for defining a more descriptive robot's state. Here, a user can exploit the Cartesian acceleration constraint interface to specify different constraints imposed on the endeffector, along with other interfaces, and simulate what would be the robot's behavior due to these constraints and environmental impacts. Here, the resulting robot's motion can, as well, be taken from q_dotdot and X_dotdot solver's outputs. For example, MuJoCo framework (see MuJoCo's documentation) also uses Gauss' principle of least constraint to simulate constraint forces in certain situations, however, there, final derivation of this principle in the software is different. However, in the case of this more advanced forward dynamics computations, the user needs to be aware of prioritizations between input interfaces (mentioned in "Prioritizations" section above) and internal policies on handling singularities (mentioned in "Singularities and matrix inversions" section bellow).
To find the minimalenergy solution to the Inverse Dynamics (ID) problem, i.e. find the Langrage multiplier nu, the solver needs to compute the inverse of a socalled "acceleration constraint coupling matrix"[3] in the balance equation before starting the third sweep. However, the robot's configuration has a direct impact on this matrix and its inversion. Namely, if the robot is in a singular configuration for the task specified via acceleration constraints, this matrix will become rankdeficient. This means that it is not possible to find a feasible solution for that particular endeffector's DOF that is (or DOFs that are) lost due to the singular configuration. In other words, it is not possible to find constraint torques that will satisfy imposed acceleration constraints in that DOF/DOFs. Nevertheless, in this situation, it is still possible to find the (energyoptimal) solution for other "nonsingular" DOFs. For that reason, in KDL's implementation of the solver, matrix inverse is found by using the SVD technique to construct a pseudo inverse. Additionally, in this implementation, a control policy is introduced via the truncatedSVD method to deactivate, i.e. more specifically ignore, acceleration constraints for the DOFs that are lost due to robot's singular configuration. Of course, this is a choice (control policy), i.e. only one option for solving the singularity problem and producing safe joint commands. It is left for the user to explore other control policies (options) for this particular problem if of course, the user is not satisfied with the current control policy.
KDL's current implementation of the Vereshchagin HD solver supports only robot chains that have equal number of joints and segments. Moreover, this implementation can only compute dynamics for serial type of chains, i.e. currently, tree robot structures are not supported in this solver. Nevertheless, the original solver's derivation has been extended in [3] to account for multiple motion constraints imposed on a tree robot structure. This extension does not only account for acceleration constraints imposed on multiple endeffectors but also for acceleration constraints imposed on more proximal segments. However, the abovementioned extensions are currently not implemented in this version of KDL.
[1] E. P. Popov, A. F. Vereshchagin, and S. L. Zenkevich, "Manipulyatsionnye roboty: Dinamika i algoritmy", Nauka, Moscow, 1978.
[2] A. F. Vereshchagin, “Modelling and control of motion of manipulation robots”, Soviet Journal of Computer and Systems Sciences, vol. 27, pp. 29–38, 1989.
[3] A. Shakhimardanov, “Composable robot motion stack: Implementing constrained hybrid dynamics using semantic models of kinematic chains”, PhD thesis, KU Leuven, 2015.
[4] R. Featherstone, Rigid body dynamics algorithms. Springer, 2008.
[5] S. Schneider and H. Bruyninckx, “Exploiting linearity in dynamics solvers for the design of composable robotic manipulation architectures”, in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2019.
[6] H. Bruyninckx and O. Khatib, "Gauss’ principle and the dynamics of redundant and constrained manipulators", in IEEE International Conference on Robotics and Automation, 2000.
[7] C. F. Gauß, "Über ein neues allgemeines Grundgesetz der Mechanik.", Journal für die reine und angewandte Mathematik, vol. 4, pp. 232–235, 1829.
[8] A. F. Vereshchagin, “Computer simulation of the dynamics of complicated mechanisms of robotmanipulators”, Engineering Cybernetics, 12(6), pp. 65–70, 1974.
[9] E. P. Popov, "Control of robotsmanipulators", Engineering Cybernetics, 1974.
[10] E. Ramm, “Principles of least action and of least constraint”, GAMMMitteilungen, vol. 34, pp. 164–182, 2011.
[11] D. Vukcevic, "Lazy Robot Control by Relaxation of Motion and Force Constraints." Technical Report/Hochschule BonnRheinSieg University of Applied Sciences, Department of Computer Science, 2020.
Definition at line 366 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 369 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 371 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 372 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 368 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 370 of file chainhdsolver_vereshchagin.hpp.
KDL::ChainHdSolver_Vereshchagin::ChainHdSolver_Vereshchagin  (  const Chain &  chain, 
const Twist &  root_acc,  
const unsigned int  nc  
) 
Constructor for the solver, it will allocate all the necessary memory
chain  The kinematic chain to calculate the hybrid dynamics for. An internal copy will be made. 
root_acc  The acceleration twist of the root segment to use during the calculation (usually contains gravity). Note: This solver takes gravity acceleration with opposite sign comparead to the KDL's FD and RNE solvers 
nc  Number of constraints imposed on the robot's endeffector (maximum is 6). 
Definition at line 31 of file chainhdsolver_vereshchagin.cpp.

inline 
Definition at line 384 of file chainhdsolver_vereshchagin.hpp.
int KDL::ChainHdSolver_Vereshchagin::CartToJnt  (  const JntArray &  q, 
const JntArray &  q_dot,  
JntArray &  q_dotdot,  
const Jacobian &  alfa,  
const JntArray &  beta,  
const Wrenches &  f_ext,  
const JntArray &  ff_torques,  
JntArray &  constraint_torques  
) 
This method calculates joint space constraint torques and accelerations. It returns 0 when it succeeds, otherwise 1 or 2 for nonmatching matrix and array sizes. Input parameters:
q  The current joint positions 
q_dot  The current joint velocities 
alpha  The active constraint directions (unit constraint forces expressed w.r.t. robot's base frame) 
beta  The acceleration energy setpoints (expressed w.r.t. abovedefined unit constraint forces) 
f_ext  The external forces (no gravity, it is given in root acceleration) on the segments 
ff_torques  The feedforward joint space torques 
Output parameters:
q_dotdot  The resulting joint accelerations 
constraint_torques  The resulting joint constraint torques (what each joint feels due to the constraint forces acting on the endeffector) 
Definition at line 56 of file chainhdsolver_vereshchagin.cpp.

private 
This method calculates constraint force magnitudes.
Definition at line 264 of file chainhdsolver_vereshchagin.cpp.

private 
This method is a force balance sweep. It calculates articulated body inertias and bias forces. Additionally, acceleration energies generated by bias forces and unit forces are calculated here.
Additionally adding joint inertia to s.D, see:
Definition at line 138 of file chainhdsolver_vereshchagin.cpp.

private 
This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together.
Definition at line 299 of file chainhdsolver_vereshchagin.cpp.
void KDL::ChainHdSolver_Vereshchagin::getContraintForceMagnitude  (  Eigen::VectorXd &  nu_  ) 
Definition at line 368 of file chainhdsolver_vereshchagin.cpp.
void KDL::ChainHdSolver_Vereshchagin::getTotalTorque  (  JntArray &  total_tau  ) 
Definition at line 361 of file chainhdsolver_vereshchagin.cpp.
void KDL::ChainHdSolver_Vereshchagin::getTransformedLinkAcceleration  (  Twists &  x_dotdot  ) 
Definition at line 352 of file chainhdsolver_vereshchagin.cpp.

private 
This method calculates all cartesian space poses, twists, bias accelerations. External forces are also taken into account in this outward sweep.
Definition at line 77 of file chainhdsolver_vereshchagin.cpp.

virtual 
Update the internal data structures. This is required if the number of segments or number of joints of a chain/tree have changed. This provides a single point of contact for solver memory allocations.
Implements KDL::SolverI.
Definition at line 49 of file chainhdsolver_vereshchagin.cpp.

private 
Definition at line 470 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 471 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 472 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 476 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 466 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 483 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 473 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 469 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 467 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 468 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 477 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 478 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 482 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 530 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 479 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 480 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 481 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 474 of file chainhdsolver_vereshchagin.hpp.

private 
Definition at line 475 of file chainhdsolver_vereshchagin.hpp.