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>

Inheritance diagram for KDL::ChainHdSolver_Vereshchagin:

[legend]

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...

Detailed Description

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	1	0	0	0
0	0	0	0	0
0	0	1	0	0
0	0	0	1	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.

Definition at line 366 of file chainhdsolver_vereshchagin.hpp.

Member Typedef Documentation

◆ Frames

typedef std::vector<Frame> KDL::ChainHdSolver_Vereshchagin::Frames

private

Definition at line 369 of file chainhdsolver_vereshchagin.hpp.

◆ Matrix6d

typedef Eigen::Matrix<double, 6, 6 > KDL::ChainHdSolver_Vereshchagin::Matrix6d

private

Definition at line 371 of file chainhdsolver_vereshchagin.hpp.

◆ Matrix6Xd

typedef Eigen::Matrix<double, 6, Eigen::Dynamic> KDL::ChainHdSolver_Vereshchagin::Matrix6Xd

private

Definition at line 372 of file chainhdsolver_vereshchagin.hpp.

◆ Twists

typedef std::vector<Twist> KDL::ChainHdSolver_Vereshchagin::Twists

private

Definition at line 368 of file chainhdsolver_vereshchagin.hpp.

◆ Vector6d

typedef Eigen::Matrix<double, 6, 1 > KDL::ChainHdSolver_Vereshchagin::Vector6d

private

Definition at line 370 of file chainhdsolver_vereshchagin.hpp.

Constructor & Destructor Documentation

◆ ChainHdSolver_Vereshchagin()

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

Parameters

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 31 of file chainhdsolver_vereshchagin.cpp.

◆ ~ChainHdSolver_Vereshchagin()

KDL::ChainHdSolver_Vereshchagin::~ChainHdSolver_Vereshchagin ( )

inline

Definition at line 384 of file chainhdsolver_vereshchagin.hpp.

Member Function Documentation

◆ CartToJnt()

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:

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:

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)

Returns: error/success code

Definition at line 56 of file chainhdsolver_vereshchagin.cpp.

◆ constraint_calculation()

void KDL::ChainHdSolver_Vereshchagin::constraint_calculation ( const JntArray & beta )

private

This method calculates constraint force magnitudes.

Definition at line 264 of file chainhdsolver_vereshchagin.cpp.

◆ downwards_sweep()

void KDL::ChainHdSolver_Vereshchagin::downwards_sweep	(	const Jacobian &	alfa,
		const JntArray &	ff_torques
	)

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:

equation a) in Vereshchagin89
equation 9.28, page 188, Featherstone book 2008

Definition at line 138 of file chainhdsolver_vereshchagin.cpp.

◆ final_upwards_sweep()

void KDL::ChainHdSolver_Vereshchagin::final_upwards_sweep	(	JntArray &	q_dotdot,
		JntArray &	constraint_torques
	)

private

This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together.

Definition at line 299 of file chainhdsolver_vereshchagin.cpp.

◆ getContraintForceMagnitude()

void KDL::ChainHdSolver_Vereshchagin::getContraintForceMagnitude ( Eigen::VectorXd & nu_ )

Definition at line 368 of file chainhdsolver_vereshchagin.cpp.

◆ getTotalTorque()

void KDL::ChainHdSolver_Vereshchagin::getTotalTorque ( JntArray & total_tau )

Definition at line 361 of file chainhdsolver_vereshchagin.cpp.

◆ getTransformedLinkAcceleration()

void KDL::ChainHdSolver_Vereshchagin::getTransformedLinkAcceleration ( Twists & x_dotdot )

Definition at line 352 of file chainhdsolver_vereshchagin.cpp.

◆ initial_upwards_sweep()

void KDL::ChainHdSolver_Vereshchagin::initial_upwards_sweep	(	const JntArray &	q,
		const JntArray &	q_dot,
		const JntArray &	q_dotdot,
		const Wrenches &	f_ext
	)

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.

◆ updateInternalDataStructures()

void KDL::ChainHdSolver_Vereshchagin::updateInternalDataStructures ( )

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.