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 end-effector (task space / cartesian space). In the robotics literature, this algorithm is also known under the following names: Acceleration Constrained Hybrid Dynamics (ACHD) and Popov-Vereshchagin 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 end-effector (task space / cartesian space). In the robotics literature, this algorithm is also known under the following names: Acceleration Constrained Hybrid Dynamics (ACHD) and Popov-Vereshchagin 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, feed-forward joint torques and external Cartesian wrenches. The solver is derived from a well-known principle of mechanics - Gauss' principle of least constraint [6] and provides an analytical (closed-form) solution to the hybrid dynamics problem with linear-time, 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 Popov-Vereshchagin algorithm [1], geometric motion constraints are Cartesian acceleration constraints imposed on the robot's end-effector. This domain-specific 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 Popov-Vereshchagin 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 closed-form algorithm (an analytical solution to the above-described 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 Popov-Vereshchagin 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 base-link'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 Popov-Vereshchagin solver [3] takes into account the following inputs:
The following outlines the above-listed 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. task-imposed) constraints defined by the operational space task definition for the end-effector (tool-tip) 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. above-defined 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 end-effector segment in only one direction, namely linear x-direction, 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 end-effector 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 end-effector such that it is only allowed to freely move in the linear z-direction, 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 above-described 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 end-effector's z-direction). 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 end-effector "**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 z-linear constraint is deactivated)
beta =
0 |
0 |
0 |
0 |
0 |
0 |
The last example involves the full specification of the desired end-effector 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, end-effector (tool-tip). The reader should note that we can directly assign values (magnitudes) of the desired (task-defined) 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 task-introduced) 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 task-introduced) joint torques, for example, spring and/or damper-based 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 Popov-Vereshchagin 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 Popov-Vereshchagin 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 Popov-Vereshchagin 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 end-effector segment) over other two motion drivers (Cartesian external wrenches and feedforward joint torques) [11]. In practice, this means the following:
Nevertheless, the above-described prioritization can be changed (see [3] & [5] for more details) but those features are not implemented in KDL.
The reader should note that the Popov-Vereshchagin solver represents an extension to the well-known forward-dynamics 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 Popov-Vereshchagin solver can also be purely used as this Articulated Body Algorithm forward-dynamics 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 Popov-Vereshchagin 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 use-case 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 end-effector, 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 minimal-energy solution to the Inverse Dynamics (ID) problem, i.e. find the Langrage multiplier nu, the solver needs to compute the inverse of a so-called "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 rank-deficient. This means that it is not possible to find a feasible solution for that particular end-effector'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 (energy-optimal) solution for other "non-singular" 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 truncated-SVD 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 end-effectors but also for acceleration constraints imposed on more proximal segments. However, the above-mentioned 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 robot-manipulators”, Engineering Cybernetics, 12(6), pp. 65–70, 1974.
[9] E. P. Popov, "Control of robots-manipulators", Engineering Cybernetics, 1974.
[10] E. Ramm, “Principles of least action and of least constraint”, GAMM-Mitteilungen, vol. 34, pp. 164–182, 2011.
[11] D. Vukcevic, "Lazy Robot Control by Relaxation of Motion and Force Constraints." Technical Report/Hochschule Bonn-Rhein-Sieg 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 end-effector (maximum is 6). |
Definition at line 32 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. above-defined unit constraint forces) |
f_ext | The external forces (no gravity, it is given in root acceleration) on the segments |
ff_torques | The feed-forward 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 end-effector) |
Definition at line 57 of file chainhdsolver_vereshchagin.cpp.
|
private |
This method calculates constraint force magnitudes.
Definition at line 265 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 139 of file chainhdsolver_vereshchagin.cpp.
|
private |
This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together.
Definition at line 300 of file chainhdsolver_vereshchagin.cpp.
void KDL::ChainHdSolver_Vereshchagin::getContraintForceMagnitude | ( | Eigen::VectorXd & | nu_ | ) |
Definition at line 369 of file chainhdsolver_vereshchagin.cpp.
void KDL::ChainHdSolver_Vereshchagin::getTotalTorque | ( | JntArray & | total_tau | ) |
Definition at line 362 of file chainhdsolver_vereshchagin.cpp.
void KDL::ChainHdSolver_Vereshchagin::getTransformedLinkAcceleration | ( | Twists & | x_dotdot | ) |
Definition at line 353 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 78 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 50 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.