Constraint_Loop.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_LOOP_CONSTRAINT_H
#define RBDL_LOOP_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 LoopConstraint : public Constraint {
 
public:
 
 
  LoopConstraint();
 
  LoopConstraint(
      const unsigned int bodyIdPredecessor,
      const unsigned int bodyIdSuccessor,
      const Math::SpatialTransform &XPredecessor,
      const Math::SpatialTransform &XSuccessor,
      const Math::SpatialVector &constraintAxisInPredessor,
      bool enableBaumgarteStabilization = false,
      double stabilizationTimeConstant = 0.1,
      const char *loopConstraintName = NULL,
      unsigned int userDefinedId = std::numeric_limits<unsigned int>::max(),
      bool positionLevelConstraint=true,
      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::SpatialVector >& getConstraintAxes(){
    return T;
  }
 
  void appendConstraintAxis(const Math::SpatialVector &constraintAxis,
                          bool positionLevelConstraint = true,
                          bool velocityLevelConstraint = true);
 
 
private:
  std::vector< Math::SpatialVector > T;
  double dblA;
};
 
} 
 
/* RBDL_LOOP_CONSTRAINT_H */
#endif