meswork/smart_wasm
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
zhangshun
2026-06-01 15:55:59 +08:00
commit 40f9bdb590
1799 changed files with 362227 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

535
KDL/install/include/kdl/chainhdsolver_vereshchagin.hpp Normal file
View File

@@ -0,0 +1,535 @@
// Copyright (C) 2009 Ruben Smits <ruben dot smits at intermodalics dot eu>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
// Author: Herman Bruyninckx
// Author: Azamat Shakhimardanov
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// 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<Eigen/StdVector>
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 bellow).
*
* ### 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<Twist> Twists;
typedef std::vector<Frame> Frames;
typedef Eigen::Matrix<double, 6, 1 > Vector6d;
typedef Eigen::Matrix<double, 6, 6 > Matrix6d;
typedef Eigen::Matrix<double, 6, Eigen::Dynamic> 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<segment_info, Eigen::aligned_allocator<segment_info> > results;
};
}
#endif // KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP