43 using SE3 = pinocchio::SE3Tpl<Scalar>;
44 using Motion = pinocchio::MotionTpl<Scalar>;
58 using pinocchio::GeometryModel;
60 if (!eigenpy::check_registration<shared_ptr<PinData>>())
61 bp::register_ptr_to_python<shared_ptr<PinData>>();
65 bp::class_<FramePlacement, bp::bases<UnaryFunction>>(
66 "FramePlacementResidual",
"Frame placement residual function.",
67 bp::init<int, int, const PinModel &, const SE3 &, pinocchio::FrameIndex>(
68 (
"self"_a,
"ndx",
"nu",
"model",
"p_ref",
"id")))
71 .def(
"getReference", &FramePlacement::getReference,
"self"_a,
72 bp::return_internal_reference<>(),
"Get the target frame in SE3.")
73 .def(
"setReference", &FramePlacement::setReference, (
"self"_a,
"p_new"),
74 "Set the target frame in SE3.");
76 bp::register_ptr_to_python<shared_ptr<FramePlacementData>>();
78 bp::class_<FramePlacementData, bp::bases<context::StageFunctionData>>(
79 "FramePlacementData",
"Data struct for FramePlacementResidual.",
81 .def_readonly(
"rMf", &FramePlacementData::rMf_,
"Frame placement error.")
82 .def_readonly(
"rJf", &FramePlacementData::rJf_)
83 .def_readonly(
"fJf", &FramePlacementData::fJf_)
84 .def_readonly(
"pin_data", &FramePlacementData::pin_data_,
85 "Pinocchio data struct.");
87 bp::class_<FrameVelocity, bp::bases<UnaryFunction>>(
88 "FrameVelocityResidual",
"Frame velocity residual function.",
89 bp::init<int, int,
const PinModel &,
const Motion &,
90 pinocchio::FrameIndex, pinocchio::ReferenceFrame>(
91 (
"self"_a,
"ndx",
"nu",
"model",
"v_ref",
"id",
"reference_frame")))
94 .def_readonly(
"pin_model", &FrameVelocity::pin_model_)
95 .def_readwrite(
"vref", &FrameVelocity::vref_)
96 .def_readwrite(
"type", &FrameVelocity::type_)
97 .def(
"getReference", &FrameVelocity::getReference,
"self"_a,
98 bp::return_internal_reference<>(),
99 deprecated_member<>(
"Get the target frame velocity."))
100 .def(
"setReference", &FrameVelocity::setReference, (
"self"_a,
"v_new"),
101 deprecated_member<>(
"Set the target frame velocity."));
103 bp::register_ptr_to_python<shared_ptr<FrameVelocityData>>();
105 bp::class_<FrameVelocityData, bp::bases<context::StageFunctionData>>(
106 "FrameVelocityData",
"Data struct for FrameVelocityResidual.",
108 .def_readonly(
"pin_data", &FrameVelocityData::pin_data_,
109 "Pinocchio data struct.");
111 bp::class_<FrameTranslation, bp::bases<UnaryFunction>>(
112 "FrameTranslationResidual",
"Frame placement residual function.",
113 bp::init<int, int,
const PinModel &,
const context::Vector3s &,
114 pinocchio::FrameIndex>(
115 (
"self"_a,
"ndx",
"nu",
"model",
"p_ref",
"id")))
118 .def(
"getReference", &FrameTranslation::getReference,
"self"_a,
119 bp::return_internal_reference<>(),
120 "Get the target frame translation.")
121 .def(
"setReference", &FrameTranslation::setReference, (
"self"_a,
"p_new"),
122 "Set the target frame translation.");
124 bp::register_ptr_to_python<shared_ptr<FrameTranslationData>>();
126 bp::class_<FrameTranslationData, bp::bases<context::StageFunctionData>>(
127 "FrameTranslationData",
"Data struct for FrameTranslationResidual.",
129 .def_readonly(
"fJf", &FrameTranslationData::fJf_)
130 .def_readonly(
"pin_data", &FrameTranslationData::pin_data_,
131 "Pinocchio data struct.");
133 bp::class_<FrameCollision, bp::bases<UnaryFunction>>(
134 "FrameCollisionResidual",
"Frame collision residual function.",
135 bp::init<int, int,
const PinModel &,
const GeometryModel &,
136 pinocchio::PairIndex>(
137 (
"self"_a,
"ndx",
"nu",
"model",
"geom_model",
"frame_pair_id")))
141 bp::register_ptr_to_python<shared_ptr<FrameCollisionData>>();
143 bp::class_<FrameCollisionData, bp::bases<context::StageFunctionData>>(
144 "FrameCollisionData",
"Data struct for FrameCollisionResidual.",
146 .def_readonly(
"pin_data", &FrameCollisionData::pin_data_,
147 "Pinocchio data struct.")
148 .def_readonly(
"geom_data", &FrameCollisionData::geom_data,
149 "Geometry data struct.");
154 const ConstVectorRef &v,
const ConstMatrixRef &actMatrix,
157 long nu = actMatrix.cols();
159 for (
size_t k = 0; k < constraint_models.size(); ++k) {
160 d += (int)constraint_models[k].size();
162 context::VectorXs out(nu + d);
164 model, data, q, v, actMatrix, constraint_models, constraint_datas, out);
166 return bp::make_tuple((context::VectorXs)out.head(nu),
167 (context::VectorXs)out.tail(d));
ManifoldAbstractTpl< Scalar > Manifold
pinocchio::ModelTpl< Scalar, Options > PinModel
StageFunctionTpl< Scalar > StageFunction
UnaryFunctionTpl< Scalar > UnaryFunction
pinocchio::DataTpl< Scalar, Options > PinData
void underactuatedConstrainedInverseDynamics(const ModelTpl< Scalar, Options > &model, DataTpl< Scalar, Options > &data, const Eigen::MatrixBase< ConfigType > &q, Eigen::MatrixBase< VelType > const &v, const Eigen::MatrixBase< MatrixType > &actMatrix, const StdVectorEigenAligned< RigidConstraintModelTpl< Scalar, Options > > &constraint_models, StdVectorEigenAligned< RigidConstraintDataTpl< Scalar, Options > > &constraint_datas, const Eigen::MatrixBase< OutType > &res_)