pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
contact-info.hpp
1//
2// Copyright (c) 2019-2022 INRIA
3//
4
5#ifndef __pinocchio_python_algorithm_contact_info_hpp__
6#define __pinocchio_python_algorithm_contact_info_hpp__
7
8#include <eigenpy/memory.hpp>
9
10#include "pinocchio/algorithm/contact-info.hpp"
11
12#include "pinocchio/bindings/python/utils/cast.hpp"
13#include "pinocchio/bindings/python/utils/macros.hpp"
14#include "pinocchio/bindings/python/utils/comparable.hpp"
15#include "pinocchio/bindings/python/utils/std-vector.hpp"
16
17namespace pinocchio
18{
19 namespace python
20 {
21 namespace bp = boost::python;
22
23 template<typename BaumgarteCorrectorParameters>
24 struct BaumgarteCorrectorParametersPythonVisitor
25 : public boost::python::def_visitor<
26 BaumgarteCorrectorParametersPythonVisitor<BaumgarteCorrectorParameters>>
27 {
28 typedef typename BaumgarteCorrectorParameters::Scalar Scalar;
29 typedef typename BaumgarteCorrectorParameters::Vector6Max Vector6Max;
30 typedef BaumgarteCorrectorParameters Self;
31
32 public:
33 template<class PyClass>
34 void visit(PyClass & cl) const
35 {
36 cl.def(bp::init<int>(bp::args("self", "size"), "Default constructor."))
37
38 .def_readwrite("Kp", &Self::Kp, "Proportional corrector value.")
39 .def_readwrite("Kd", &Self::Kd, "Damping corrector value.")
40
41 .def(CastVisitor<Self>())
42 .def(ExposeConstructorByCastVisitor<
43 Self, ::pinocchio::context::RigidConstraintModel::BaumgarteCorrectorParameters>())
44 .def(ComparableVisitor<Self, pinocchio::is_floating_point<Scalar>::value>());
45 }
46
47 static void expose()
48 {
49 eigenpy::enableEigenPySpecific<Vector6Max>();
50 bp::class_<BaumgarteCorrectorParameters>(
51 "BaumgarteCorrectorParameters", "Paramaters of the Baumgarte Corrector.", bp::no_init)
52 .def(BaumgarteCorrectorParametersPythonVisitor());
53 }
54 };
55
56 template<typename RigidConstraintModel>
57 struct RigidConstraintModelPythonVisitor
58 : public boost::python::def_visitor<RigidConstraintModelPythonVisitor<RigidConstraintModel>>
59 {
60 typedef typename RigidConstraintModel::Scalar Scalar;
61 typedef typename RigidConstraintModel::SE3 SE3;
62 typedef RigidConstraintModel Self;
63 typedef typename RigidConstraintModel::ContactData ContactData;
64 typedef
65 typename RigidConstraintModel::BaumgarteCorrectorParameters BaumgarteCorrectorParameters;
66
67 typedef ModelTpl<Scalar, RigidConstraintModel::Options, JointCollectionDefaultTpl> Model;
68
69 public:
70 template<class PyClass>
71 void visit(PyClass & cl) const
72 {
73 cl
74 // .def(bp::init<>(bp::arg("self"),
75 // "Default constructor."))
76 .def(bp::init<
77 ContactType, const Model &, JointIndex, const SE3 &, JointIndex, const SE3 &,
78 bp::optional<ReferenceFrame>>(
79 (bp::arg("self"), bp::arg("contact_type"), bp::arg("model"), bp::arg("joint1_id"),
80 bp::arg("joint1_placement"), bp::arg("joint2_id"), bp::arg("joint2_placement"),
81 bp::arg("reference_frame")),
82 "Contructor from a given ContactType, joint index and placement for the two joints "
83 "implied in the constraint."))
84 .def(bp::init<
85 ContactType, const Model &, JointIndex, const SE3 &, bp::optional<ReferenceFrame>>(
86 (bp::arg("self"), bp::arg("contact_type"), bp::arg("model"), bp::arg("joint1_id"),
87 bp::arg("joint1_placement"), bp::arg("reference_frame")),
88 "Contructor from a given ContactType, joint index and placement only for the first "
89 "joint implied in the constraint."))
90 .def(bp::init<ContactType, const Model &, JointIndex, bp::optional<ReferenceFrame>>(
91 (bp::arg("self"), bp::arg("contact_type"), bp::arg("model"), bp::arg("joint1_id"),
92 bp::arg("reference_frame")),
93 "Contructor from a given ContactType and joint index. The base joint is taken as 0 in "
94 "the constraint."))
95 .PINOCCHIO_ADD_PROPERTY(Self, name, "Name of the contact.")
96 .PINOCCHIO_ADD_PROPERTY(Self, type, "Type of the contact.")
97 .PINOCCHIO_ADD_PROPERTY(Self, joint1_id, "Index of first parent joint in the model tree.")
98 .PINOCCHIO_ADD_PROPERTY(
99 Self, joint2_id, "Index of second parent joint in the model tree.")
100 .PINOCCHIO_ADD_PROPERTY(
101 Self, joint1_placement, "Relative placement with respect to the frame of joint1.")
102 .PINOCCHIO_ADD_PROPERTY(
103 Self, joint2_placement, "Relative placement with respect to the frame of joint2.")
104 .PINOCCHIO_ADD_PROPERTY(
105 Self, reference_frame,
106 "Reference frame where the constraint is expressed (WORLD, "
107 "LOCAL_WORLD_ALIGNED or LOCAL).")
108 .PINOCCHIO_ADD_PROPERTY(Self, desired_contact_placement, "Desired contact placement.")
109 .PINOCCHIO_ADD_PROPERTY(
110 Self, desired_contact_velocity, "Desired contact spatial velocity.")
111 .PINOCCHIO_ADD_PROPERTY(
112 Self, desired_contact_acceleration, "Desired contact spatial acceleration.")
113 .PINOCCHIO_ADD_PROPERTY(Self, corrector, "Corrector parameters.")
114
115 .PINOCCHIO_ADD_PROPERTY(
116 Self, colwise_joint1_sparsity, "Sparsity pattern associated to joint 1.")
117 .PINOCCHIO_ADD_PROPERTY(
118 Self, colwise_joint2_sparsity, "Sparsity pattern associated to joint 2.")
119 .PINOCCHIO_ADD_PROPERTY(
120 Self, colwise_span_indexes, "Indexes of the columns spanned by the constraints.")
121
122 .def("size", &RigidConstraintModel::size, "Size of the constraint")
123
124 .def(
125 "createData", &RigidConstraintModelPythonVisitor::createData,
126 "Create a Data object for the given model.")
127 .def(ComparableVisitor<Self, pinocchio::is_floating_point<Scalar>::value>());
128 }
129
130 static void expose()
131 {
132 bp::class_<RigidConstraintModel>(
133 "RigidConstraintModel", "Rigid contact model for contact dynamic algorithms.",
134 bp::no_init)
135 .def(RigidConstraintModelPythonVisitor())
136 .def(CastVisitor<RigidConstraintModel>())
137 .def(ExposeConstructorByCastVisitor<
138 RigidConstraintModel, ::pinocchio::context::RigidConstraintModel>());
139
140 BaumgarteCorrectorParametersPythonVisitor<BaumgarteCorrectorParameters>::expose();
141 }
142
143 static ContactData createData(const Self & self)
144 {
145 return ContactData(self);
146 }
147 };
148
149 template<typename RigidConstraintData>
150 struct RigidConstraintDataPythonVisitor
151 : public boost::python::def_visitor<RigidConstraintDataPythonVisitor<RigidConstraintData>>
152 {
153 typedef typename RigidConstraintData::Scalar Scalar;
154 typedef RigidConstraintData Self;
155 typedef typename RigidConstraintData::ContactModel ContactModel;
156
157 public:
158 template<class PyClass>
159 void visit(PyClass & cl) const
160 {
161 cl.def(bp::init<const ContactModel &>(
162 bp::args("self", "contact_model"), "Default constructor."))
163
164 .PINOCCHIO_ADD_PROPERTY(Self, contact_force, "Constraint force.")
165 .PINOCCHIO_ADD_PROPERTY(
166 Self, oMc1, "Placement of the constraint frame 1 with respect to the WORLD frame.")
167 .PINOCCHIO_ADD_PROPERTY(
168 Self, oMc2, "Placement of the constraint frame 2 with respect to the WORLD frame.")
169 .PINOCCHIO_ADD_PROPERTY(Self, c1Mc2, "Relative displacement between the two frames.")
170 .PINOCCHIO_ADD_PROPERTY(
171 Self, contact_placement_error,
172 "Current contact placement error between the two contact Frames.\n"
173 "This corresponds to the relative placement between the two contact Frames seen as a "
174 "Motion error.")
175 .PINOCCHIO_ADD_PROPERTY(
176 Self, contact1_velocity, "Current contact Spatial velocity of the constraint 1.")
177 .PINOCCHIO_ADD_PROPERTY(
178 Self, contact2_velocity, "Current contact Spatial velocity of the constraint 2.")
179 .PINOCCHIO_ADD_PROPERTY(
180 Self, contact_velocity_error,
181 "Current contact Spatial velocity error between the two contact Frames.\n"
182 "This corresponds to the relative velocity between the two contact Frames.")
183 .PINOCCHIO_ADD_PROPERTY(
184 Self, contact_acceleration, "Current contact Spatial acceleration.")
185 .PINOCCHIO_ADD_PROPERTY(
186 Self, contact_acceleration_desired, "Desired contact acceleration.")
187 .PINOCCHIO_ADD_PROPERTY(
188 Self, contact_acceleration_error,
189 "Current contact spatial error (due to the integration step).")
190 .PINOCCHIO_ADD_PROPERTY(
191 Self, contact1_acceleration_drift,
192 "Current contact drift acceleration (acceleration only due to "
193 "the Coriolis and centrifugal effects) of the contact frame 1.")
194 .PINOCCHIO_ADD_PROPERTY(
195 Self, contact2_acceleration_drift,
196 "Current contact drift acceleration (acceleration only due to "
197 "the Coriolis and centrifugal effects) of the contact frame 2.")
198 .PINOCCHIO_ADD_PROPERTY(
199 Self, contact_acceleration_deviation,
200 "Contact deviation from the reference acceleration (a.k.a the error).")
201
202 .PINOCCHIO_ADD_PROPERTY(
203 Self, extended_motion_propagators_joint1,
204 "Extended force/motion propagators for joint 1.")
205 .PINOCCHIO_ADD_PROPERTY(
206 Self, lambdas_joint1, "Extended force/motion propagators for joint 1.")
207 .PINOCCHIO_ADD_PROPERTY(
208 Self, extended_motion_propagators_joint2,
209 "Extended force/motion propagators for joint 2.")
210
211 .def(ComparableVisitor<Self, pinocchio::is_floating_point<Scalar>::value>());
212 }
213
214 static void expose()
215 {
216 bp::class_<RigidConstraintData>(
217 "RigidConstraintData",
218 "Rigid constraint data associated to a "
219 "RigidConstraintModel for contact dynamic algorithms.",
220 bp::no_init)
221 .def(RigidConstraintDataPythonVisitor());
222
223 typedef typename RigidConstraintData::VectorOfMatrix6 VectorOfMatrix6;
224 StdVectorPythonVisitor<VectorOfMatrix6, true>::expose("StdVec_Matrix6_");
225 }
226 };
227
228 } // namespace python
229} // namespace pinocchio
230
231#endif // ifndef __pinocchio_python_algorithm_contact_info_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
ContactType
&#160;
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.