937 lines
31 KiB
C++
937 lines
31 KiB
C++
/*
|
|
* RBDL - Rigid Body Dynamics Library
|
|
* Copyright (c) 2011-2015 Martin Felis <martin@fysx.org>
|
|
*
|
|
* Licensed under the zlib license. See LICENSE for more details.
|
|
*/
|
|
|
|
#ifndef RBDL_CONSTRAINTS_H
|
|
#define RBDL_CONSTRAINTS_H
|
|
|
|
#include <rbdl/rbdl_math.h>
|
|
#include <rbdl/rbdl_mathutils.h>
|
|
|
|
namespace RigidBodyDynamics {
|
|
|
|
/** \page contacts_page External Constraints
|
|
*
|
|
* All functions related to contacts are specified in the \ref
|
|
* constraints_group "Constraints Module".
|
|
|
|
* \defgroup constraints_group Constraints
|
|
*
|
|
* Constraints are handled by specification of a \link
|
|
* RigidBodyDynamics::ConstraintSet
|
|
* ConstraintSet \endlink which contains all informations about the
|
|
* current constraints and workspace memory.
|
|
*
|
|
* Separate constraints can be specified by calling
|
|
* ConstraintSet::AddContactConstraint() and ConstraintSet::AddLoopConstraint().
|
|
* After all constraints have been specified, this \link
|
|
* RigidBodyDynamics::ConstraintSet
|
|
* ConstraintSet \endlink has to be bound to the model via
|
|
* ConstraintSet::Bind(). This initializes workspace memory that is
|
|
* later used when calling one of the contact functions, such as
|
|
* ForwardDynamicsContactsDirects().
|
|
*
|
|
* The values in the vectors ConstraintSet::force and
|
|
* ConstraintSet::impulse contain the computed force or
|
|
* impulse values for each constraint when returning from one of the
|
|
* contact functions.
|
|
*
|
|
* \section solution_constraint_system Solution of the Constraint System
|
|
*
|
|
* \subsection constraint_system Linear System of the Constrained Dynamics
|
|
*
|
|
* In the presence of constraints, to compute the
|
|
* acceleration one has to solve a linear system of the form: \f[
|
|
\left(
|
|
\begin{array}{cc}
|
|
H & G^T \\
|
|
G & 0
|
|
\end{array}
|
|
\right)
|
|
\left(
|
|
\begin{array}{c}
|
|
\ddot{q} \\
|
|
- \lambda
|
|
\end{array}
|
|
\right)
|
|
=
|
|
\left(
|
|
\begin{array}{c}
|
|
-C + \tau \\
|
|
\gamma
|
|
\end{array}
|
|
\right)
|
|
* \f] where \f$H\f$ is the joint space inertia matrix computed with the
|
|
* CompositeRigidBodyAlgorithm(), \f$G\f$ is the constraint jacobian,
|
|
* \f$C\f$ the bias force (sometimes called "non-linear
|
|
* effects"), and \f$\gamma\f$ the generalized acceleration independent
|
|
* part of the constraints.
|
|
*
|
|
* \subsection collision_system Linear System of the Contact Collision
|
|
*
|
|
* Similarly to compute the response of the model to a contact gain one has
|
|
* to solve a system of the following form: \f[
|
|
\left(
|
|
\begin{array}{cc}
|
|
H & G^T \\
|
|
G & 0
|
|
\end{array}
|
|
\right)
|
|
\left(
|
|
\begin{array}{c}
|
|
\dot{q}^{+} \\
|
|
\Lambda
|
|
\end{array}
|
|
\right)
|
|
=
|
|
\left(
|
|
\begin{array}{c}
|
|
H \dot{q}^{-} \\
|
|
v^{+}
|
|
\end{array}
|
|
\right)
|
|
* \f] where \f$H\f$ is the joint space inertia matrix computed with the
|
|
* CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the
|
|
* contact points, \f$\dot{q}^{+}\f$ the generalized velocity after the
|
|
* impact, \f$\Lambda\f$ the impulses at each constraint, \f$\dot{q}^{-}\f$
|
|
* the generalized velocity before the impact, and \f$v^{+}\f$ the desired
|
|
* velocity of each constraint after the impact (known beforehand, usually
|
|
* 0). The value of \f$v^{+}\f$ can is specified via the variable
|
|
* ConstraintSet::v_plus and defaults to 0.
|
|
*
|
|
* \subsection solution_methods Solution Methods
|
|
*
|
|
* There are essentially three different approaches to solve these systems:
|
|
* -# \b Direct: solve the full system to simultaneously compute
|
|
* \f$\ddot{q}\f$ and \f$\lambda\f$. This may be slow for large systems
|
|
* and many constraints.
|
|
* -# \b Range-Space: solve first for \f$\lambda\f$ and then for
|
|
* \f$\ddot{q}\f$.
|
|
* -# \b Null-Space: solve furst for \f$\ddot{q}\f$ and then for
|
|
* \f$\lambda\f$
|
|
* The methods are the same for the contact gaints just with different
|
|
* variables on the right-hand-side.
|
|
*
|
|
* RBDL provides methods for all approaches. The implementation for the
|
|
* range-space method also exploits sparsities in the joint space inertia
|
|
* matrix using a sparse structure preserving \f$L^TL\f$ decomposition as
|
|
* described in Chapter 8.5 of "Rigid Body Dynamics Algorithms".
|
|
*
|
|
* None of the methods is generally superior to the others and each has
|
|
* different trade-offs as factors such as model topology, number of
|
|
* constraints, constrained bodies, numerical stability, and performance
|
|
* vary and evaluation has to be made on a case-by-case basis.
|
|
*
|
|
* \subsection solving_constraints_dynamics Methods for Solving Constrained
|
|
* Dynamics
|
|
*
|
|
* RBDL provides the following methods to compute the acceleration of a
|
|
* constrained system:
|
|
*
|
|
* - ForwardDynamicsConstraintsDirect()
|
|
* - ForwardDynamicsConstraintsRangeSpaceSparse()
|
|
* - ForwardDynamicsConstraintsNullSpace()
|
|
*
|
|
* \subsection solving_constraints_collisions Methods for Computing Collisions
|
|
*
|
|
* RBDL provides the following methods to compute the collision response on
|
|
* new contact gains:
|
|
*
|
|
* - ComputeConstraintImpulsesDirect()
|
|
* - ComputeConstraintImpulsesRangeSpaceSparse()
|
|
* - ComputeConstraintImpulsesNullSpace()
|
|
*
|
|
* \subsection assembly_q_qdot Computing generalized joint positions and velocities satisfying the constraint equations
|
|
*
|
|
* When considering a model subject position level constraints expressed by the
|
|
* equation \f$\phi (q) = 0\f$, it is often necessary to comput generalized joint
|
|
* position and velocities which satisfy the constraints.
|
|
*
|
|
* In order to compute a vector of generalized joint positions that satisfy
|
|
* the constraints it is necessary to solve the following optimization problem:
|
|
* \f{eqnarray*}{
|
|
* \text{minimize} && \sum_{i = 0}^{n} (q - q_{0})^T W (q - q_{0}) \\
|
|
* \text{over} && q \\
|
|
* \text{subject to} && \phi (q) = 0
|
|
* \f}
|
|
*
|
|
* In order to compute a vector of generalized joint velocities that satisfy
|
|
* the constraints it is necessary to solve the following optimization problem:
|
|
* \f{eqnarray*}{
|
|
* \text{minimize} && \sum_{i = 0}^{n} (\dot{q} - \dot{q}_{0})^T W (\dot{q} - \dot{q}_{0}) \\
|
|
* \text{over} && \dot{q} \\
|
|
* \text{subject to} && \dot{\phi} (q) = \phi _{q}(q) \dot{q} + \phi _{t}(q) = 0
|
|
* \f}
|
|
*
|
|
* \f$q_{0}\f$ and \f$\dot{q}_{0}\f$ are initial guesses, \f$\phi _{q}\f$ is the
|
|
* constraints jacobian (partial derivative of \f$\phi\f$ with respect to \f$q\f$),
|
|
* and \f$\phi _{t}(q)\f$ is the partial derivative of \f$\phi\f$ with respect
|
|
* to time. \f$W\f$ is a diagonal weighting matrix, which can be exploited
|
|
* to prioritize changes in the position/ velocity of specific joints.
|
|
* With a solver capable of handling singular matrices, it is possible to set to
|
|
* 1 the weight of the joint positions/ velocities that should not be changed
|
|
* from the initial guesses, and to 0 those corresponding to the values that
|
|
* can be changed.
|
|
*
|
|
* These problems are solved using the Lagrange multipliers method. For the
|
|
* velocity problem the solution is exact. For the position problem the
|
|
* constraints are linearized in the form
|
|
* \f$ \phi (q_{0}) + \phi _{t}(q0) + \phi _{q_0}(q) (q - q_{0}) \f$
|
|
* and the linearized problem is solved iteratively until the constraint position
|
|
* errors are smaller than a given threshold.
|
|
*
|
|
* RBDL provides two functions to compute feasible joint position and velocities:
|
|
* - CalcAssemblyQ()
|
|
* - CalcAssemblyQDot()
|
|
*
|
|
* \subsection baumgarte_stabilization Baumgarte stabilization
|
|
*
|
|
* The constrained dynamic equations are correct in theory, but are not stable
|
|
* during numeric integration. RBDL implements Baumgarte stabilization to avoid
|
|
* the accumulation of position and velocity errors.
|
|
*
|
|
* The dynamic equations are changed to the following form: \f[
|
|
\left(
|
|
\begin{array}{cc}
|
|
H & G^T \\
|
|
G & 0
|
|
\end{array}
|
|
\right)
|
|
\left(
|
|
\begin{array}{c}
|
|
\ddot{q} \\
|
|
- \lambda
|
|
\end{array}
|
|
\right)
|
|
=
|
|
\left(
|
|
\begin{array}{c}
|
|
-C + \tau \\
|
|
\gamma + \gamma _{stab}
|
|
\end{array}
|
|
\right)
|
|
* \f] A term \f$\gamma _{stab}\f$ is added to the right hand side of the
|
|
* equation, defined in the following way: \f[
|
|
\gamma _{stab} =
|
|
- \frac{2}{T_{stab}} \dot{\phi} (q)
|
|
- \left(\frac{1}{T_{stab}} \right)^2 \phi (q)
|
|
* \f] where \f$\phi (q)\f$ are the position level constraint errors and
|
|
* \f$T_{stab}\f$ is a tuning coefficient. The value of \f$T_{stab}\f$ must
|
|
* be chosen carefully. If it is too large the stabilization will not be able
|
|
* to compensate the errors and the position and velocity errors will increase.
|
|
* If it is too small, the system of equations will become unnecessarily stiff.
|
|
*
|
|
* @{
|
|
*/
|
|
|
|
struct Model;
|
|
|
|
/** \brief Structure that contains both constraint information and workspace memory.
|
|
*
|
|
* This structure is used to reduce the amount of memory allocations that
|
|
* are needed when computing constraint forces.
|
|
*
|
|
* The ConstraintSet has to be bound to a model using ConstraintSet::Bind()
|
|
* before it can be used in \link RigidBodyDynamics::ForwardDynamicsContacts
|
|
* ForwardDynamicsContacts \endlink.
|
|
*/
|
|
struct RBDL_DLLAPI ConstraintSet {
|
|
ConstraintSet() :
|
|
linear_solver (Math::LinearSolverColPivHouseholderQR),
|
|
bound (false) {}
|
|
|
|
// Enum to describe the type of a constraint.
|
|
enum ConstraintType {
|
|
ContactConstraint,
|
|
LoopConstraint,
|
|
ConstraintTypeLast,
|
|
};
|
|
|
|
/** \brief Adds a contact constraint to the constraint set.
|
|
*
|
|
* This type of constraints ensures that the velocity and acceleration of a specified
|
|
* body point along a specified axis are null. This constraint does not act
|
|
* at the position level.
|
|
*
|
|
* \param body_id the body which is affected directly by the constraint
|
|
* \param body_point the point that is constrained relative to the
|
|
* contact body
|
|
* \param world_normal the normal along the constraint acts (in base
|
|
* coordinates)
|
|
* \param constraint_name a human readable name (optional, default: NULL)
|
|
* \param normal_acceleration the acceleration of the contact along the normal
|
|
* (optional, default: 0.)
|
|
*/
|
|
unsigned int AddContactConstraint (
|
|
unsigned int body_id,
|
|
const Math::Vector3d &body_point,
|
|
const Math::Vector3d &world_normal,
|
|
const char *constraint_name = NULL,
|
|
double normal_acceleration = 0.);
|
|
|
|
/** \brief Adds a loop constraint to the constraint set.
|
|
*
|
|
* This type of constraints ensures that the relative orientation and position,
|
|
* spatial velocity, and spatial acceleration between two frames in two bodies
|
|
* are null along a specified spatial constraint axis.
|
|
*
|
|
* \param id_predecessor the identifier of the predecessor body
|
|
* \param id_successor the identifier of the successor body
|
|
* \param X_predecessor a spatial transform localizing the constrained
|
|
* frames on the predecessor body, expressed with respect to the predecessor
|
|
* body frame
|
|
* \param X_successor a spatial transform localizing the constrained
|
|
* frames on the successor body, expressed with respect to the successor
|
|
* body frame
|
|
* \param axis a spatial vector indicating the axis along which the constraint
|
|
* acts
|
|
* \param T_stab_inv coefficient for Baumgarte stabilization.
|
|
* \param constraint_name a human readable name (optional, default: NULL)
|
|
*
|
|
*/
|
|
unsigned int AddLoopConstraint(
|
|
unsigned int id_predecessor,
|
|
unsigned int id_successor,
|
|
const Math::SpatialTransform &X_predecessor,
|
|
const Math::SpatialTransform &X_successor,
|
|
const Math::SpatialVector &axis,
|
|
double T_stab_inv,
|
|
const char *constraint_name = NULL
|
|
);
|
|
|
|
/** \brief Copies the constraints and resets its ConstraintSet::bound
|
|
* flag.
|
|
*/
|
|
ConstraintSet Copy() {
|
|
ConstraintSet result (*this);
|
|
result.bound = false;
|
|
|
|
return result;
|
|
}
|
|
|
|
/** \brief Specifies which method should be used for solving undelying linear systems.
|
|
*/
|
|
void SetSolver (Math::LinearSolver solver) {
|
|
linear_solver = solver;
|
|
}
|
|
|
|
/** \brief Initializes and allocates memory for the constraint set.
|
|
*
|
|
* This function allocates memory for temporary values and matrices that
|
|
* are required for contact force computation. Both model and constraint
|
|
* set must not be changed after a binding as the required memory is
|
|
* dependent on the model size (i.e. the number of bodies and degrees of
|
|
* freedom) and the number of constraints in the Constraint set.
|
|
*
|
|
* The values of ConstraintSet::acceleration may still be
|
|
* modified after the set is bound to the model.
|
|
*
|
|
*/
|
|
bool Bind (const Model &model);
|
|
|
|
/** \brief Returns the number of constraints. */
|
|
size_t size() const {
|
|
return acceleration.size();
|
|
}
|
|
|
|
/** \brief Clears all variables in the constraint set. */
|
|
void clear ();
|
|
|
|
/// Method that should be used to solve internal linear systems.
|
|
Math::LinearSolver linear_solver;
|
|
/// Whether the constraint set was bound to a model (mandatory!).
|
|
bool bound;
|
|
|
|
// Common constraints variables.
|
|
std::vector<ConstraintType> constraintType;
|
|
std::vector<std::string> name;
|
|
std::vector<unsigned int> contactConstraintIndices;
|
|
std::vector<unsigned int> loopConstraintIndices;
|
|
|
|
// Contact constraints variables.
|
|
std::vector<unsigned int> body;
|
|
std::vector<Math::Vector3d> point;
|
|
std::vector<Math::Vector3d> normal;
|
|
|
|
// Loop constraints variables.
|
|
std::vector<unsigned int> body_p;
|
|
std::vector<unsigned int> body_s;
|
|
std::vector<Math::SpatialTransform> X_p;
|
|
std::vector<Math::SpatialTransform> X_s;
|
|
std::vector<Math::SpatialVector> constraintAxis;
|
|
/** Baumgarte stabilization parameter */
|
|
std::vector<double> T_stab_inv;
|
|
/** Position error for the Baumgarte stabilization */
|
|
Math::VectorNd err;
|
|
/** Velocity error for the Baumgarte stabilization */
|
|
Math::VectorNd errd;
|
|
|
|
/** Enforced accelerations of the contact points along the contact
|
|
* normal. */
|
|
Math::VectorNd acceleration;
|
|
/** Actual constraint forces along the contact normals. */
|
|
Math::VectorNd force;
|
|
/** Actual constraint impulses along the contact normals. */
|
|
Math::VectorNd impulse;
|
|
/** The velocities we want to have along the contact normals */
|
|
Math::VectorNd v_plus;
|
|
|
|
// Variables used by the Lagrangian methods
|
|
|
|
/// Workspace for the joint space inertia matrix.
|
|
Math::MatrixNd H;
|
|
/// Workspace for the coriolis forces.
|
|
Math::VectorNd C;
|
|
/// Workspace of the lower part of b.
|
|
Math::VectorNd gamma;
|
|
Math::MatrixNd G;
|
|
/// Workspace for the Lagrangian left-hand-side matrix.
|
|
Math::MatrixNd A;
|
|
/// Workspace for the Lagrangian right-hand-side.
|
|
Math::VectorNd b;
|
|
/// Workspace for the Lagrangian solution.
|
|
Math::VectorNd x;
|
|
|
|
/// Workspace when evaluating contact Jacobians
|
|
Math::MatrixNd Gi;
|
|
/// Workspace when evaluating loop Jacobians
|
|
Math::MatrixNd GSpi;
|
|
/// Workspace when evaluating loop Jacobians
|
|
Math::MatrixNd GSsi;
|
|
/// Workspace when evaluating loop Jacobians
|
|
Math::MatrixNd GSJ;
|
|
|
|
/// Workspace for the QR decomposition of the null-space method
|
|
#ifdef RBDL_USE_SIMPLE_MATH
|
|
SimpleMath::HouseholderQR<Math::MatrixNd> GT_qr;
|
|
#else
|
|
Eigen::HouseholderQR<Math::MatrixNd> GT_qr;
|
|
#endif
|
|
|
|
Math::MatrixNd GT_qr_Q;
|
|
Math::MatrixNd Y;
|
|
Math::MatrixNd Z;
|
|
Math::VectorNd qddot_y;
|
|
Math::VectorNd qddot_z;
|
|
|
|
// Variables used by the IABI methods
|
|
|
|
/// Workspace for the Inverse Articulated-Body Inertia.
|
|
Math::MatrixNd K;
|
|
/// Workspace for the accelerations of due to the test forces
|
|
Math::VectorNd a;
|
|
/// Workspace for the test accelerations.
|
|
Math::VectorNd QDDot_t;
|
|
/// Workspace for the default accelerations.
|
|
Math::VectorNd QDDot_0;
|
|
/// Workspace for the test forces.
|
|
std::vector<Math::SpatialVector> f_t;
|
|
/// Workspace for the actual spatial forces.
|
|
std::vector<Math::SpatialVector> f_ext_constraints;
|
|
/// Workspace for the default point accelerations.
|
|
std::vector<Math::Vector3d> point_accel_0;
|
|
|
|
/// Workspace for the bias force due to the test force
|
|
std::vector<Math::SpatialVector> d_pA;
|
|
/// Workspace for the acceleration due to the test force
|
|
std::vector<Math::SpatialVector> d_a;
|
|
Math::VectorNd d_u;
|
|
|
|
/// Workspace for the inertia when applying constraint forces
|
|
std::vector<Math::SpatialMatrix> d_IA;
|
|
/// Workspace when applying constraint forces
|
|
std::vector<Math::SpatialVector> d_U;
|
|
/// Workspace when applying constraint forces
|
|
Math::VectorNd d_d;
|
|
|
|
std::vector<Math::Vector3d> d_multdof3_u;
|
|
};
|
|
|
|
/** \brief Computes the position errors for the given ConstraintSet.
|
|
*
|
|
* \param model the model
|
|
* \param Q the generalized positions of the joints
|
|
* \param CS the constraint set for which the error should be computed
|
|
* \param err (output) vector where the error will be stored in (should have
|
|
* the size of CS).
|
|
* \param update_kinematics whether the kinematics of the model should be
|
|
* updated from Q.
|
|
*
|
|
* \note the position error is always 0 for contact constraints.
|
|
*
|
|
*/
|
|
RBDL_DLLAPI
|
|
void CalcConstraintsPositionError(
|
|
Model& model,
|
|
const Math::VectorNd &Q,
|
|
ConstraintSet &CS,
|
|
Math::VectorNd& err,
|
|
bool update_kinematics = true
|
|
);
|
|
|
|
/** \brief Computes the Jacobian for the given ConstraintSet
|
|
*
|
|
* \param model the model
|
|
* \param Q the generalized positions of the joints
|
|
* \param CS the constraint set for which the Jacobian should be computed
|
|
* \param G (output) matrix where the output will be stored in
|
|
* \param update_kinematics whether the kinematics of the model should be
|
|
* updated from Q
|
|
*
|
|
*/
|
|
RBDL_DLLAPI
|
|
void CalcConstraintsJacobian(
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
ConstraintSet &CS,
|
|
Math::MatrixNd &G,
|
|
bool update_kinematics = true
|
|
);
|
|
|
|
/** \brief Computes the velocity errors for the given ConstraintSet.
|
|
*
|
|
*
|
|
* \param model the model
|
|
* \param Q the generalized positions of the joints
|
|
* \param QDot the generalized velocities of the joints
|
|
* \param CS the constraint set for which the error should be computed
|
|
* \param err (output) vector where the error will be stored in (should have
|
|
* the size of CS).
|
|
* \param update_kinematics whether the kinematics of the model should be
|
|
* updated from Q.
|
|
*
|
|
* \note this is equivalent to multiplying the constraint jacobian by the
|
|
* generalized velocities of the joints.
|
|
*
|
|
*/
|
|
RBDL_DLLAPI
|
|
void CalcConstraintsVelocityError(
|
|
Model& model,
|
|
const Math::VectorNd &Q,
|
|
const Math::VectorNd &QDot,
|
|
ConstraintSet &CS,
|
|
Math::VectorNd& err,
|
|
bool update_kinematics = true
|
|
);
|
|
|
|
/** \brief Computes the terms \f$H\f$, \f$G\f$, and \f$\gamma\f$ of the
|
|
* constrained dynamic problem and stores them in the ConstraintSet.
|
|
*
|
|
*
|
|
* \param model the model
|
|
* \param Q the generalized positions of the joints
|
|
* \param QDot the generalized velocities of the joints
|
|
* \param Tau the generalized forces of the joints
|
|
* \param CS the constraint set for which the error should be computed
|
|
*
|
|
* \note This function is normally called automatically in the various
|
|
* constrained dynamics functions, the user normally does not have to call it.
|
|
*
|
|
*/
|
|
RBDL_DLLAPI
|
|
void CalcConstrainedSystemVariables (
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
const Math::VectorNd &QDot,
|
|
const Math::VectorNd &Tau,
|
|
ConstraintSet &CS
|
|
);
|
|
|
|
/** \brief Computes a feasible initial value of the generalized joint positions.
|
|
*
|
|
* \param model the model
|
|
* \param QInit initial guess for the generalized positions of the joints
|
|
* \param CS the constraint set for which the error should be computed
|
|
* \param Q (output) vector of the generalized joint positions.
|
|
* \param weights weighting coefficients for the different joint positions.
|
|
* \param tolerance the function will return successfully if the constraint
|
|
* position error norm is lower than this value.
|
|
* \param max_iter the funciton will return unsuccessfully after performing
|
|
* this number of iterations.
|
|
*
|
|
* \return true if the generalized joint positions were computed successfully,
|
|
* false otherwise.
|
|
*
|
|
*/
|
|
RBDL_DLLAPI
|
|
bool CalcAssemblyQ(
|
|
Model &model,
|
|
Math::VectorNd QInit,
|
|
ConstraintSet &CS,
|
|
Math::VectorNd &Q,
|
|
const Math::VectorNd &weights,
|
|
double tolerance = 1e-12,
|
|
unsigned int max_iter = 100
|
|
);
|
|
|
|
/** \brief Computes a feasible initial value of the generalized joint velocities.
|
|
*
|
|
* \param model the model
|
|
* \param Q the generalized joint position of the joints. It is assumed that
|
|
* this vector satisfies the position level assemblt constraints.
|
|
* \param QDotInit initial guess for the generalized velocities of the joints
|
|
* \param CS the constraint set for which the error should be computed
|
|
* \param QDot (output) vector of the generalized joint velocities.
|
|
* \param weights weighting coefficients for the different joint positions.
|
|
*
|
|
*/
|
|
RBDL_DLLAPI
|
|
void CalcAssemblyQDot(
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
const Math::VectorNd &QDotInit,
|
|
ConstraintSet &CS,
|
|
Math::VectorNd &QDot,
|
|
const Math::VectorNd &weights
|
|
);
|
|
|
|
/** \brief Computes forward dynamics with contact by constructing and solving
|
|
* the full lagrangian equation
|
|
*
|
|
* This method builds and solves the linear system \f[
|
|
\left(
|
|
\begin{array}{cc}
|
|
H & G^T \\
|
|
G & 0
|
|
\end{array}
|
|
\right)
|
|
\left(
|
|
\begin{array}{c}
|
|
\ddot{q} \\
|
|
-\lambda
|
|
\end{array}
|
|
\right)
|
|
=
|
|
\left(
|
|
\begin{array}{c}
|
|
-C + \tau \\
|
|
\gamma
|
|
\end{array}
|
|
\right)
|
|
* \f] where \f$H\f$ is the joint space inertia matrix computed with the
|
|
* CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the
|
|
* contact points, \f$C\f$ the bias force (sometimes called "non-linear
|
|
* effects"), and \f$\gamma\f$ the generalized acceleration independent
|
|
* part of the contact point accelerations.
|
|
*
|
|
* \note So far, only constraints acting along cartesian coordinate axes
|
|
* are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must
|
|
* not specify redundant constraints!
|
|
*
|
|
* \par
|
|
*
|
|
* \note To increase performance group constraints body and pointwise such
|
|
* that constraints acting on the same body point are sequentially in
|
|
* ConstraintSet. This can save computation of point jacobians \f$G\f$.
|
|
*
|
|
* \param model rigid body model
|
|
* \param Q state vector of the internal joints
|
|
* \param QDot velocity vector of the internal joints
|
|
* \param Tau actuations of the internal joints
|
|
* \param CS the description of all acting constraints
|
|
* \param QDDot accelerations of the internals joints (output)
|
|
*
|
|
* \note During execution of this function values such as
|
|
* ConstraintSet::force get modified and will contain the value
|
|
* of the force acting along the normal.
|
|
*
|
|
*/
|
|
RBDL_DLLAPI
|
|
void ForwardDynamicsConstraintsDirect (
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
const Math::VectorNd &QDot,
|
|
const Math::VectorNd &Tau,
|
|
ConstraintSet &CS,
|
|
Math::VectorNd &QDDot
|
|
);
|
|
|
|
RBDL_DLLAPI
|
|
void ForwardDynamicsConstraintsRangeSpaceSparse (
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
const Math::VectorNd &QDot,
|
|
const Math::VectorNd &Tau,
|
|
ConstraintSet &CS,
|
|
Math::VectorNd &QDDot
|
|
);
|
|
|
|
RBDL_DLLAPI
|
|
void ForwardDynamicsConstraintsNullSpace (
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
const Math::VectorNd &QDot,
|
|
const Math::VectorNd &Tau,
|
|
ConstraintSet &CS,
|
|
Math::VectorNd &QDDot
|
|
);
|
|
|
|
/** \brief Computes forward dynamics that accounts for active contacts in
|
|
* ConstraintSet.
|
|
*
|
|
* The method used here is the one described by Kokkevis and Metaxas in the
|
|
* Paper "Practical Physics for Articulated Characters", Game Developers
|
|
* Conference, 2004.
|
|
*
|
|
* It does this by recursively computing the inverse articulated-body inertia (IABI)
|
|
* \f$\Phi_{i,j}\f$ which is then used to build and solve a system of the form:
|
|
\f[
|
|
\left(
|
|
\begin{array}{c}
|
|
\dot{v}_1 \\
|
|
\dot{v}_2 \\
|
|
\vdots \\
|
|
\dot{v}_n
|
|
\end{array}
|
|
\right)
|
|
=
|
|
\left(
|
|
\begin{array}{cccc}
|
|
\Phi_{1,1} & \Phi_{1,2} & \cdots & \Phi{1,n} \\
|
|
\Phi_{2,1} & \Phi_{2,2} & \cdots & \Phi{2,n} \\
|
|
\cdots & \cdots & \cdots & \vdots \\
|
|
\Phi_{n,1} & \Phi_{n,2} & \cdots & \Phi{n,n}
|
|
\end{array}
|
|
\right)
|
|
\left(
|
|
\begin{array}{c}
|
|
f_1 \\
|
|
f_2 \\
|
|
\vdots \\
|
|
f_n
|
|
\end{array}
|
|
\right)
|
|
+
|
|
\left(
|
|
\begin{array}{c}
|
|
\phi_1 \\
|
|
\phi_2 \\
|
|
\vdots \\
|
|
\phi_n
|
|
\end{array}
|
|
\right).
|
|
\f]
|
|
Here \f$n\f$ is the number of constraints and the method for building the system
|
|
uses the Articulated Body Algorithm to efficiently compute entries of the system. The
|
|
values \f$\dot{v}_i\f$ are the constraint accelerations, \f$f_i\f$ the constraint forces,
|
|
and \f$\phi_i\f$ are the constraint bias forces.
|
|
*
|
|
* \param model rigid body model
|
|
* \param Q state vector of the internal joints
|
|
* \param QDot velocity vector of the internal joints
|
|
* \param Tau actuations of the internal joints
|
|
* \param CS a list of all contact points
|
|
* \param QDDot accelerations of the internals joints (output)
|
|
*
|
|
* \note During execution of this function values such as
|
|
* ConstraintSet::force get modified and will contain the value
|
|
* of the force acting along the normal.
|
|
*
|
|
* \note This function supports only contact constraints.
|
|
*
|
|
* \todo Allow for external forces
|
|
*/
|
|
RBDL_DLLAPI
|
|
void ForwardDynamicsContactsKokkevis (
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
const Math::VectorNd &QDot,
|
|
const Math::VectorNd &Tau,
|
|
ConstraintSet &CS,
|
|
Math::VectorNd &QDDot
|
|
);
|
|
|
|
/** \brief Computes contact gain by constructing and solving the full lagrangian
|
|
* equation
|
|
*
|
|
* This method builds and solves the linear system \f[
|
|
\left(
|
|
\begin{array}{cc}
|
|
H & G^T \\
|
|
G & 0
|
|
\end{array}
|
|
\right)
|
|
\left(
|
|
\begin{array}{c}
|
|
\dot{q}^{+} \\
|
|
\Lambda
|
|
\end{array}
|
|
\right)
|
|
=
|
|
\left(
|
|
\begin{array}{c}
|
|
H \dot{q}^{-} \\
|
|
v^{+}
|
|
\end{array}
|
|
\right)
|
|
* \f] where \f$H\f$ is the joint space inertia matrix computed with the
|
|
* CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the
|
|
* contact points, \f$\dot{q}^{+}\f$ the generalized velocity after the
|
|
* impact, \f$\Lambda\f$ the impulses at each constraint, \f$\dot{q}^{-}\f$
|
|
* the generalized velocity before the impact, and \f$v^{+}\f$ the desired
|
|
* velocity of each constraint after the impact (known beforehand, usually
|
|
* 0). The value of \f$v^{+}\f$ can is specified via the variable
|
|
* ConstraintSet::v_plus and defaults to 0.
|
|
*
|
|
* \note So far, only constraints acting along cartesian coordinate axes
|
|
* are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must
|
|
* not specify redundant constraints!
|
|
*
|
|
* \par
|
|
*
|
|
* \note To increase performance group constraints body and pointwise such
|
|
* that constraints acting on the same body point are sequentially in
|
|
* ConstraintSet. This can save computation of point Jacobians \f$G\f$.
|
|
*
|
|
* \param model rigid body model
|
|
* \param Q state vector of the internal joints
|
|
* \param QDotMinus velocity vector of the internal joints before the impact
|
|
* \param CS the set of active constraints
|
|
* \param QDotPlus velocities of the internals joints after the impact (output)
|
|
*/
|
|
RBDL_DLLAPI
|
|
void ComputeConstraintImpulsesDirect (
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
const Math::VectorNd &QDotMinus,
|
|
ConstraintSet &CS,
|
|
Math::VectorNd &QDotPlus
|
|
);
|
|
|
|
/** \brief Resolves contact gain using SolveContactSystemRangeSpaceSparse()
|
|
*/
|
|
RBDL_DLLAPI
|
|
void ComputeConstraintImpulsesRangeSpaceSparse (
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
const Math::VectorNd &QDotMinus,
|
|
ConstraintSet &CS,
|
|
Math::VectorNd &QDotPlus
|
|
);
|
|
|
|
/** \brief Resolves contact gain using SolveContactSystemNullSpace()
|
|
*/
|
|
RBDL_DLLAPI
|
|
void ComputeConstraintImpulsesNullSpace (
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
const Math::VectorNd &QDotMinus,
|
|
ConstraintSet &CS,
|
|
Math::VectorNd &QDotPlus
|
|
);
|
|
|
|
/** \brief Solves the full contact system directly, i.e. simultaneously for
|
|
* contact forces and joint accelerations.
|
|
*
|
|
* This solves a \f$ (n_\textit{dof} +
|
|
* n_c) \times (n_\textit{dof} + n_c\f$ linear system.
|
|
*
|
|
* \param H the joint space inertia matrix
|
|
* \param G the constraint jacobian
|
|
* \param c the \f$ \mathbb{R}^{n_\textit{dof}}\f$ vector of the upper part of
|
|
* the right hand side of the system
|
|
* \param gamma the \f$ \mathbb{R}^{n_c}\f$ vector of the lower part of the
|
|
* right hand side of the system
|
|
* \param qddot result: joint accelerations
|
|
* \param lambda result: constraint forces
|
|
* \param A work-space for the matrix of the linear system
|
|
* \param b work-space for the right-hand-side of the linear system
|
|
* \param x work-space for the solution of the linear system
|
|
* \param type of solver that should be used to solve the system
|
|
*/
|
|
RBDL_DLLAPI
|
|
void SolveConstrainedSystemDirect (
|
|
Math::MatrixNd &H,
|
|
const Math::MatrixNd &G,
|
|
const Math::VectorNd &c,
|
|
const Math::VectorNd &gamma,
|
|
Math::VectorNd &qddot,
|
|
Math::VectorNd &lambda,
|
|
Math::MatrixNd &A,
|
|
Math::VectorNd &b,
|
|
Math::VectorNd &x,
|
|
Math::LinearSolver &linear_solver
|
|
);
|
|
|
|
/** \brief Solves the contact system by first solving for the the joint
|
|
* accelerations and then the contact forces using a sparse matrix
|
|
* decomposition of the joint space inertia matrix.
|
|
*
|
|
* This method exploits the branch-induced sparsity by the structure
|
|
* preserving \f$L^TL \f$ decomposition described in RBDL, Section 6.5.
|
|
*
|
|
* \param H the joint space inertia matrix
|
|
* \param G the constraint jacobian
|
|
* \param c the \f$ \mathbb{R}^{n_\textit{dof}}\f$ vector of the upper part of
|
|
* the right hand side of the system
|
|
* \param gamma the \f$ \mathbb{R}^{n_c}\f$ vector of the lower part of the
|
|
* right hand side of the system
|
|
* \param qddot result: joint accelerations
|
|
* \param lambda result: constraint forces
|
|
* \param K work-space for the matrix of the constraint force linear system
|
|
* \param a work-space for the right-hand-side of the constraint force linear
|
|
* system
|
|
* \param linear_solver type of solver that should be used to solve the
|
|
* constraint force system
|
|
*/
|
|
RBDL_DLLAPI
|
|
void SolveConstrainedSystemRangeSpaceSparse (
|
|
Model &model,
|
|
Math::MatrixNd &H,
|
|
const Math::MatrixNd &G,
|
|
const Math::VectorNd &c,
|
|
const Math::VectorNd &gamma,
|
|
Math::VectorNd &qddot,
|
|
Math::VectorNd &lambda,
|
|
Math::MatrixNd &K,
|
|
Math::VectorNd &a,
|
|
Math::LinearSolver linear_solver
|
|
);
|
|
|
|
/** \brief Solves the contact system by first solving for the joint
|
|
* accelerations and then for the constraint forces.
|
|
*
|
|
* This methods requires a \f$n_\textit{dof} \times n_\textit{dof}\f$
|
|
* matrix of the form \f$\left[ \ Y \ | Z \ \right]\f$ with the property
|
|
* \f$GZ = 0\f$ that can be computed using a QR decomposition (e.g. see
|
|
* code for ForwardDynamicsContactsNullSpace()).
|
|
*
|
|
* \param H the joint space inertia matrix
|
|
* \param G the constraint jacobian
|
|
* \param c the \f$ \mathbb{R}^{n_\textit{dof}}\f$ vector of the upper part of
|
|
* the right hand side of the system
|
|
* \param gamma the \f$ \mathbb{R}^{n_c}\f$ vector of the lower part of the
|
|
* right hand side of the system
|
|
* \param qddot result: joint accelerations
|
|
* \param lambda result: constraint forces
|
|
* \param Y basis for the range-space of the constraints
|
|
* \param Z basis for the null-space of the constraints
|
|
* \param qddot_y work-space of size \f$\mathbb{R}^{n_\textit{dof}}\f$
|
|
* \param qddot_z work-space of size \f$\mathbb{R}^{n_\textit{dof}}\f$
|
|
* \param linear_solver type of solver that should be used to solve the system
|
|
*/
|
|
RBDL_DLLAPI
|
|
void SolveConstrainedSystemNullSpace (
|
|
Math::MatrixNd &H,
|
|
const Math::MatrixNd &G,
|
|
const Math::VectorNd &c,
|
|
const Math::VectorNd &gamma,
|
|
Math::VectorNd &qddot,
|
|
Math::VectorNd &lambda,
|
|
Math::MatrixNd &Y,
|
|
Math::MatrixNd &Z,
|
|
Math::VectorNd &qddot_y,
|
|
Math::VectorNd &qddot_z,
|
|
Math::LinearSolver &linear_solver
|
|
);
|
|
|
|
/** @} */
|
|
|
|
} /* namespace RigidBodyDynamics */
|
|
|
|
/* RBDL_CONSTRAINTS_H */
|
|
#endif
|