5#ifndef __pinocchio_python_algorithm_contact_info_hpp__
6#define __pinocchio_python_algorithm_contact_info_hpp__
8#include <eigenpy/memory.hpp>
10#include "pinocchio/algorithm/contact-info.hpp"
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"
21 namespace bp = boost::python;
23 template<
typename BaumgarteCorrectorParameters>
24 struct BaumgarteCorrectorParametersPythonVisitor
25 :
public boost::python::def_visitor<
26 BaumgarteCorrectorParametersPythonVisitor<BaumgarteCorrectorParameters>>
28 typedef typename BaumgarteCorrectorParameters::Scalar Scalar;
29 typedef typename BaumgarteCorrectorParameters::Vector6Max Vector6Max;
30 typedef BaumgarteCorrectorParameters Self;
33 template<
class PyClass>
34 void visit(PyClass & cl)
const
36 cl.def(bp::init<int>(bp::args(
"self",
"size"),
"Default constructor."))
38 .def_readwrite(
"Kp", &Self::Kp,
"Proportional corrector value.")
39 .def_readwrite(
"Kd", &Self::Kd,
"Damping corrector value.")
41 .def(CastVisitor<Self>())
42 .def(ExposeConstructorByCastVisitor<
43 Self, ::pinocchio::context::RigidConstraintModel::BaumgarteCorrectorParameters>())
49 eigenpy::enableEigenPySpecific<Vector6Max>();
50 bp::class_<BaumgarteCorrectorParameters>(
51 "BaumgarteCorrectorParameters",
"Paramaters of the Baumgarte Corrector.", bp::no_init)
52 .def(BaumgarteCorrectorParametersPythonVisitor());
56 template<
typename Rig
idConstra
intModel>
57 struct RigidConstraintModelPythonVisitor
58 :
public boost::python::def_visitor<RigidConstraintModelPythonVisitor<RigidConstraintModel>>
60 typedef typename RigidConstraintModel::Scalar Scalar;
61 typedef typename RigidConstraintModel::SE3 SE3;
62 typedef RigidConstraintModel Self;
63 typedef typename RigidConstraintModel::ContactData ContactData;
65 typename RigidConstraintModel::BaumgarteCorrectorParameters BaumgarteCorrectorParameters;
67 typedef ModelTpl<Scalar, RigidConstraintModel::Options, JointCollectionDefaultTpl> Model;
70 template<
class PyClass>
71 void visit(PyClass & cl)
const
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."))
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 "
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.")
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.")
122 .def(
"size", &RigidConstraintModel::size,
"Size of the constraint")
125 "createData", &RigidConstraintModelPythonVisitor::createData,
126 "Create a Data object for the given model.")
132 bp::class_<RigidConstraintModel>(
133 "RigidConstraintModel",
"Rigid contact model for contact dynamic algorithms.",
135 .def(RigidConstraintModelPythonVisitor())
136 .def(CastVisitor<RigidConstraintModel>())
137 .def(ExposeConstructorByCastVisitor<
138 RigidConstraintModel, ::pinocchio::context::RigidConstraintModel>());
140 BaumgarteCorrectorParametersPythonVisitor<BaumgarteCorrectorParameters>::expose();
143 static ContactData createData(
const Self & self)
145 return ContactData(self);
149 template<
typename Rig
idConstra
intData>
150 struct RigidConstraintDataPythonVisitor
151 :
public boost::python::def_visitor<RigidConstraintDataPythonVisitor<RigidConstraintData>>
153 typedef typename RigidConstraintData::Scalar Scalar;
154 typedef RigidConstraintData Self;
155 typedef typename RigidConstraintData::ContactModel ContactModel;
158 template<
class PyClass>
159 void visit(PyClass & cl)
const
161 cl.def(bp::init<const ContactModel &>(
162 bp::args(
"self",
"contact_model"),
"Default constructor."))
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 "
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).")
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.")
216 bp::class_<RigidConstraintData>(
217 "RigidConstraintData",
218 "Rigid constraint data associated to a "
219 "RigidConstraintModel for contact dynamic algorithms.",
221 .def(RigidConstraintDataPythonVisitor());
223 typedef typename RigidConstraintData::VectorOfMatrix6 VectorOfMatrix6;
224 StdVectorPythonVisitor<VectorOfMatrix6, true>::expose(
"StdVec_Matrix6_");
Main pinocchio namespace.
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.