// Copyright (C) 2009 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Author: Herman Bruyninckx // Author: Azamat Shakhimardanov // Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or // modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation; either // version 2.1 of the License, or (at your option) any later version. // This library is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // Lesser General Public License for more details. // You should have received a copy of the GNU Lesser General Public // License along with this library; if not, write to the Free Software // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA #ifndef KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP #define KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP #include "chainidsolver.hpp" #include "frames.hpp" #include "articulatedbodyinertia.hpp" #include namespace KDL { /** * \brief **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. * * ## INTRODUCTION * * 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]. * * ## INTERFACES * * ### Solver's Input * * For computing solutions to the constrained hybrid dynamics problem, this original derivation of * the Popov-Vereshchagin solver [3] takes into account the following inputs: * * * Robot's **model** defined by: kinematic parameters of the chain, segments' mass and rigid-body * inertia, and effective inertia of each joint rotor -> **chain** parameter in solver's constructor * * * **Root** acceleration of the robot's base segment (usually gravitational) -> **root_acc** parameter * in solver's constructor * * * Current joint configuration (angles) -> **q** parameter in the **CartToJnt** function * * * Current joint velocities -> **q_dot** parameter in the **CartToJnt** function * * * Motion drivers: * * - **Cartesian Acceleration Constraints** imposed on the end-effector segment -> **alpha** and * **beta** parameters in the **CartToJnt** function * * - **Cartesian External Wrench** acting on each segment -> **f_ext** parameter in the **CartToJnt** function * * - **Feed-Forward Torque** acting on each joint -> **ff_torques** parameter in the **CartToJnt** function * * The following outlines the above-listed task interfaces in more detail. * * #### Cartesian Acceleration Constraints: alpha & beta * * 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. * * #### External Forces: f_ext * * 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. * * ### Feed-Forward Joint Torques: ff_torque * * 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": * * * VereshchaginTest() function - an example on how to use all interfaces of this solver for computing * the solution to the Hybrid Dynamics (HD) problem. * * * FdAndVereshchaginSolversConsistencyTest() function - an example on how to only use this solver for * computing the solution to the Featherstone's (i.e. Articulated Body Algorithm (ABA)) version of * the Forward Dynamics (FD) problem. * * ### Solver's Output * * 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]: * * * Magnitudes of constraint forces that act on the end-effector, denoted by the Lagrange multiplier **nu** * in the solver's implementation and original solver's publication[2]. * * * Joint constraint torques required for achieving the desired (acceleration-constraints-defined) behavior of * the robot: **constraint_torque**. These torques represent control commands that should be sent to robot's joint drivers. * * * Argument that defines the solution to the originally formulated optimization problem in **Gauss' principle**. * In other words, the joint accelerations **q_dotdot** resulting from the total torque acting on each joint (**total_torque**), * i.e. from the aforementioned constraint torques and all natural and external forces acting on the system. * * * The resulting and complete spatial accelerations of each segment in the kinematic chain: **X_dotdot** * * 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]. * * ## PRACTICAL INSIGHTS/CONSIDERATIONS * * 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. * * ### Prioritizations between motion drivers (interfaces) * * 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: * * * If the external wrenches and/or feedforward joint torques contribute positively (i.e. assist) in producing Cartesian accelerations * (specified via acceleration constraint interface), the Vereshchagin solver will take advantage of these forces to compute * (acceleration-) energy optimal motions. * * * On the other hand, if the aforementioned external wrenches and/or feedforward joint torques contribute negatively (i.e. interfere) * the Cartesian accelerations, the Vereshchagin solver will compensate all of those forces to correctly produce constrained * accelerations of the end-effector. More specifically, additional torque commands will be computed under * **constrained joint torques** (**ctrl_torques** in this implementation), to overcome those "disturbances". * * Nevertheless, the above-described prioritization can be changed (see [3] & [5] for more details) but those features are not implemented in KDL. * * ### Using the algorithm for solving forward dynamics (FD) problem * * 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 below). * * ### Singularities and matrix inversions * * 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. * * ### Supported robot models * * 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. * * ## REFERENCES * * [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. * * @ingroup KinematicFamily */ class ChainHdSolver_Vereshchagin : KDL::SolverI { typedef std::vector Twists; typedef std::vector Frames; typedef Eigen::Matrix Vector6d; typedef Eigen::Matrix Matrix6d; typedef Eigen::Matrix Matrix6Xd; public: /** * Constructor for the solver, it will allocate all the necessary memory * \param chain The kinematic chain to calculate the hybrid dynamics for. An internal copy will be made. * \param 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 * \param nc Number of constraints imposed on the robot's end-effector (maximum is 6). */ ChainHdSolver_Vereshchagin(const Chain& chain, const Twist &root_acc, const unsigned int nc); ~ChainHdSolver_Vereshchagin() { }; /** * 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: * \param q The current joint positions * \param q_dot The current joint velocities * \param alpha The active constraint directions (unit constraint forces expressed w.r.t. robot's base frame) * \param beta The acceleration energy setpoints (expressed w.r.t. above-defined unit constraint forces) * \param f_ext The external forces (no gravity, it is given in root acceleration) on the segments * \param ff_torques The feed-forward joint space torques * * Output parameters: * \param q_dotdot The resulting joint accelerations * \param constraint_torques The resulting joint constraint torques (what each joint feels due to the constraint forces acting on the end-effector) * * @return error/success code */ 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); /// @copydoc KDL::SolverI::updateInternalDataStructures virtual void updateInternalDataStructures(); //Returns cartesian acceleration of links in base coordinates void getTransformedLinkAcceleration(Twists& x_dotdot); // Returns total torque acting on each joint (constraints + nature + external forces) void getTotalTorque(JntArray &total_tau); // Returns magnitude of the constraint forces acting on the end-effector: Lagrange Multiplier void getContraintForceMagnitude(Eigen::VectorXd &nu_); /* //Returns cartesian positions of links in base coordinates void getLinkCartesianPose(Frames& x_base); //Returns cartesian velocities of links in base coordinates void getLinkCartesianVelocity(Twists& xDot_base); //Returns cartesian acceleration of links in base coordinates void getLinkCartesianAcceleration(Twists& xDotDot_base); //Returns cartesian positions of links in link tip coordinates void getLinkPose(Frames& x_local); //Returns cartesian velocities of links in link tip coordinates void getLinkVelocity(Twists& xDot_local); //Returns cartesian acceleration of links in link tip coordinates void getLinkAcceleration(Twists& xDotdot_local); //Acceleration energy due to unit constraint forces at the end-effector void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M); //Acceleration energy due to arm configuration: bias force plus input joint torques void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G); void getLinkUnitForceMatrix(Matrix6Xd& E_tilde); void getLinkBiasForceMatrix(Wrenches& R_tilde); void getJointBiasAcceleration(JntArray &bias_q_dotdot); */ private: /** * This method calculates all cartesian space poses, twists, bias accelerations. * External forces are also taken into account in this outward sweep. */ void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext); /** * 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. */ void downwards_sweep(const Jacobian& alfa, const JntArray& ff_torques); /** * This method calculates constraint force magnitudes. * */ void constraint_calculation(const JntArray& beta); /** * This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together. * */ void final_upwards_sweep(JntArray &q_dotdot, JntArray &constraint_torques); private: const Chain& chain; unsigned int nj; unsigned int ns; unsigned int nc; Twist acc_root; Jacobian alfa_N; Jacobian alfa_N2; Eigen::MatrixXd M_0_inverse; Eigen::MatrixXd Um; Eigen::MatrixXd Vm; JntArray beta_N; Eigen::VectorXd nu; Eigen::VectorXd nu_sum; Eigen::VectorXd Sm; Eigen::VectorXd tmpm; Eigen::VectorXd total_torques; // all the contributions that are felt at the joint: constraints + nature + external forces Wrench qdotdot_sum; Frame F_total; struct segment_info { Frame F; //local pose with respect to previous link in segments coordinates Frame F_base; // pose of a segment in root coordinates Twist Z; //Unit twist Twist v; //twist Twist acc; //acceleration twist Wrench U; //wrench p of the bias forces (in cartesian space) Wrench R; //wrench p of the bias forces Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form Twist C; //constraint Twist A; //constraint ArticulatedBodyInertia H; //I (expressed in 6*6 matrix) ArticulatedBodyInertia P; //I (expressed in 6*6 matrix) ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix) Wrench PZ; //vector U[i] = I_A[i]*S[i] Wrench PC; //vector E[i] = I_A[i]*c[i] double D; //vector D[i] = S[i]^T*U[i] Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints Matrix6Xd E_tilde; Eigen::MatrixXd M; //acceleration energy already generated at link i Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i Eigen::VectorXd EZ; //K[i] = Etiltde'*Z double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace double u; //vector u[i] = torques(i) - S[i]^T*(p_A[i] + I_A[i]*C[i]) in joint subspace. Azamat: In code u[i] = torques(i) - s[i].totalBias segment_info(unsigned int nc): D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0) { E.resize(6, nc); E_tilde.resize(6, nc); G.resize(nc); M.resize(nc, nc); EZ.resize(nc); E.setZero(); E_tilde.setZero(); M.setZero(); G.setZero(); EZ.setZero(); }; }; std::vector > results; }; } #endif // KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP