Constraint_Contact.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_CONTACT_CONSTRAINT_H
#define RBDL_CONTACT_CONSTRAINT_H
 
#include <rbdl/rbdl_math.h>
#include <rbdl/rbdl_mathutils.h>
#include <rbdl/Kinematics.h>
#include <rbdl/Constraint.h>
#include <assert.h>
 
namespace RigidBodyDynamics {
 
class RBDL_DLLAPI ContactConstraint : public Constraint {
 
public:
 
 
  ContactConstraint();
 
  ContactConstraint(
      const unsigned int bodyId,
      const Math::Vector3d &bodyPoint,
      const Math::Vector3d &groundConstraintNormalVectors,
      const char *contactConstraintName = NULL,
      unsigned int userDefinedId = std::numeric_limits<unsigned int>::max(),
      bool enableBaumgarteStabilization=false,
      double stabilizationTimeConstant=0.1,      
      bool velocityLevelConstraint=true);
 
 
  void bind( const Model &model) override;
 
  void calcConstraintJacobian(  Model &model,
                                const double time,
                                const Math::VectorNd &Q,
                                const Math::VectorNd &QDot,
                                Math::MatrixNd &GSysUpd,
                                ConstraintCache &cache,
                                bool updateKinematics=false) override;
 
  void calcGamma( Model &model,
                  const double time,
                  const Math::VectorNd &Q,
                  const Math::VectorNd &QDot,
                  const Math::MatrixNd &GSys,
                  Math::VectorNd &gammaSysUpd,
                  ConstraintCache &cache,
                  bool updateKinematics=false) override;
 
  void calcPositionError( Model &model,
                          const double time,
                          const Math::VectorNd &Q,
                          Math::VectorNd &errSysUpd,
                          ConstraintCache &cache,
                          bool updateKinematics=false) override;
 
  void calcVelocityError( Model &model,
                          const double time,
                          const Math::VectorNd &Q,
                          const Math::VectorNd &QDot,
                          const Math::MatrixNd &GSys,
                          Math::VectorNd &derrSysUpd,
                          ConstraintCache &cache,
                          bool updateKinematics=false) override;
 
  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 > &constraintBodiesUpd,
        std::vector< Math::SpatialTransform > &constraintBodyFramesUpd,
        std::vector< Math::SpatialVector > &constraintForcesUpd,
        ConstraintCache &cache,
        bool resolveAllInRootFrame = false,
        bool updateKinematics=false) override;
 
 
 
  const std::vector< Math::Vector3d >& getConstraintNormalVectors(){
    return T;
  }
 
  void appendNormalVector(const Math::Vector3d &normal,
                          bool velocityLevelConstraint=true);
 
  /*** @brief Added to support ForwardDynamicsKokkevis
 
    @param model: the multibody model
    @param Q: the generalized positions of the model
    @param QDot: the generalized velocities of the model    
    @param QDDot: the generalized accelerations of the model    
    @param pointAccelerationSysUpd the system's vector of point accelerations
    @param updateKinematics setting this to true will force kinematic functions
            to use the values of Q, QDot, and QDDot passed into this function.
  */
  void calcPointAccelerations(Model &model,
                              const Math::VectorNd &Q,
                              const Math::VectorNd &QDot,
                              const Math::VectorNd &QDDot,
                              std::vector<Math::Vector3d> &pointAccelerationSysUpd,
                              bool updateKinematics=false);
 
  /*** @brief To support ForwardDynamicsKokkevis
    @param model: the multibody model
    @param Q: the generalized positions of the model
    @param QDot: the generalized velocities of the model    
    @param QDDot: the generalized accelerations of the model    
    @param pointAccelerationUpd the acclerations of the one point in this constraint
    @param updateKinematics setting this to true will force kinematic functions
            to use the values of Q, QDot, and QDDot passed into this function.
 
  */
 
  void calcPointAccelerations(Model &model,
                              const Math::VectorNd &Q,
                              const Math::VectorNd &QDot,
                              const Math::VectorNd &QDDot,
                              Math::Vector3d &pointAccelerationUpd,
                              bool updateKinematics=false);
 
 
  /*** @brief To support ForwardDynamicsKokkevis
    @param model a reference to the multibody model
    @param pointAccelerationSysUpd the system's vector of point accelerations
    @param ddErrSysUpd: the error in the the acceleration of the point in this
                        constraint along the normal directions.
 
  */
  void calcPointAccelerationError(
                    const std::vector<Math::Vector3d> &pointAccelerationsSys,
                    Math::VectorNd &ddErrSysUpd);
 
  /*** @brief To support ForwardDynamicsKokkevis
    @param model a reference to the multibody model
    @param Q: the generalized positions of the model
    @param cache: a ConstraintCache struct used for working memory
    @param fExtSysUpd: the point force constraint Jacobian
    @param updateKinematics setting this to true will force kinematic functions
            to use the value of Q passed in.
  */
  void calcPointForceJacobian(
          Model &model,
          const Math::VectorNd &Q,
          ConstraintCache &cache,
          std::vector<Math::SpatialVector> &fExtSysUpd,
          bool updateKinematics=false);
 
private:
  std::vector< Math::Vector3d > T;
  Math::Vector3d groundPoint;
  double dblA;
 
};
 
 
 
} 
 
/* RBDL_CONTACT_CONSTRAINT_H */
#endif