Constraints.h

Go to the documentation of this file.

/*
 * RBDL - Rigid Body Dynamics Library
 * Copyright (c) 2011-2018 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 {

struct Model;

struct RBDL_DLLAPI CustomConstraint;

struct RBDL_DLLAPI ConstraintSet {
  ConstraintSet() :
    linear_solver (Math::LinearSolverColPivHouseholderQR),
    bound (false) {}

  // Enum to describe the type of a constraint.
  enum ConstraintType {
    ContactConstraint,
    LoopConstraint,
    ConstraintTypeCustom,
    ConstraintTypeLast,
  };

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

  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,
    bool enable_stabilization = false,
    const double stabilization_param = 0.1,
    const char *constraint_name = NULL
    );

  unsigned int AddCustomConstraint(
    CustomConstraint* custom_constraint,
    unsigned int id_predecessor,
    unsigned int id_successor,
    const Math::SpatialTransform &X_predecessor,
    const Math::SpatialTransform &X_successor,
    bool enable_stabilization = false,
    const double stabilization_param = 0.1,
    const char *constraint_name = NULL
    );


  ConstraintSet Copy() {
    ConstraintSet result (*this);
    result.bound = false;

    return result;
  }

  void SetSolver (Math::LinearSolver solver) {
    linear_solver = solver;
  }

  bool Bind (const Model &model);

  size_t size() const {
    return acceleration.size();
  }

  void clear ();

  Math::LinearSolver linear_solver;
  bool bound;

  // Common constraints variables.
  std::vector<ConstraintType> constraintType;
  std::vector<std::string> name;
  std::vector<unsigned int> mContactConstraintIndices;
  std::vector<unsigned int> mLoopConstraintIndices;
  std::vector<unsigned int> mCustomConstraintIndices;
  std::vector< CustomConstraint* > mCustomConstraints;

  // 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;
  std::vector<Math::Vector2d> baumgarteParameters;
  Math::VectorNd err;
  Math::VectorNd errd;

  Math::VectorNd acceleration;
  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 Gi;
  Math::MatrixNd GSpi;
  Math::MatrixNd GSsi;
  Math::MatrixNd GSJ;

#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

  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;

  //CustomConstraint variables.

  
};

RBDL_DLLAPI
void CalcConstraintsPositionError(
  Model& model,
  const Math::VectorNd &Q,
  ConstraintSet &CS,
  Math::VectorNd& err,
  bool update_kinematics = true
);

RBDL_DLLAPI
void CalcConstraintsJacobian(
  Model &model,
  const Math::VectorNd &Q,
  ConstraintSet &CS,
  Math::MatrixNd &G,
  bool update_kinematics = true
);

RBDL_DLLAPI
void CalcConstraintsVelocityError(
  Model& model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDot,
  ConstraintSet &CS,
  Math::VectorNd& err,
  bool update_kinematics = true
);

RBDL_DLLAPI
void CalcConstrainedSystemVariables (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDot,
  const Math::VectorNd &Tau,
  ConstraintSet &CS,
  std::vector<Math::SpatialVector> *f_ext = NULL
);

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
);

RBDL_DLLAPI
void CalcAssemblyQDot(
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDotInit,
  ConstraintSet &CS,
  Math::VectorNd &QDot,
  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 &QDDot,
  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 &QDDot,
  std::vector<Math::SpatialVector> *f_ext = NULL
);

RBDL_DLLAPI
void ForwardDynamicsConstraintsNullSpace (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDot,
  const Math::VectorNd &Tau,
  ConstraintSet &CS,
  Math::VectorNd &QDDot,
  std::vector<Math::SpatialVector> *f_ext = NULL
);

RBDL_DLLAPI
void ForwardDynamicsContactsKokkevis (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDot,
  const Math::VectorNd &Tau,
  ConstraintSet &CS,
  Math::VectorNd &QDDot
);

RBDL_DLLAPI
void ComputeConstraintImpulsesDirect (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDotMinus,
  ConstraintSet &CS,
  Math::VectorNd &QDotPlus
);

RBDL_DLLAPI
void ComputeConstraintImpulsesRangeSpaceSparse (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDotMinus,
  ConstraintSet &CS,
  Math::VectorNd &QDotPlus
);

RBDL_DLLAPI
void ComputeConstraintImpulsesNullSpace (
  Model &model,
  const Math::VectorNd &Q,
  const Math::VectorNd &QDotMinus,
  ConstraintSet &CS,
  Math::VectorNd &QDotPlus
);

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
);

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
);


struct RBDL_DLLAPI CustomConstraint {
    unsigned int mConstraintCount;
    //unsigned int mAssemblyConstraintCount;

    CustomConstraint(unsigned int constraintCount):mConstraintCount(constraintCount){}

    virtual ~CustomConstraint(){};    

    virtual void CalcConstraintsJacobianAndConstraintAxis(
                                          Model &model,
                                          unsigned int custom_constraint_id,
                                          const Math::VectorNd &Q,
                                          ConstraintSet &CS,
                                          Math::MatrixNd &G,
                                          unsigned int GrowStart,
                                          unsigned int GcolStart) = 0;

    virtual void CalcGamma( Model &model,
                            unsigned int custom_constraint_id,
                            const Math::VectorNd &Q,
                            const Math::VectorNd &QDot,
                            ConstraintSet &CS,
                            const Math::MatrixNd &Gblock,
                            Math::VectorNd &gamma,
                            unsigned int gammaStartIndex) = 0;


    virtual void CalcPositionError( Model &model,
                                    unsigned int custom_constraint_id,
                                    const Math::VectorNd &Q,
                                    ConstraintSet &CS,
                                    Math::VectorNd &err,
                                    unsigned int errStartIdx) = 0;

    virtual void CalcVelocityError( Model &model,
                                    unsigned int custom_constraint_id,
                                    const Math::VectorNd &Q,
                                    const Math::VectorNd &QDot,
                                    ConstraintSet &CS,
                                    const Math::MatrixNd &Gblock,
                                    Math::VectorNd &err,
                                    unsigned int errStartIndex) = 0;

};



} /* namespace RigidBodyDynamics */

/* RBDL_CONSTRAINTS_H */
#endif