robot-wrapper.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018 CNRS
3 //
4 // This file is part of tsid
5 // tsid is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 // tsid is distributed in the hope that it will be
10 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 // General Lesser Public License for more details. You should have
13 // received a copy of the GNU Lesser General Public License along with
14 // tsid If not, see
15 // <http://www.gnu.org/licenses/>.
16 //
17 
18 #ifndef __tsid_python_robot_wrapper_hpp__
19 #define __tsid_python_robot_wrapper_hpp__
20 
22 
24 
25 namespace tsid
26 {
27  namespace python
28  {
29  namespace bp = boost::python;
30 
31  template<typename Robot>
33  : public boost::python::def_visitor< RobotPythonVisitor<Robot> >
34  {
35  typedef std::vector<std::string> std_vec;
36  typedef Eigen::Matrix<double,3,Eigen::Dynamic> Matrix3x;
37 
38  template<class PyClass >
39 
40  void visit(PyClass& cl) const
41  {
42  cl
43  .def(bp::init<std::string, std_vec, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("verbose")), "Default constructor without RootJoint."))
44  .def(bp::init<std::string, std_vec, pinocchio::JointModelVariant &, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("roottype"), bp::arg("verbose")), "Default constructor with RootJoint."))
45  .def(bp::init<pinocchio::Model, bool>((bp::arg("Pinocchio Model"), bp::arg("verbose")), "Default constructor from pinocchio model without RootJoint."))
46  .def(bp::init<pinocchio::Model, robots::RobotWrapper::RootJointType, bool>((bp::arg("Pinocchio Model"), bp::arg("rootJoint"), bp::arg("verbose")), "Default constructor from pinocchio model with RootJoint."))
47  .def("__init__",bp::make_constructor(RobotPythonVisitor<Robot> ::makeClass))
48  .add_property("nq", &Robot::nq)
49  .add_property("nv", &Robot::nv)
50  .add_property("na", &Robot::na)
51  .add_property("nq_actuated", &Robot::nq_actuated)
52  .add_property("is_fixed_base", &Robot::is_fixed_base)
53 
54  .def("model", &RobotPythonVisitor::model)
55  .def("data", &RobotPythonVisitor::data)
56 
57  .add_property("rotor_inertias", &RobotPythonVisitor::rotor_inertias)
58  .add_property("gear_ratios", &RobotPythonVisitor::gear_ratios)
59  .def("set_rotor_inertias", &RobotPythonVisitor::set_rotor_inertias, bp::arg("inertia vector"))
60  .def("set_gear_ratios", &RobotPythonVisitor::set_gear_ratios, bp::arg("gear ratio vector"))
61 
62  .def("computeAllTerms", &RobotPythonVisitor::computeAllTerms, bp::args("data", "q", "v"), "compute all dynamics")
63  .def("com", &RobotPythonVisitor::com, bp::arg("data"))
64  .def("com_vel", &RobotPythonVisitor::com_vel, bp::arg("data"))
65  .def("com_acc", &RobotPythonVisitor::com_acc, bp::arg("data"))
66  .def("Jcom", &RobotPythonVisitor::Jcom, bp::arg("data"))
67  .def("mass", &RobotPythonVisitor::mass, bp::arg("data"))
68  .def("nonLinearEffect", &RobotPythonVisitor::nonLinearEffects, bp::arg("data"))
69  .def("position", &RobotPythonVisitor::position, bp::args("data", "index"))
70  .def("velocity", &RobotPythonVisitor::velocity, bp::args("data", "index"))
71  .def("acceleration", &RobotPythonVisitor::acceleration, bp::args("data", "index"))
72 
73  .def("framePosition", &RobotPythonVisitor::framePosition, bp::args("data", "index"))
74  .def("frameVelocity", &RobotPythonVisitor::frameVelocity, bp::args("data", "index"))
75  .def("frameAcceleration", &RobotPythonVisitor::frameAcceleration, bp::args("data", "index"))
76  .def("frameClassicAcceleration", &RobotPythonVisitor::frameClassicAcceleration, bp::args("data", "index"))
77  .def("frameVelocityWorldOriented", &RobotPythonVisitor::frameVelocityWorldOriented, bp::args("data", "index"))
78  .def("frameAccelerationWorldOriented", &RobotPythonVisitor::frameAccelerationWorldOriented, bp::args("data", "index"))
79  .def("frameClassicAccelerationWorldOriented", &RobotPythonVisitor::frameClassicAccelerationWorldOriented, bp::args("data", "index"))
80  .def("angularMomentumTimeVariation", &RobotPythonVisitor::angularMomentumTimeVariation, bp::arg("data"))
81  .def("setGravity", &RobotPythonVisitor::setGravity, bp::arg("gravity"))
82  ;
83  }
84 
85  static boost::shared_ptr<Robot> makeClass(const std::string &filename,
86  const std::vector<std::string>
87  &stdvec,
88  bp::object & bpObject,
89  bool verbose)
90  {
91  pinocchio::JointModelFreeFlyer root_joint =
92  bp::extract<pinocchio::JointModelFreeFlyer>(bpObject)();
93  boost::shared_ptr<Robot> p(new tsid::robots::RobotWrapper(filename,
94  stdvec,
95  root_joint,
96  verbose));
97  return p;
98  }
99 
100  static pinocchio::Model model (const Robot & self){
101  return self.model();
102  }
103  static pinocchio::Data data(const Robot & self){
104  pinocchio::Data data(self.model());
105  return data;
106  }
107  static Eigen::VectorXd rotor_inertias(const Robot & self){
108  return self.rotor_inertias();
109  }
110  static Eigen::VectorXd gear_ratios(const Robot & self){
111  return self.gear_ratios();
112  }
113  static bool set_rotor_inertias(Robot & self, Eigen::VectorXd & rotor_inertias){
114  return self.rotor_inertias(rotor_inertias);
115  }
116  static bool set_gear_ratios(Robot & self, Eigen::VectorXd & gear_ratios){
117  return self.gear_ratios(gear_ratios);
118  }
119 
120  static Eigen::Vector3d com (const Robot & self, const pinocchio::Data & data){
121  return self.com(data);
122  }
123  static Eigen::Vector3d com_vel (const Robot & self, const pinocchio::Data & data){
124  return self.com_vel(data);
125  }
126  static Eigen::Vector3d com_acc (const Robot & self, const pinocchio::Data & data){
127  return self.com_acc(data);
128  }
129  static Matrix3x Jcom (const Robot & self, const pinocchio::Data & data){
130  return self.Jcom(data);
131  }
132  static void computeAllTerms (const Robot & self, pinocchio::Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v){
133  self.computeAllTerms(data, q, v);
134  }
135  static Eigen::MatrixXd mass (Robot & self, pinocchio::Data & data){
136  return self.mass(data);
137  }
138  static Eigen::VectorXd nonLinearEffects(const Robot & self, const pinocchio::Data & data){
139  return self.nonLinearEffects(data);
140  }
141  static pinocchio::SE3 position(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::JointIndex & index){
142  return self.position(data, index);
143  }
144  static pinocchio::Motion velocity(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::JointIndex & index){
145  return self.velocity(data, index);
146  }
147  static pinocchio::Motion acceleration(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::JointIndex & index){
148  return self.acceleration(data, index);
149  }
150  static pinocchio::SE3 framePosition(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
151  return self.framePosition(data, index);
152  }
153  static pinocchio::Motion frameVelocity(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
154  return self.frameVelocity(data, index);
155  }
156  static pinocchio::Motion frameAcceleration(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
157  return self.frameAcceleration(data, index);
158  }
159  static pinocchio::Motion frameClassicAcceleration(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
160  return self.frameClassicAcceleration(data, index);
161  }
162  static pinocchio::Motion frameVelocityWorldOriented(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
163  return self.frameVelocityWorldOriented(data, index);
164  }
165  static pinocchio::Motion frameAccelerationWorldOriented(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
166  return self.frameAccelerationWorldOriented(data, index);
167  }
168  static pinocchio::Motion frameClassicAccelerationWorldOriented(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
169  return self.frameClassicAccelerationWorldOriented(data, index);
170  }
171  static Eigen::Vector3d angularMomentumTimeVariation(const Robot & self, const pinocchio::Data & data){
172  return self.angularMomentumTimeVariation(data);
173  }
174  static void setGravity(Robot & self, const pinocchio::Motion & gravity){
175  return self.setGravity(gravity);
176  }
177  static void expose(const std::string & class_name)
178  {
179  std::string doc = "Robot Wrapper info.";
180  bp::class_<Robot>(class_name.c_str(),
181  doc.c_str(),
182  bp::no_init)
184  ;
185  bp::enum_<robots::RobotWrapper::RootJointType>("RootJointType")
186  .value("FIXED_BASE_SYSTEM", robots::RobotWrapper::FIXED_BASE_SYSTEM)
187  .value("FLOATING_BASE_SYSTEM", robots::RobotWrapper::FLOATING_BASE_SYSTEM)
188  .export_values()
189  ;
190  }
191  };
192  }
193 }
194 
195 
196 #endif // ifndef __tsid_python_robot_wrapper_hpp__
tsid::python::RobotPythonVisitor::makeClass
static boost::shared_ptr< Robot > makeClass(const std::string &filename, const std::vector< std::string > &stdvec, bp::object &bpObject, bool verbose)
Definition: robot-wrapper.hpp:85
tsid::python::RobotPythonVisitor
Definition: robot-wrapper.hpp:32
tsid::python::RobotPythonVisitor::nonLinearEffects
static Eigen::VectorXd nonLinearEffects(const Robot &self, const pinocchio::Data &data)
Definition: robot-wrapper.hpp:138
tsid::python::RobotPythonVisitor::com_acc
static Eigen::Vector3d com_acc(const Robot &self, const pinocchio::Data &data)
Definition: robot-wrapper.hpp:126
tsid::python::RobotPythonVisitor::gear_ratios
static Eigen::VectorXd gear_ratios(const Robot &self)
Definition: robot-wrapper.hpp:110
Data
pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.cpp:29
tsid::python::RobotPythonVisitor::frameVelocity
static pinocchio::Motion frameVelocity(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:153
tsid::python::RobotPythonVisitor::set_gear_ratios
static bool set_gear_ratios(Robot &self, Eigen::VectorXd &gear_ratios)
Definition: robot-wrapper.hpp:116
tsid::python::RobotPythonVisitor::com_vel
static Eigen::Vector3d com_vel(const Robot &self, const pinocchio::Data &data)
Definition: robot-wrapper.hpp:123
tsid::python::RobotPythonVisitor::frameVelocityWorldOriented
static pinocchio::Motion frameVelocityWorldOriented(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:162
tsid::robots::RobotWrapper::FLOATING_BASE_SYSTEM
@ FLOATING_BASE_SYSTEM
Definition: robot-wrapper.hpp:63
tsid::python::RobotPythonVisitor::data
static pinocchio::Data data(const Robot &self)
Definition: robot-wrapper.hpp:103
tsid::python::RobotPythonVisitor::std_vec
std::vector< std::string > std_vec
Definition: robot-wrapper.hpp:35
tsid::python::RobotPythonVisitor::frameAccelerationWorldOriented
static pinocchio::Motion frameAccelerationWorldOriented(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:165
tsid::python::RobotPythonVisitor::acceleration
static pinocchio::Motion acceleration(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::JointIndex &index)
Definition: robot-wrapper.hpp:147
tsid::python::SE3
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
tsid::robots::RobotWrapper::FIXED_BASE_SYSTEM
@ FIXED_BASE_SYSTEM
Definition: robot-wrapper.hpp:62
tsid::python::RobotPythonVisitor::model
static pinocchio::Model model(const Robot &self)
Definition: robot-wrapper.hpp:100
tsid::python::RobotPythonVisitor::expose
static void expose(const std::string &class_name)
Definition: robot-wrapper.hpp:177
tsid::python::RobotPythonVisitor::setGravity
static void setGravity(Robot &self, const pinocchio::Motion &gravity)
Definition: robot-wrapper.hpp:174
tsid::python::RobotPythonVisitor::velocity
static pinocchio::Motion velocity(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::JointIndex &index)
Definition: robot-wrapper.hpp:144
robot-wrapper.hpp
tsid::python::RobotPythonVisitor::rotor_inertias
static Eigen::VectorXd rotor_inertias(const Robot &self)
Definition: robot-wrapper.hpp:107
tsid::python::RobotPythonVisitor::com
static Eigen::Vector3d com(const Robot &self, const pinocchio::Data &data)
Definition: robot-wrapper.hpp:120
tsid::python::RobotPythonVisitor::framePosition
static pinocchio::SE3 framePosition(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:150
tsid::python::RobotPythonVisitor::position
static pinocchio::SE3 position(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::JointIndex &index)
Definition: robot-wrapper.hpp:141
tsid::python::RobotPythonVisitor::angularMomentumTimeVariation
static Eigen::Vector3d angularMomentumTimeVariation(const Robot &self, const pinocchio::Data &data)
Definition: robot-wrapper.hpp:171
tsid
Definition: constraint-bound.hpp:26
tsid::python::RobotPythonVisitor::Jcom
static Matrix3x Jcom(const Robot &self, const pinocchio::Data &data)
Definition: robot-wrapper.hpp:129
tsid::robots::RobotWrapper
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
tsid::python::RobotPythonVisitor::frameClassicAccelerationWorldOriented
static pinocchio::Motion frameClassicAccelerationWorldOriented(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:168
tsid::python::RobotPythonVisitor::visit
void visit(PyClass &cl) const
Definition: robot-wrapper.hpp:40
fwd.hpp
tsid::python::RobotPythonVisitor::computeAllTerms
static void computeAllTerms(const Robot &self, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: robot-wrapper.hpp:132
tsid::python::RobotPythonVisitor::Matrix3x
Eigen::Matrix< double, 3, Eigen::Dynamic > Matrix3x
Definition: robot-wrapper.hpp:36
tsid::python::RobotPythonVisitor::mass
static Eigen::MatrixXd mass(Robot &self, pinocchio::Data &data)
Definition: robot-wrapper.hpp:135
tsid::python::RobotPythonVisitor::frameClassicAcceleration
static pinocchio::Motion frameClassicAcceleration(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:159
tsid::python::RobotPythonVisitor::set_rotor_inertias
static bool set_rotor_inertias(Robot &self, Eigen::VectorXd &rotor_inertias)
Definition: robot-wrapper.hpp:113
tsid::python::RobotPythonVisitor::frameAcceleration
static pinocchio::Motion frameAcceleration(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:156