Loading...
Searching...
No Matches
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
25namespace 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__
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:41
@ FLOATING_BASE_SYSTEM
Definition: robot-wrapper.hpp:63
@ FIXED_BASE_SYSTEM
Definition: robot-wrapper.hpp:62
Definition: constraint-bound.hpp:27
Definition: robot-wrapper.hpp:34
static pinocchio::Motion frameClassicAcceleration(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:159
static pinocchio::SE3 framePosition(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:150
static bool set_rotor_inertias(Robot &self, Eigen::VectorXd &rotor_inertias)
Definition: robot-wrapper.hpp:113
static Eigen::VectorXd nonLinearEffects(const Robot &self, const pinocchio::Data &data)
Definition: robot-wrapper.hpp:138
static pinocchio::Motion frameClassicAccelerationWorldOriented(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:168
static pinocchio::Motion velocity(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::JointIndex &index)
Definition: robot-wrapper.hpp:144
static Eigen::Vector3d com(const Robot &self, const pinocchio::Data &data)
Definition: robot-wrapper.hpp:120
static pinocchio::Motion acceleration(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::JointIndex &index)
Definition: robot-wrapper.hpp:147
static Eigen::VectorXd rotor_inertias(const Robot &self)
Definition: robot-wrapper.hpp:107
static pinocchio::Motion frameVelocityWorldOriented(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:162
static pinocchio::Motion frameAcceleration(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:156
static pinocchio::Motion frameAccelerationWorldOriented(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:165
static Eigen::Vector3d com_vel(const Robot &self, const pinocchio::Data &data)
Definition: robot-wrapper.hpp:123
static Matrix3x Jcom(const Robot &self, const pinocchio::Data &data)
Definition: robot-wrapper.hpp:129
Eigen::Matrix< double, 3, Eigen::Dynamic > Matrix3x
Definition: robot-wrapper.hpp:36
static Eigen::VectorXd gear_ratios(const Robot &self)
Definition: robot-wrapper.hpp:110
static Eigen::MatrixXd mass(Robot &self, pinocchio::Data &data)
Definition: robot-wrapper.hpp:135
static pinocchio::Motion frameVelocity(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::FrameIndex &index)
Definition: robot-wrapper.hpp:153
static Eigen::Vector3d angularMomentumTimeVariation(const Robot &self, const pinocchio::Data &data)
Definition: robot-wrapper.hpp:171
static void computeAllTerms(const Robot &self, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: robot-wrapper.hpp:132
static pinocchio::Data data(const Robot &self)
Definition: robot-wrapper.hpp:103
static Eigen::Vector3d com_acc(const Robot &self, const pinocchio::Data &data)
Definition: robot-wrapper.hpp:126
static pinocchio::Model model(const Robot &self)
Definition: robot-wrapper.hpp:100
static bool set_gear_ratios(Robot &self, Eigen::VectorXd &gear_ratios)
Definition: robot-wrapper.hpp:116
static void expose(const std::string &class_name)
Definition: robot-wrapper.hpp:177
std::vector< std::string > std_vec
Definition: robot-wrapper.hpp:35
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
void visit(PyClass &cl) const
Definition: robot-wrapper.hpp:40
static void setGravity(Robot &self, const pinocchio::Motion &gravity)
Definition: robot-wrapper.hpp:174
static pinocchio::SE3 position(const Robot &self, const pinocchio::Data &data, const pinocchio::Model::JointIndex &index)
Definition: robot-wrapper.hpp:141