Constraint.h

Go to the documentation of this file.

/*
 * RBDL - Rigid Body Dynamics Library
 * Copyright (c) 2019 Matthew Millard <millard.matthew@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */
 
#ifndef RBDL_CONSTRAINT_H
#define RBDL_CONSTRAINT_H
 
#include <rbdl/rbdl_math.h>
#include <rbdl/rbdl_mathutils.h>
#include <assert.h>
 
namespace RigidBodyDynamics {
 
enum ConstraintType {
  ConstraintTypeContact=0,
  ConstraintTypeLoop,
  ConstraintTypeCustom,
  ConstraintTypeLast,
};
 
struct ConstraintCache {
 
  Math::VectorNd vecNZeros;
 
  Math::VectorNd vecNA, vecNB, vecNC, vecND;
 
  Math::Vector3d  vec3A, vec3B, vec3C, vec3D, vec3E, vec3F;
 
  Math::Matrix3d  mat3A, mat3B, mat3C, mat3D, mat3E, mat3F;
 
  Math::MatrixNd mat3NA, mat3NB, mat3NC, mat3ND;
 
  Math::MatrixNd mat6NA, mat6NB, mat6NC, mat6ND;
 
  Math::SpatialVector  svecA, svecB, svecC, svecD, svecE, svecF;
 
  Math::SpatialTransform stA, stB, stC, stD;
 
  ConstraintCache(){}
};
 
 
class RBDL_DLLAPI Constraint {
  public:
 
 
//==============================================================================
// Extend the functions below for your own constraint
//==============================================================================
    virtual void bind(const Model &model)=0;
 
 
    virtual void calcConstraintJacobian(  Model &model,
                                          const double time,
                                          const Math::VectorNd &Q,
                                          const Math::VectorNd &QDot,
                                          Math::MatrixNd &GSysOutput,
                                          ConstraintCache &cache,
                                          bool updateKinematics=false) = 0;
 
    virtual void calcGamma( Model &model,
                            const double time,
                            const Math::VectorNd &Q,
                            const Math::VectorNd &QDot,
                            const Math::MatrixNd &GSys,
                            Math::VectorNd &gammaSysOutput,
                            ConstraintCache &cache,
                            bool updateKinematics=false) = 0;
 
 
    virtual void calcPositionError( Model &model,
                                    const double time,
                                    const Math::VectorNd &Q,
                                    Math::VectorNd &errSysOutput,
                                    ConstraintCache &cache,
                                    bool updateKinematics=false) = 0;
 
 
    virtual void calcVelocityError( Model &model,
                                    const double time,
                                    const Math::VectorNd &Q,
                                    const Math::VectorNd &QDot,
                                    const Math::MatrixNd &GSys,
                                    Math::VectorNd &derrSysOutput,
                                    ConstraintCache &cache,
                                    bool updateKinematics=false) = 0;
 
 
    virtual void calcConstraintForces(
                 Model &model,
                 const double time,
                 const Math::VectorNd &Q,
                 const Math::VectorNd &QDot,
                 const Math::MatrixNd &GSys,
                 const Math::VectorNd &LagrangeMultipliersSys,
                 std::vector< unsigned int > &constraintBodiesOutput,
                 std::vector< Math::SpatialTransform > &constraintBodyFramesOutput,
                 std::vector< Math::SpatialVector > &constraintForcesOutput,
                 ConstraintCache &cache,
                 bool resolveAllInRootFrame = false,
                 bool updateKinematics=false) = 0;
 
 
//==============================================================================
// DO NOT TOUCH!!!
//==============================================================================
 
    virtual ~Constraint(){};    
 
    Constraint();
 
    Constraint(const char* nameOfConstraint,
               unsigned int typeOfConstraint,
               unsigned int sizeOfConstraint,
               unsigned int userDefinedIdNumber):               
               typeOfConstraint(typeOfConstraint),
               id(userDefinedIdNumber),
               sizeOfConstraint(sizeOfConstraint),
               baumgarteParameters(1./0.1,1./0.1),
               baumgarteEnabled(false)
    {
      name = "";
      if(nameOfConstraint){
        name = nameOfConstraint;
      }
      positionConstraint.resize(sizeOfConstraint);
      velocityConstraint.resize(sizeOfConstraint);
      for(unsigned int i=0; i<sizeOfConstraint;++i){
        positionConstraint[i]=false;
        velocityConstraint[i]=false;
      }
    }
 
 
    void addToConstraintSet(
        const unsigned int rowIndex)
    {
      rowInSystem = rowIndex;
    }
 
    unsigned int getUserDefinedId(){
      return id;
    }
 
    void setUserDefinedId(unsigned int userDefinedId){
      id = userDefinedId;
    }
 
    void getConstraintJacobian(const Math::MatrixNd &GSys,
                               Math::MatrixNd &GConstraint){
      GConstraint = GSys.block(rowInSystem,0,sizeOfConstraint,GSys.cols());
    }
 
 
    void getGamma(const Math::VectorNd &gammaSys,
                  Math::VectorNd &gammaConstraint){
      gammaConstraint = gammaSys.block(rowInSystem,0,sizeOfConstraint,1);
    }
 
    void getPositionError(const Math::VectorNd &errSys,
                          Math::VectorNd &errConstraint){
      errConstraint = errSys.block(rowInSystem,0,sizeOfConstraint,1);
    }
 
 
    void getVelocityError(const Math::VectorNd &derrSys,
                          Math::VectorNd &derrConstraint){
      derrConstraint = derrSys.block(rowInSystem,0,sizeOfConstraint,1);
    }
 
    void getBaumgarteStabilizationParameters(Math::Vector2d& bgParamsUpd){
      bgParamsUpd = baumgarteParameters;
    }
 
    void getBaumgarteStabilizationForces(const Math::VectorNd &errPos,
                                         const Math::VectorNd &errVel,
                                         Math::VectorNd &baumgarteForces)
    {
 
        baumgarteForces=(-2*baumgarteParameters[0]*errVel
                         -baumgarteParameters[1]*baumgarteParameters[1]*errPos);
    }
 
    void addInBaumgarteStabilizationForces(const Math::VectorNd &errPosSys,
                                           const Math::VectorNd &errVelSys,
                                           Math::VectorNd &gammaSysOutput)
    {
 
      //Here a for loop is used rather than a block operation
      //to be compatible with SimpleMath.
      for(unsigned int i=0; i<sizeOfConstraint;++i){
        gammaSysOutput[rowInSystem+i] +=
              -2.*baumgarteParameters[0]*errVelSys[rowInSystem+i]
             -(baumgarteParameters[1]*baumgarteParameters[1]
              )*errPosSys[rowInSystem+i];
      }
    }
 
    unsigned int getConstraintType(){
      return typeOfConstraint;
    }
 
    unsigned int getConstraintSize(){
      return sizeOfConstraint;
    }
 
    unsigned int getConstraintIndex(){
      return rowInSystem;
    }
 
 
    void setBaumgarteTimeConstant(double tStab){
      assert(tStab > 0);
      baumgarteParameters[0] = 1./tStab;
      baumgarteParameters[1] = 1./tStab;
    }
 
 
    void setEnableBaumgarteStabilization(bool flagEnableBaumgarteStabilization){
      baumgarteEnabled = flagEnableBaumgarteStabilization;
    }
 
    bool isBaumgarteStabilizationEnabled(){
      return baumgarteEnabled;
    }
 
 
    void enableConstraintErrorFromPositionLevel(unsigned int constraintSubIndex)
    {
      assert(constraintSubIndex < sizeOfConstraint);
      positionConstraint[constraintSubIndex] = true;
      velocityConstraint[constraintSubIndex] = true;      
    }
 
    void enableConstraintErrorFromVelocityLevel(unsigned int constraintSubIndex)
    {
      assert(constraintSubIndex < sizeOfConstraint);
      positionConstraint[constraintSubIndex] = false;
      velocityConstraint[constraintSubIndex] = true;      
    }
 
 
    void enableConstraintErrorFromAccelerationLevel(unsigned int constraintSubIndex)
    {
      assert(constraintSubIndex < sizeOfConstraint);
      positionConstraint[constraintSubIndex] = false;
      velocityConstraint[constraintSubIndex] = false;      
    }
 
    void enableConstraintErrorFromPositionLevel()
    {
        for(unsigned int i=0; i<sizeOfConstraint;++i){
          positionConstraint[i] = true;
          velocityConstraint[i] = true;                
        }
    }
 
    void enableConstraintErrorFromVelocityLevel()
    {
        for(unsigned int i=0; i<sizeOfConstraint;++i){
          positionConstraint[i] = false;
          velocityConstraint[i] = true;                
        }
    }
 
    void enableConstraintErrorFromAccelerationLevel()
    {
        for(unsigned int i=0; i<sizeOfConstraint;++i){
          positionConstraint[i] = false;
          velocityConstraint[i] = false;                
        }
    }
 
 
    bool getPositionLevelError(unsigned int constraintSubIndex){
      assert(constraintSubIndex < sizeOfConstraint);
      return positionConstraint[constraintSubIndex];      
    }
 
    bool getVelocityLevelError(unsigned int constraintSubIndex){
      assert(constraintSubIndex < sizeOfConstraint);
      return velocityConstraint[constraintSubIndex];      
    }
 
    const char* getName(){
      return name.c_str();
    }
 
    const std::vector< unsigned int >& getBodyIds(){
      return bodyIds;
    }
 
    const std::vector< Math::SpatialTransform >& getBodyFrames(){
      return bodyFrames;
    }
 
  protected:
    std::string name;
 
    unsigned int id;
    unsigned int typeOfConstraint;
 
    unsigned int sizeOfConstraint;
 
    unsigned int rowInSystem;
 
    std::vector< unsigned int > bodyIds;
 
    std::vector< Math::SpatialTransform > bodyFrames;
 
    Math::Vector2d baumgarteParameters;
 
    bool baumgarteEnabled;
 
    std::vector< bool > positionConstraint;
    std::vector< bool > velocityConstraint;
};
 
 
 
 
} 
 
/* namespace RigidBodyDynamics */
 
/* RBDL_CONSTRAINT_H */
#endif