pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
constraint-model-base.hpp
1//
2// Copyright (c) 2023 INRIA
3//
4
5#ifndef __pinocchio_algorithm_constraint_model_base_hpp__
6#define __pinocchio_algorithm_constraint_model_base_hpp__
7
8#include "pinocchio/multibody/model.hpp"
9#include "pinocchio/algorithm/fwd.hpp"
10
11namespace pinocchio
12{
13
14 template<class Derived>
16 {
17 typedef typename traits<Derived>::Scalar Scalar;
18 enum
19 {
21 };
22 typedef typename traits<Derived>::ConstraintData ConstraintData;
23
24 typedef Eigen::Matrix<bool, Eigen::Dynamic, 1, Options> BooleanVector;
25 // typedef Eigen::Matrix<Eigen::DenseIndex,Eigen::Dynamic,1,Options> IndexVector;
26 typedef std::vector<Eigen::DenseIndex> IndexVector;
27
28 Derived & derived()
29 {
30 return static_cast<Derived &>(*this);
31 }
32 const Derived & derived() const
33 {
34 return static_cast<const Derived &>(*this);
35 }
36
37 template<typename NewScalar>
38 typename CastType<NewScalar, Derived>::type cast() const
39 {
40 return derived().template cast<NewScalar>();
41 }
42
45 template<int Options, template<typename, int> class JointCollectionTpl>
46 void calc(
49 ConstraintData & cdata) const
50 {
51 derived().calc(model, data, cdata);
52 }
53
54 template<typename JacobianMatrix, int Options, template<typename, int> class JointCollectionTpl>
55 void jacobian(
58 ConstraintData & cdata,
59 const Eigen::MatrixBase<JacobianMatrix> & jacobian_matrix) const
60 {
61 derived().jacobian(model, data, cdata, jacobian_matrix.const_cast_derived());
62 }
63
64 // Attributes common to all constraints
65
67 std::string name;
68
70 BooleanVector colwise_sparsity;
71
74
75 template<typename OtherDerived>
76 bool operator==(const ConstraintModelBase<OtherDerived> & other) const
77 {
78 return name == other.name && colwise_sparsity == other.colwise_sparsity
79 && colwise_span_indexes == other.colwise_span_indexes;
80 }
81
82 template<typename OtherDerived>
84 {
85 name = other.name;
86 colwise_sparsity = other.colwise_sparsity;
87 colwise_span_indexes = other.colwise_span_indexes;
88
89 return *this;
90 }
91
92 ConstraintData createData() const
93 {
94 return derived().createData();
95 }
96
97 protected:
98 template<int Options, template<typename, int> class JointCollectionTpl>
99 ConstraintModelBase(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
100 : colwise_sparsity(model.nv)
101 {
102 static const bool default_sparsity_value = false;
103 colwise_sparsity.fill(default_sparsity_value);
104 }
105
108 {
109 }
110
111 ConstraintModelBase & base()
112 {
113 return *this;
114 }
115 const ConstraintModelBase & base() const
116 {
117 return *this;
118 }
119 };
120
121} // namespace pinocchio
122
123#endif // ifndef __pinocchio_algorithm_constraint_model_base_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition fwd.hpp:99
BooleanVector colwise_sparsity
Sparsity pattern associated to the constraint;.
IndexVector colwise_span_indexes
Indexes of the columns spanned by the constraints.
std::string name
Name of the constraint.
void calc(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, ConstraintData &cdata) const
Evaluate the constraint values at the current state given by data and store the results in cdata.
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72