1 |
|
|
// |
2 |
|
|
// Copyright (c) 2019-2020 INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/bindings/python/algorithm/algorithms.hpp" |
6 |
|
|
#include "pinocchio/bindings/python/utils/list.hpp" |
7 |
|
|
#include "pinocchio/bindings/python/utils/std-vector.hpp" |
8 |
|
|
#include "pinocchio/algorithm/model.hpp" |
9 |
|
|
|
10 |
|
|
namespace pinocchio |
11 |
|
|
{ |
12 |
|
|
namespace python |
13 |
|
|
{ |
14 |
|
|
|
15 |
|
|
namespace bp = boost::python; |
16 |
|
|
|
17 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
18 |
|
1 |
bp::tuple appendModel_proxy(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA, |
19 |
|
|
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB, |
20 |
|
|
const GeometryModel & geomModelA, |
21 |
|
|
const GeometryModel & geomModelB, |
22 |
|
|
const FrameIndex frameInModelA, |
23 |
|
|
const SE3Tpl<Scalar,Options> & aMb) |
24 |
|
|
{ |
25 |
|
|
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
26 |
✓✗ |
2 |
Model model; |
27 |
✓✗ |
2 |
GeometryModel geom_model; |
28 |
|
|
|
29 |
✓✗ |
1 |
appendModel(modelA,modelB,geomModelA,geomModelB,frameInModelA,aMb,model,geom_model); |
30 |
|
|
|
31 |
✓✗ |
2 |
return bp::make_tuple(model,geom_model); |
32 |
|
|
} |
33 |
|
|
|
34 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
35 |
|
|
bp::tuple |
36 |
|
1 |
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
37 |
|
|
const GeometryModel & geom_model, |
38 |
|
|
const std::vector<JointIndex> & list_of_joints_to_lock, |
39 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration) |
40 |
|
|
{ |
41 |
|
|
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
42 |
✓✗✓✗
|
2 |
Model reduced_model; GeometryModel reduced_geom_model; |
43 |
|
|
|
44 |
✓✗ |
1 |
buildReducedModel(model,geom_model,list_of_joints_to_lock, |
45 |
|
|
reference_configuration,reduced_model,reduced_geom_model); |
46 |
|
|
|
47 |
✓✗ |
2 |
return bp::make_tuple(reduced_model,reduced_geom_model); |
48 |
|
|
} |
49 |
|
|
|
50 |
|
|
template <typename Scalar, int Options, |
51 |
|
|
template <typename, int> class JointCollectionTpl, |
52 |
|
|
typename ConfigVectorType> |
53 |
|
2 |
bp::tuple buildReducedModel(const ModelTpl<Scalar, Options, JointCollectionTpl> &model, |
54 |
|
|
const std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > &list_of_geom_models, |
55 |
|
|
const std::vector<JointIndex> &list_of_joints_to_lock, |
56 |
|
|
const Eigen::MatrixBase<ConfigVectorType> &reference_configuration) |
57 |
|
|
{ |
58 |
|
|
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; |
59 |
✓✗ |
4 |
std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > reduced_geom_models; |
60 |
✓✗ |
4 |
Model reduced_model; |
61 |
✓✗ |
2 |
buildReducedModel(model, list_of_geom_models, list_of_joints_to_lock, |
62 |
|
|
reference_configuration, reduced_model, |
63 |
|
|
reduced_geom_models); |
64 |
✓✗ |
4 |
return bp::make_tuple(reduced_model, reduced_geom_models); |
65 |
|
|
} |
66 |
|
|
|
67 |
|
19 |
void exposeModelAlgo() |
68 |
|
|
{ |
69 |
|
|
using namespace Eigen; |
70 |
|
|
|
71 |
|
|
typedef std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > GeometryModelVector; |
72 |
✓✗✓✗ ✓✗ |
19 |
StdVectorPythonVisitor<GeometryModel,GeometryModelVector::allocator_type>::expose("StdVec_GeometryModel"); |
73 |
|
|
|
74 |
✓✗ |
19 |
bp::def("appendModel", |
75 |
|
|
(Model (*)(const Model &, const Model &, const FrameIndex, const SE3 &))&appendModel<double,0,JointCollectionDefaultTpl>, |
76 |
|
38 |
bp::args("modelA","modelB","frame_in_modelA","aMb"), |
77 |
|
|
"Append a child model into a parent model, after a specific frame given by its index.\n\n" |
78 |
|
|
"Parameters:\n" |
79 |
|
|
"\tmodelA: the parent model\n" |
80 |
|
|
"\tmodelB: the child model\n" |
81 |
|
|
"\tframeInModelA: index of the frame of modelA where to append modelB\n" |
82 |
|
|
"\taMb: pose of modelB universe joint (index 0) in frameInModelA\n"); |
83 |
|
|
|
84 |
✓✗ |
19 |
bp::def("appendModel", |
85 |
|
|
&appendModel_proxy<double,0,JointCollectionDefaultTpl>, |
86 |
|
38 |
bp::args("modelA","modelB","geomModelA", "geomModelB","frame_in_modelA","aMb"), |
87 |
|
|
"Append a child (geometry) model into a parent (geometry) model, after a specific frame given by its index.\n\n" |
88 |
|
|
"Parameters:\n" |
89 |
|
|
"\tmodelA: the parent model\n" |
90 |
|
|
"\tmodelB: the child model\n" |
91 |
|
|
"\tgeomModelA: the parent geometry model\n" |
92 |
|
|
"\tgeomModelB: the child geometry model\n" |
93 |
|
|
"\tframeInModelA: index of the frame of modelA where to append modelB\n" |
94 |
|
|
"\taMb: pose of modelB universe joint (index 0) in frameInModelA\n"); |
95 |
|
|
|
96 |
✓✗ |
19 |
bp::def("buildReducedModel", |
97 |
|
|
(Model (*)(const Model &, const std::vector<JointIndex> &, const Eigen::MatrixBase<VectorXd> &)) |
98 |
|
|
&pinocchio::buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>, |
99 |
|
38 |
bp::args("model", |
100 |
|
|
"list_of_joints_to_lock", |
101 |
|
|
"reference_configuration"), |
102 |
|
|
"Build a reduce model from a given input model and a list of joint to lock.\n\n" |
103 |
|
|
"Parameters:\n" |
104 |
|
|
"\tmodel: input kinematic modell to reduce\n" |
105 |
|
|
"\tlist_of_joints_to_lock: list of joint indexes to lock\n" |
106 |
|
|
"\treference_configuration: reference configuration to compute the placement of the lock joints\n"); |
107 |
|
|
|
108 |
✓✗ |
19 |
bp::def("buildReducedModel", |
109 |
|
|
(bp::tuple (*)(const Model &, |
110 |
|
|
const GeometryModel &, |
111 |
|
|
const std::vector<JointIndex> &, |
112 |
|
|
const Eigen::MatrixBase<VectorXd> &)) |
113 |
|
|
&buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>, |
114 |
|
38 |
bp::args("model", |
115 |
|
|
"geom_model", |
116 |
|
|
"list_of_joints_to_lock", |
117 |
|
|
"reference_configuration"), |
118 |
|
|
"Build a reduced model and a reduced geometry model from a given input model," |
119 |
|
|
"an input geometry model and a list of joints to lock.\n\n" |
120 |
|
|
"Parameters:\n" |
121 |
|
|
"\tmodel: input kinematic model to reduce\n" |
122 |
|
|
"\tgeom_model: input geometry model to reduce\n" |
123 |
|
|
"\tlist_of_joints_to_lock: list of joint indexes to lock\n" |
124 |
|
|
"\treference_configuration: reference configuration to compute the placement of the locked joints\n"); |
125 |
|
|
|
126 |
✓✗ |
19 |
bp::def("buildReducedModel", |
127 |
|
|
(bp::tuple(*)(const Model &, |
128 |
|
|
const std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > &, |
129 |
|
|
const std::vector<JointIndex> &, |
130 |
|
|
const Eigen::MatrixBase<VectorXd> &)) |
131 |
|
|
buildReducedModel<double, 0, JointCollectionDefaultTpl, VectorXd>, |
132 |
|
38 |
bp::args("model", "list_of_geom_models", "list_of_joints_to_lock", |
133 |
|
|
"reference_configuration"), |
134 |
|
|
"Build a reduced model and the related reduced geometry models from a given " |
135 |
|
|
"input model," |
136 |
|
|
"a list of input geometry models and a list of joints to lock.\n\n" |
137 |
|
|
"Parameters:\n" |
138 |
|
|
"\tmodel: input kinematic model to reduce\n" |
139 |
|
|
"\tlist_of_geom_models: input geometry models to reduce\n" |
140 |
|
|
"\tlist_of_joints_to_lock: list of joint indexes to lock\n" |
141 |
|
|
"\treference_configuration: reference configuration to compute the " |
142 |
|
|
"placement of the locked joints\n"); |
143 |
|
19 |
} |
144 |
|
|
|
145 |
|
|
} // namespace python |
146 |
|
|
} // namespace pinocchio |