Rigid Body Dynamics Library
Constraints.h
Go to the documentation of this file.
1 /*
2  * RBDL - Rigid Body Dynamics Library
3  * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
4  *
5  * Licensed under the zlib license. See LICENSE for more details.
6  */
7 
8 #ifndef RBDL_CONSTRAINTS_H
9 #define RBDL_CONSTRAINTS_H
10 
11 #include <rbdl/rbdl_math.h>
12 #include <rbdl/rbdl_mathutils.h>
13 
14 namespace RigidBodyDynamics {
15 
253 struct Model;
254 
255 struct RBDL_DLLAPI CustomConstraint;
256 
266 struct RBDL_DLLAPI ConstraintSet {
268  linear_solver (Math::LinearSolverColPivHouseholderQR),
269  bound (false) {}
270 
271  // Enum to describe the type of a constraint.
277  };
278 
294  unsigned int AddContactConstraint (
295  unsigned int body_id,
296  const Math::Vector3d &body_point,
297  const Math::Vector3d &world_normal,
298  const char *constraint_name = NULL,
299  double normal_acceleration = 0.);
300 
324  unsigned int AddLoopConstraint(
325  unsigned int id_predecessor,
326  unsigned int id_successor,
327  const Math::SpatialTransform &X_predecessor,
328  const Math::SpatialTransform &X_successor,
329  const Math::SpatialVector &axis,
330  bool enable_stabilization = false,
331  const double stabilization_param = 0.1,
332  const char *constraint_name = NULL
333  );
334 
356  unsigned int AddCustomConstraint(
357  CustomConstraint* custom_constraint,
358  unsigned int id_predecessor,
359  unsigned int id_successor,
360  const Math::SpatialTransform &X_predecessor,
361  const Math::SpatialTransform &X_successor,
362  bool enable_stabilization = false,
363  const double stabilization_param = 0.1,
364  const char *constraint_name = NULL
365  );
366 
367 
372  ConstraintSet result (*this);
373  result.bound = false;
374 
375  return result;
376  }
377 
380  void SetSolver (Math::LinearSolver solver) {
381  linear_solver = solver;
382  }
383 
396  bool Bind (const Model &model);
397 
399  size_t size() const {
400  return acceleration.size();
401  }
402 
404  void clear ();
405 
407  Math::LinearSolver linear_solver;
409  bool bound;
410 
411  // Common constraints variables.
412  std::vector<ConstraintType> constraintType;
413  std::vector<std::string> name;
414  std::vector<unsigned int> mContactConstraintIndices;
415  std::vector<unsigned int> mLoopConstraintIndices;
416  std::vector<unsigned int> mCustomConstraintIndices;
417  std::vector< CustomConstraint* > mCustomConstraints;
418 
419  // Contact constraints variables.
420  std::vector<unsigned int> body;
421  std::vector<Math::Vector3d> point;
422  std::vector<Math::Vector3d> normal;
423 
424  // Loop constraints variables.
425  std::vector<unsigned int> body_p;
426  std::vector<unsigned int> body_s;
427  std::vector<Math::SpatialTransform> X_p;
428  std::vector<Math::SpatialTransform> X_s;
429  std::vector<Math::SpatialVector> constraintAxis;
431  std::vector<Math::Vector2d> baumgarteParameters;
436 
446 
447  // Variables used by the Lagrangian methods
448 
462 
471 
473 #ifdef RBDL_USE_SIMPLE_MATH
474  SimpleMath::HouseholderQR<Math::MatrixNd> GT_qr;
475 #else
476  Eigen::HouseholderQR<Math::MatrixNd> GT_qr;
477 #endif
478 
484 
485  // Variables used by the IABI methods
486 
496  std::vector<Math::SpatialVector> f_t;
498  std::vector<Math::SpatialVector> f_ext_constraints;
500  std::vector<Math::Vector3d> point_accel_0;
501 
503  std::vector<Math::SpatialVector> d_pA;
505  std::vector<Math::SpatialVector> d_a;
507 
509  std::vector<Math::SpatialMatrix> d_IA;
511  std::vector<Math::SpatialVector> d_U;
514 
515  std::vector<Math::Vector3d> d_multdof3_u;
516 
517  //CustomConstraint variables.
518 
519 
520 };
521 
535 RBDL_DLLAPI
537  Model& model,
538  const Math::VectorNd &Q,
539  ConstraintSet &CS,
540  Math::VectorNd& err,
541  bool update_kinematics = true
542 );
543 
554 RBDL_DLLAPI
556  Model &model,
557  const Math::VectorNd &Q,
558  ConstraintSet &CS,
559  Math::MatrixNd &G,
560  bool update_kinematics = true
561 );
562 
579 RBDL_DLLAPI
581  Model& model,
582  const Math::VectorNd &Q,
583  const Math::VectorNd &QDot,
584  ConstraintSet &CS,
585  Math::VectorNd& err,
586  bool update_kinematics = true
587 );
588 
604 RBDL_DLLAPI
606  Model &model,
607  const Math::VectorNd &Q,
608  const Math::VectorNd &QDot,
609  const Math::VectorNd &Tau,
610  ConstraintSet &CS,
611  std::vector<Math::SpatialVector> *f_ext = NULL
612 );
613 
630 RBDL_DLLAPI
631 bool CalcAssemblyQ(
632  Model &model,
633  Math::VectorNd QInit,
634  ConstraintSet &CS,
635  Math::VectorNd &Q,
636  const Math::VectorNd &weights,
637  double tolerance = 1e-12,
638  unsigned int max_iter = 100
639 );
640 
652 RBDL_DLLAPI
653 void CalcAssemblyQDot(
654  Model &model,
655  const Math::VectorNd &Q,
656  const Math::VectorNd &QDotInit,
657  ConstraintSet &CS,
658  Math::VectorNd &QDot,
659  const Math::VectorNd &weights
660 );
661 
713 RBDL_DLLAPI
715  Model &model,
716  const Math::VectorNd &Q,
717  const Math::VectorNd &QDot,
718  const Math::VectorNd &Tau,
719  ConstraintSet &CS,
720  Math::VectorNd &QDDot,
721  std::vector<Math::SpatialVector> *f_ext = NULL
722 );
723 
724 RBDL_DLLAPI
726  Model &model,
727  const Math::VectorNd &Q,
728  const Math::VectorNd &QDot,
729  const Math::VectorNd &Tau,
730  ConstraintSet &CS,
731  Math::VectorNd &QDDot,
732  std::vector<Math::SpatialVector> *f_ext = NULL
733 );
734 
735 RBDL_DLLAPI
737  Model &model,
738  const Math::VectorNd &Q,
739  const Math::VectorNd &QDot,
740  const Math::VectorNd &Tau,
741  ConstraintSet &CS,
742  Math::VectorNd &QDDot,
743  std::vector<Math::SpatialVector> *f_ext = NULL
744 );
745 
811 RBDL_DLLAPI
813  Model &model,
814  const Math::VectorNd &Q,
815  const Math::VectorNd &QDot,
816  const Math::VectorNd &Tau,
817  ConstraintSet &CS,
818  Math::VectorNd &QDDot
819 );
820 
869 RBDL_DLLAPI
871  Model &model,
872  const Math::VectorNd &Q,
873  const Math::VectorNd &QDotMinus,
874  ConstraintSet &CS,
875  Math::VectorNd &QDotPlus
876 );
877 
880 RBDL_DLLAPI
882  Model &model,
883  const Math::VectorNd &Q,
884  const Math::VectorNd &QDotMinus,
885  ConstraintSet &CS,
886  Math::VectorNd &QDotPlus
887 );
888 
891 RBDL_DLLAPI
893  Model &model,
894  const Math::VectorNd &Q,
895  const Math::VectorNd &QDotMinus,
896  ConstraintSet &CS,
897  Math::VectorNd &QDotPlus
898 );
899 
919 RBDL_DLLAPI
921  Math::MatrixNd &H,
922  const Math::MatrixNd &G,
923  const Math::VectorNd &c,
924  const Math::VectorNd &gamma,
925  Math::VectorNd &qddot,
926  Math::VectorNd &lambda,
927  Math::MatrixNd &A,
928  Math::VectorNd &b,
929  Math::VectorNd &x,
930  Math::LinearSolver &linear_solver
931 );
932 
954 RBDL_DLLAPI
956  Model &model,
957  Math::MatrixNd &H,
958  const Math::MatrixNd &G,
959  const Math::VectorNd &c,
960  const Math::VectorNd &gamma,
961  Math::VectorNd &qddot,
962  Math::VectorNd &lambda,
963  Math::MatrixNd &K,
964  Math::VectorNd &a,
965  Math::LinearSolver linear_solver
966 );
967 
990 RBDL_DLLAPI
992  Math::MatrixNd &H,
993  const Math::MatrixNd &G,
994  const Math::VectorNd &c,
995  const Math::VectorNd &gamma,
996  Math::VectorNd &qddot,
997  Math::VectorNd &lambda,
998  Math::MatrixNd &Y,
999  Math::MatrixNd &Z,
1000  Math::VectorNd &qddot_y,
1001  Math::VectorNd &qddot_z,
1002  Math::LinearSolver &linear_solver
1003 );
1004 
1005 
1048 struct RBDL_DLLAPI CustomConstraint {
1049  unsigned int mConstraintCount;
1050  //unsigned int mAssemblyConstraintCount;
1051 
1052  CustomConstraint(unsigned int constraintCount):mConstraintCount(constraintCount){}
1053 
1054  virtual ~CustomConstraint(){};
1055 
1056  virtual void CalcConstraintsJacobianAndConstraintAxis(
1057  Model &model,
1058  unsigned int custom_constraint_id,
1059  const Math::VectorNd &Q,
1060  ConstraintSet &CS,
1061  Math::MatrixNd &G,
1062  unsigned int GrowStart,
1063  unsigned int GcolStart) = 0;
1064 
1065  virtual void CalcGamma( Model &model,
1066  unsigned int custom_constraint_id,
1067  const Math::VectorNd &Q,
1068  const Math::VectorNd &QDot,
1069  ConstraintSet &CS,
1070  const Math::MatrixNd &Gblock,
1071  Math::VectorNd &gamma,
1072  unsigned int gammaStartIndex) = 0;
1073 
1074 
1075  virtual void CalcPositionError( Model &model,
1076  unsigned int custom_constraint_id,
1077  const Math::VectorNd &Q,
1078  ConstraintSet &CS,
1079  Math::VectorNd &err,
1080  unsigned int errStartIdx) = 0;
1081 
1082  virtual void CalcVelocityError( Model &model,
1083  unsigned int custom_constraint_id,
1084  const Math::VectorNd &Q,
1085  const Math::VectorNd &QDot,
1086  ConstraintSet &CS,
1087  const Math::MatrixNd &Gblock,
1088  Math::VectorNd &err,
1089  unsigned int errStartIndex) = 0;
1090 
1091 };
1092 
1093 
1094 
1097 } /* namespace RigidBodyDynamics */
1098 
1099 /* RBDL_CONSTRAINTS_H */
1100 #endif
std::vector< std::string > name
Definition: Constraints.h:413
Compact representation of spatial transformations.
Math::MatrixNd K
Workspace for the Inverse Articulated-Body Inertia.
Definition: Constraints.h:488
Math::VectorNd b
Workspace for the Lagrangian right-hand-side.
Definition: Constraints.h:459
Math::VectorNd a
Workspace for the accelerations of due to the test forces.
Definition: Constraints.h:490
std::vector< unsigned int > mContactConstraintIndices
Definition: Constraints.h:414
Math::VectorNd C
Workspace for the coriolis forces.
Definition: Constraints.h:452
Math::MatrixNd GSsi
Workspace when evaluating loop/CustomConstraint Jacobians.
Definition: Constraints.h:468
std::vector< ConstraintType > constraintType
Definition: Constraints.h:412
Math::VectorNd x
Workspace for the Lagrangian solution.
Definition: Constraints.h:461
Contains all information about the rigid body model.
Definition: Model.h:121
std::vector< CustomConstraint *> mCustomConstraints
Definition: Constraints.h:417
Math::VectorNd QDDot_0
Workspace for the default accelerations.
Definition: Constraints.h:494
std::vector< unsigned int > mCustomConstraintIndices
Definition: Constraints.h:416
Math::MatrixNd Gi
Workspace when evaluating contact Jacobians.
Definition: Constraints.h:464
std::vector< Math::SpatialVector > d_U
Workspace when applying constraint forces.
Definition: Constraints.h:511
Math::MatrixNd H
Workspace for the joint space inertia matrix.
Definition: Constraints.h:450
RBDL_DLLAPI void CalcConstraintsPositionError(Model &model, const Math::VectorNd &Q, ConstraintSet &CS, Math::VectorNd &err, bool update_kinematics)
Computes the position errors for the given ConstraintSet.
Definition: Constraints.cc:498
std::vector< Math::SpatialVector > constraintAxis
Definition: Constraints.h:429
RBDL_DLLAPI void ComputeConstraintImpulsesDirect(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
Computes contact gain by constructing and solving the full lagrangian equation.
Namespace for all structures of the RigidBodyDynamics library.
Definition: Constraints.cc:23
CustomConstraint(unsigned int constraintCount)
Definition: Constraints.h:1052
RBDL_DLLAPI void ForwardDynamicsConstraintsNullSpace(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext)
RBDL_DLLAPI void ComputeConstraintImpulsesRangeSpaceSparse(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
Resolves contact gain using SolveContactSystemRangeSpaceSparse()
Math::VectorNd d_d
Workspace when applying constraint forces.
Definition: Constraints.h:513
RBDL_DLLAPI void CalcConstraintsVelocityError(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, ConstraintSet &CS, Math::VectorNd &err, bool update_kinematics)
Computes the velocity errors for the given ConstraintSet.
Definition: Constraints.cc:666
RBDL_DLLAPI void ForwardDynamicsConstraintsDirect(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext)
Computes forward dynamics with contact by constructing and solving the full lagrangian equation...
Definition: Constraints.cc:980
RBDL_DLLAPI bool CalcAssemblyQ(Model &model, Math::VectorNd QInit, ConstraintSet &cs, Math::VectorNd &Q, const Math::VectorNd &weights, double tolerance, unsigned int max_iter)
Computes a feasible initial value of the generalized joint positions.
Definition: Constraints.cc:819
bool bound
Whether the constraint set was bound to a model (mandatory!).
Definition: Constraints.h:409
std::vector< Math::SpatialVector > f_t
Workspace for the test forces.
Definition: Constraints.h:496
Math::VectorNd gamma
Workspace of the lower part of b.
Definition: Constraints.h:454
void SetSolver(Math::LinearSolver solver)
Specifies which method should be used for solving undelying linear systems.
Definition: Constraints.h:380
LinearSolverColPivHouseholderQR
RBDL_DLLAPI void CalcConstraintsJacobian(Model &model, const Math::VectorNd &Q, ConstraintSet &CS, Math::MatrixNd &G, bool update_kinematics)
Computes the Jacobian for the given ConstraintSet.
Definition: Constraints.cc:570
Math::MatrixNd A
Workspace for the Lagrangian left-hand-side matrix.
Definition: Constraints.h:457
Math::VectorNd QDDot_t
Workspace for the test accelerations.
Definition: Constraints.h:492
RBDL_DLLAPI void ForwardDynamicsContactsKokkevis(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot)
Computes forward dynamics that accounts for active contacts in ConstraintSet.
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)
Solves the contact system by first solving for the joint accelerations and then for the constraint fo...
Definition: Constraints.cc:437
Interface to define general-purpose constraints.
Definition: Constraints.h:1048
Structure that contains both constraint information and workspace memory.
Definition: Constraints.h:266
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)
std::vector< unsigned int > mLoopConstraintIndices
Definition: Constraints.h:415
std::vector< Math::SpatialTransform > X_p
Definition: Constraints.h:427
std::vector< unsigned int > body_p
Definition: Constraints.h:425
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)
Solves the contact system by first solving for the the joint accelerations and then the contact force...
Definition: Constraints.cc:400
std::vector< Math::Vector3d > point_accel_0
Workspace for the default point accelerations.
Definition: Constraints.h:500
std::vector< unsigned int > body_s
Definition: Constraints.h:426
std::vector< Math::SpatialTransform > X_s
Definition: Constraints.h:428
RBDL_DLLAPI void CalcAssemblyQDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotInit, ConstraintSet &cs, Math::VectorNd &QDot, const Math::VectorNd &weights)
Computes a feasible initial value of the generalized joint velocities.
Definition: Constraints.cc:926
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)
Computes the terms , , and of the constrained dynamic problem and stores them in the ConstraintSet...
Definition: Constraints.cc:696
std::vector< Math::SpatialVector > d_a
Workspace for the acceleration due to the test force.
Definition: Constraints.h:505
std::vector< Math::Vector2d > baumgarteParameters
Definition: Constraints.h:431
RBDL_DLLAPI void ComputeConstraintImpulsesNullSpace(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
Resolves contact gain using SolveContactSystemNullSpace()
ConstraintSet Copy()
Copies the constraints and resets its ConstraintSet::bound flag.
Definition: Constraints.h:371
std::vector< Math::Vector3d > normal
Definition: Constraints.h:422
std::vector< Math::Vector3d > d_multdof3_u
Definition: Constraints.h:515
std::vector< Math::SpatialVector > f_ext_constraints
Workspace for the actual spatial forces.
Definition: Constraints.h:498
Eigen::HouseholderQR< Math::MatrixNd > GT_qr
Workspace for the QR decomposition of the null-space method.
Definition: Constraints.h:476
size_t size() const
Returns the number of constraints.
Definition: Constraints.h:399
std::vector< Math::Vector3d > point
Definition: Constraints.h:421
Math::LinearSolver linear_solver
Method that should be used to solve internal linear systems.
Definition: Constraints.h:407
Math::MatrixNd GSpi
Workspace when evaluating loop/CustomConstraint Jacobians.
Definition: Constraints.h:466
std::vector< unsigned int > body
Definition: Constraints.h:420
std::vector< Math::SpatialVector > d_pA
Workspace for the bias force due to the test force.
Definition: Constraints.h:503
Math::MatrixNd GSJ
Workspace when evaluating loop/CustomConstraint Jacobians.
Definition: Constraints.h:470
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)
Solves the full contact system directly, i.e. simultaneously for contact forces and joint acceleratio...
Definition: Constraints.cc:349
std::vector< Math::SpatialMatrix > d_IA
Workspace for the inertia when applying constraint forces.
Definition: Constraints.h:509