Constraints.h

Go to the documentation of this file.

/*
 * RBDL - Rigid Body Dynamics Library
 * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
 *               2019 Matthew Millard <mjhmilla@protonmail.com>
 * Licensed under the zlib license. See LICENSE for more details.
 */
 
#ifndef RBDL_CONSTRAINTSETS_H
#define RBDL_CONSTRAINTSETS_H
 
#include <memory>
 
#include <rbdl/rbdl_math.h>
#include <rbdl/rbdl_mathutils.h>
#include <rbdl/Kinematics.h>
#include <rbdl/Constraint.h>
#include <rbdl/Constraint_Contact.h>
#include <rbdl/Constraint_Loop.h>
#include <string.h>
#include <assert.h>
#include <map>
 
namespace RigidBodyDynamics {
 
 
 
 
struct Model;
 
 
 
//class RBDL_DLLAPI Constraint;
 
 
struct RBDL_DLLAPI ConstraintSet {
  ConstraintSet() :
    linear_solver (Math::LinearSolverColPivHouseholderQR),
    bound (false) {}
 
 
 
  unsigned int getGroupIndexByName(const char* userDefinedName){
    std::string conName(userDefinedName);
    return nameGroupMap.at(conName);
  }
 
  unsigned int getGroupIndexById(unsigned int userDefinedId){
    return userDefinedIdGroupMap.at(userDefinedId);
  }
 
  unsigned int getGroupIndexByAssignedId(unsigned int assignedId){
    return idGroupMap.at(assignedId);
  }
 
 
  unsigned int getGroupIndexMax(){
    return unsigned(constraints.size()-1);
  }
 
  const char* getGroupName(unsigned int groupIndex){
    return constraints[groupIndex]->getName();
  }
 
  unsigned int getGroupSize(unsigned int groupIndex){
    return constraints[groupIndex]->getConstraintSize();
  }
 
  unsigned int getGroupType(unsigned int groupIndex){
    return constraints[groupIndex]->getConstraintType();
  }
 
  unsigned int getGroupId(unsigned int groupIndex){  
    return constraints[groupIndex]->getUserDefinedId();
  }
 
 
  unsigned int getGroupAssignedId(unsigned int groupIndex){
    unsigned int assignedId=std::numeric_limits<unsigned int>::max();
    auto it = idGroupMap.begin();
    bool found = false;
    while(it != idGroupMap.end() && found == false){
      if(it->second == groupIndex){
        assignedId = it->first;
        found = true;
      }
      it++;
    }
    if(found==false){
      std::cerr << "Error: a groupIndex of " << groupIndex
                << " could not be found. Valid groupIndex range between 0 and "
                << unsigned(constraints.size()-1);
      assert(0);
      abort();
    }
    return assignedId;
  }
 
  void calcForces(
      unsigned int groupIndex,
      Model& model,
      const Math::VectorNd &Q,
      const Math::VectorNd &QDot,
      std::vector< unsigned int > &updConstraintBodyIdsOutput,
      std::vector< Math::SpatialTransform > &updConstraintBodyFramesOutput,
      std::vector< Math::SpatialVector > &updConstraintForcesOutput,
      bool resolveAllInRootFrame = false,
      bool updateKinematics = false);
 
 
  void calcImpulses(
      unsigned int groupIndex,
      Model& model,
      const Math::VectorNd &Q,
      const Math::VectorNd &QDot,
      std::vector< unsigned int > &constraintBodyIdsOutput,
      std::vector< Math::SpatialTransform > &constraintBodyFramesOutput,
      std::vector< Math::SpatialVector > &constraintImpulsesOutput,
      bool resolveAllInRootFrame = false,
      bool updateKinematics = false);
 
  void calcPositionError(
      unsigned int groupIndex,
      Model& model,
      const Math::VectorNd &Q,
      Math::VectorNd &positionErrorOutput,
      bool updateKinematics = false);
 
 
  void calcVelocityError(
      unsigned int groupIndex,
      Model& model,
      const Math::VectorNd &Q,
      const Math::VectorNd &QDot,
      Math::VectorNd &velocityErrorOutput,
      bool updateKinematics = false);
 
 
  void calcBaumgarteStabilizationForces(
      unsigned int groupIndex,
      Model& model,
      const Math::VectorNd &positionError,
      const Math::VectorNd &velocityError,
      Math::VectorNd &baumgarteForcesOutput);
 
  bool isBaumgarteStabilizationEnabled(
      unsigned int groupIndex){
    return constraints[groupIndex]->isBaumgarteStabilizationEnabled();
  }
 
  void enableBaumgarteStabilization(
      unsigned int groupIndex){
    return constraints[groupIndex]->setEnableBaumgarteStabilization(true);
  }
 
  void disableBaumgarteStabilization(
      unsigned int groupIndex){
    return constraints[groupIndex]->setEnableBaumgarteStabilization(false);
  }
 
 
  void getBaumgarteStabilizationCoefficients(
      unsigned int groupIndex,
      Math::Vector2d& baumgartePositionVelocityCoefficientsOutput){
      constraints[groupIndex]->getBaumgarteStabilizationParameters(
                                baumgartePositionVelocityCoefficientsOutput);
  }
 
  unsigned int AddContactConstraint (
    unsigned int bodyId,
    const Math::Vector3d &bodyPoint,
    const Math::Vector3d &worldNormal,
    const char *constraintName = NULL,
    unsigned int userDefinedId = std::numeric_limits<unsigned int>::max());
 
 
    unsigned int AddLoopConstraint(
      unsigned int bodyIdPredecessor,
      unsigned int bodyIdSuccessor,
      const Math::SpatialTransform &XPredecessor,
      const Math::SpatialTransform &XSuccessor,
      const Math::SpatialVector &constraintAxisInPredecessor,
      bool enableBaumgarteStabilization = false,
      double stabilizationTimeConstant = 0.1,
      const char *constraintName = NULL,
      unsigned int userDefinedId = std::numeric_limits<unsigned int>::max());
 
 
 
 
  unsigned int AddCustomConstraint(
      std::shared_ptr<Constraint> customConstraint);
 
  ConstraintSet Copy() {
    ConstraintSet result (*this);
    result.bound = false;
 
    return result;
  }
 
  void SetSolver (Math::LinearSolver solver) {
    linear_solver = solver;
  }
 
  bool Bind (const Model &model);
 
  void SetActuationMap(const Model &model,
                          const std::vector<bool> &actuatedDof);
 
  size_t size() const {
    return constraintType.size();
  }
 
  void clear ();
 
  Math::LinearSolver linear_solver;
  bool bound;
 
  // Common constraints variables.
  std::vector<ConstraintType> constraintType;
 
  std::vector<std::string> name;
 
  std::map < std::string, unsigned int > nameGroupMap;
  std::map < unsigned int, unsigned int> userDefinedIdGroupMap;
  std::map < unsigned int, unsigned int> idGroupMap;
 
  //A shared_ptr (MM 28 May 2019)
  //  :is 2x as expensive (with the optimize flags turned on) as a regular
  //   pointer (https://www.modernescpp.com/index.php/memory-and-performance-overhead-of-smart-pointer)
  //  :But
  //    -The memory is deleted automatically when all references are deleted.
  //    -A shared_ptr works with the existing Copy function
  //    -... although the Copy is a shallow copy. A deep copy would be better
  //
  //  :A unique_ptr would be faster, but will require copy constructors to
  //   be defined for all constraint objects. In addition unique_ptr is only
  //   included in C++14, while shared_ptr is available in C++11. While this
  //   is clearly the better option, this will require me to make more
  //   edits to ConstraintSet: I don't have the time for this at the moment
  std::vector< std::shared_ptr<Constraint> > constraints;
 
  std::vector< std::shared_ptr<ContactConstraint> > contactConstraints;
 
  std::vector< std::shared_ptr<LoopConstraint> > loopConstraints;
 
  
  Math::VectorNd err;
  Math::VectorNd errd;
 
  Math::VectorNd force;
  Math::VectorNd impulse;
  Math::VectorNd v_plus;
 
  // Variables used by the Lagrangian methods
 
  Math::MatrixNd H;
  Math::VectorNd C;
  Math::VectorNd gamma;
  Math::MatrixNd G;
 
  Math::MatrixNd A;
  Math::VectorNd b;
  Math::VectorNd x;
 
  Math::MatrixNd S;
  Math::MatrixNd P;
  Math::MatrixNd W;
  Math::MatrixNd Winv;
  Math::VectorNd WinvSC;
 
  Math::VectorNd u;
  Math::VectorNd v;
 
  Math::MatrixNd F;
  Math::MatrixNd Ful,Fur,Fll,Flr; //blocks of f for SimpleMath
 
  Math::MatrixNd GT;
  Math::MatrixNd GTu, GTl;//blocks of GT for SimpleMath
  Math::VectorNd g;
  Math::MatrixNd Ru;
  Math::VectorNd py;
  Math::VectorNd pz;
 
#ifndef RBDL_USE_CASADI_MATH
  Eigen::HouseholderQR<Math::MatrixNd> GT_qr;
  Eigen::FullPivHouseholderQR<Math::MatrixNd> GPT_full_qr;
#endif
 
  Math::MatrixNd GT_qr_Q;
  Math::MatrixNd GPT;
  Math::MatrixNd Y;
  Math::MatrixNd Z;
  Math::MatrixNd R;  
  Math::VectorNd qddot_y;
  Math::VectorNd qddot_z;
 
  // Variables used by the IABI methods
  Math::MatrixNd K;
  Math::VectorNd a;
  Math::VectorNd QDDot_t;
  Math::VectorNd QDDot_0;
  std::vector<Math::SpatialVector> f_t;
  std::vector<Math::SpatialVector> f_ext_constraints;
  std::vector<Math::Vector3d> point_accel_0;
 
  std::vector<Math::SpatialVector> d_pA;
  std::vector<Math::SpatialVector> d_a;
  Math::VectorNd d_u;
 
  std::vector<Math::SpatialMatrix> d_IA;
  std::vector<Math::SpatialVector> d_U;
  Math::VectorNd d_d;
 
  std::vector<Math::Vector3d> d_multdof3_u;
 
  ConstraintCache cache;
 
 
 
  
};
 
RBDL_DLLAPI
void CalcConstraintsPositionError(
  Model& model,
  const Math::VectorNd &Q,
  ConstraintSet &CS,
  Math::VectorNd& errOutput,
  bool update_kinematics = true
);
 
RBDL_DLLAPI
void CalcConstraintsJacobian(
  Model &model,
  const Math::VectorNd &Q,
  ConstraintSet &CS,
  Math::MatrixNd &GOutput,
  bool update_kinematics = true
);
 
RBDL_DLLAPI
void CalcConstraintsVelocityError(
  Model& model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDot,
  ConstraintSet &CS,
  Math::VectorNd& errOutput,
  bool update_kinematics = true
);
 
RBDL_DLLAPI
void CalcConstrainedSystemVariables (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDot,
  const Math::VectorNd &Tau,
  ConstraintSet &CSOutput,
  bool update_kinematics = true,
  std::vector<Math::SpatialVector> *f_ext = NULL
);
 
#ifndef RBDL_USE_CASADI_MATH
RBDL_DLLAPI
bool CalcAssemblyQ(
  Model &model,
  Math::VectorNd QInit,
  ConstraintSet &CS,
  Math::VectorNd &QOutput,
  const Math::VectorNd &weights,
  double tolerance = 1e-12,
  unsigned int max_iter = 100
);
#endif
 
RBDL_DLLAPI
void CalcAssemblyQDot(
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDotInit,
  ConstraintSet &CS,
  Math::VectorNd &QDotOutput,
  const Math::VectorNd &weights
);
 
RBDL_DLLAPI
void ForwardDynamicsConstraintsDirect (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDot,
  const Math::VectorNd &Tau,
  ConstraintSet &CS,
  Math::VectorNd &QDDotOutput,
  bool update_kinematics = true,
  std::vector<Math::SpatialVector> *f_ext = NULL
);
 
RBDL_DLLAPI
void ForwardDynamicsConstraintsRangeSpaceSparse (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDot,
  const Math::VectorNd &Tau,
  ConstraintSet &CS,
  Math::VectorNd &QDDotOutput,
  bool update_kinematics = true,
  std::vector<Math::SpatialVector> *f_ext = NULL
);
 
#ifndef RBDL_USE_CASADI_MATH
RBDL_DLLAPI
void ForwardDynamicsConstraintsNullSpace (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDot,
  const Math::VectorNd &Tau,
  ConstraintSet &CS,
  Math::VectorNd &QDDotOutput,
  bool update_kinematics = true,
  std::vector<Math::SpatialVector> *f_ext = NULL
);
#endif
 
RBDL_DLLAPI
void ForwardDynamicsContactsKokkevis (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDot,
  const Math::VectorNd &Tau,
  ConstraintSet &CS,
  Math::VectorNd &QDDotOutput
);
 
 
#ifndef RBDL_USE_CASADI_MATH
RBDL_DLLAPI
void InverseDynamicsConstraintsRelaxed(
    Model &model,
    const Math::VectorNd &Q,
    const Math::VectorNd &QDot,
    const Math::VectorNd &QDDotControls,
    ConstraintSet &CS,
    Math::VectorNd &QDDotOutput,
    Math::VectorNd &TauOutput,
    bool update_kinematics=true,
    std::vector<Math::SpatialVector> *f_ext  = NULL);
#endif
RBDL_DLLAPI
void InverseDynamicsConstraints(
    Model &model,
    const Math::VectorNd &Q,
    const Math::VectorNd &QDot,
    const Math::VectorNd &QDDotDesired,
    ConstraintSet &CS,
    Math::VectorNd &QDDotOutput,
    Math::VectorNd &TauOutput,
    bool update_kinematics=true,
    std::vector<Math::SpatialVector> *f_ext  = NULL);
 
#ifndef RBDL_USE_CASADI_MATH
RBDL_DLLAPI 
bool isConstrainedSystemFullyActuated(
    Model &model,
    const Math::VectorNd &Q,
    const Math::VectorNd &QDot,
    ConstraintSet &CS,
    bool update_kinematics=true,
    std::vector<Math::SpatialVector> *f_ext  = NULL);
#endif
 
RBDL_DLLAPI
void ComputeConstraintImpulsesDirect (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDotMinus,
  ConstraintSet &CS,
  Math::VectorNd &QDotPlusOutput
);
 
RBDL_DLLAPI
void ComputeConstraintImpulsesRangeSpaceSparse (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDotMinus,
  ConstraintSet &CS,
  Math::VectorNd &QDotPlusOutput
);
 
#ifndef RBDL_USE_CASADI_MATH
RBDL_DLLAPI
void ComputeConstraintImpulsesNullSpace (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDotMinus,
  ConstraintSet &CS,
  Math::VectorNd &QDotPlusOutput
);
#endif
 
RBDL_DLLAPI
void SolveConstrainedSystemDirect (
  Math::MatrixNd &H, 
  const Math::MatrixNd &G, 
  const Math::VectorNd &c, 
  const Math::VectorNd &gamma, 
  Math::VectorNd &lambda, 
  Math::MatrixNd &A, 
  Math::VectorNd &b,
  Math::VectorNd &x,
  Math::LinearSolver &linear_solver
);
 
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
);
 
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