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