robot-wrapper.hpp

Go to the documentation of this file.

//
// Copyright (c) 2018 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#ifndef __tsid_python_robot_wrapper_hpp__
#define __tsid_python_robot_wrapper_hpp__

#include "tsid/bindings/python/fwd.hpp"

#include "tsid/robots/robot-wrapper.hpp"

namespace tsid {
namespace python {
namespace bp = boost::python;

template <typename Robot>
struct RobotPythonVisitor
 : public boost::python::def_visitor<RobotPythonVisitor<Robot> > {
 typedef std::vector<std::string> std_vec;
 typedef Eigen::Matrix<double, 3, Eigen::Dynamic> Matrix3x;

 template <class PyClass>

 void visit(PyClass& cl) const {
 cl
 .def(bp::init<std::string, std_vec, bool>(
 (bp::arg("filename"), bp::arg("package_dir"), bp::arg("verbose")),
 "Default constructor without RootJoint."))
 .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."))
 .def(bp::init<pinocchio::Model, bool>(
 (bp::arg("Pinocchio Model"), bp::arg("verbose")),
 "Default constructor from pinocchio model without RootJoint."))
 .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."))
 .def("__init__",
 bp::make_constructor(RobotPythonVisitor<Robot>::makeClass))
 .add_property("nq", &Robot::nq)
 .add_property("nv", &Robot::nv)
 .add_property("na", &Robot::na)
 .add_property("nq_actuated", &Robot::nq_actuated)
 .add_property("is_fixed_base", &Robot::is_fixed_base)

 .def("model", &RobotPythonVisitor::model)
 .def("data", &RobotPythonVisitor::data)

 .add_property("rotor_inertias", &RobotPythonVisitor::rotor_inertias)
 .add_property("gear_ratios", &RobotPythonVisitor::gear_ratios)
 .def("set_rotor_inertias", &RobotPythonVisitor::set_rotor_inertias,
 bp::arg("inertia vector"))
 .def("set_gear_ratios", &RobotPythonVisitor::set_gear_ratios,
 bp::arg("gear ratio vector"))

 .def("computeAllTerms", &RobotPythonVisitor::computeAllTerms,
 bp::args("data", "q", "v"), "compute all dynamics")
 .def("com", &RobotPythonVisitor::com, bp::arg("data"))
 .def("com_vel", &RobotPythonVisitor::com_vel, bp::arg("data"))
 .def("com_acc", &RobotPythonVisitor::com_acc, bp::arg("data"))
 .def("Jcom", &RobotPythonVisitor::Jcom, bp::arg("data"))
 .def("mass", &RobotPythonVisitor::mass, bp::arg("data"))
 .def("nonLinearEffect", &RobotPythonVisitor::nonLinearEffects,
 bp::arg("data"))
 .def("position", &RobotPythonVisitor::position,
 bp::args("data", "index"))
 .def("velocity", &RobotPythonVisitor::velocity,
 bp::args("data", "index"))
 .def("acceleration", &RobotPythonVisitor::acceleration,
 bp::args("data", "index"))

 .def("framePosition", &RobotPythonVisitor::framePosition,
 bp::args("data", "index"))
 .def("frameVelocity", &RobotPythonVisitor::frameVelocity,
 bp::args("data", "index"))
 .def("frameAcceleration", &RobotPythonVisitor::frameAcceleration,
 bp::args("data", "index"))
 .def("frameClassicAcceleration",
 &RobotPythonVisitor::frameClassicAcceleration,
 bp::args("data", "index"))
 .def("frameVelocityWorldOriented",
 &RobotPythonVisitor::frameVelocityWorldOriented,
 bp::args("data", "index"))
 .def("frameAccelerationWorldOriented",
 &RobotPythonVisitor::frameAccelerationWorldOriented,
 bp::args("data", "index"))
 .def("frameClassicAccelerationWorldOriented",
 &RobotPythonVisitor::frameClassicAccelerationWorldOriented,
 bp::args("data", "index"))
 .def("angularMomentumTimeVariation",
 &RobotPythonVisitor::angularMomentumTimeVariation, bp::arg("data"))
 .def("setGravity", &RobotPythonVisitor::setGravity, bp::arg("gravity"));
 }

 static boost::shared_ptr<Robot> makeClass(
 const std::string& filename, const std::vector<std::string>& stdvec,
 bp::object& bpObject, bool verbose) {
 pinocchio::JointModelFreeFlyer root_joint =
 bp::extract<pinocchio::JointModelFreeFlyer>(bpObject)();
 boost::shared_ptr<Robot> p(
 new tsid::robots::RobotWrapper(filename, stdvec, root_joint, verbose));
 return p;
 }

 static pinocchio::Model model(const Robot& self) { return self.model(); }
 static pinocchio::Data data(const Robot& self) {
 pinocchio::Data data(self.model());
 return data;
 }
 static Eigen::VectorXd rotor_inertias(const Robot& self) {
 return self.rotor_inertias();
 }
 static Eigen::VectorXd gear_ratios(const Robot& self) {
 return self.gear_ratios();
 }
 static bool set_rotor_inertias(Robot& self,
 const Eigen::VectorXd& rotor_inertias) {
 return self.rotor_inertias(rotor_inertias);
 }
 static bool set_gear_ratios(Robot& self, const Eigen::VectorXd& gear_ratios) {
 return self.gear_ratios(gear_ratios);
 }

 static Eigen::Vector3d com(const Robot& self, const pinocchio::Data& data) {
 return self.com(data);
 }
 static Eigen::Vector3d com_vel(const Robot& self,
 const pinocchio::Data& data) {
 return self.com_vel(data);
 }
 static Eigen::Vector3d com_acc(const Robot& self,
 const pinocchio::Data& data) {
 return self.com_acc(data);
 }
 static Matrix3x Jcom(const Robot& self, const pinocchio::Data& data) {
 return self.Jcom(data);
 }
 static void computeAllTerms(const Robot& self, pinocchio::Data& data,
 const Eigen::VectorXd& q,
 const Eigen::VectorXd& v) {
 self.computeAllTerms(data, q, v);
 }
 static Eigen::MatrixXd mass(Robot& self, pinocchio::Data& data) {
 return self.mass(data);
 }
 static Eigen::VectorXd nonLinearEffects(const Robot& self,
 const pinocchio::Data& data) {
 return self.nonLinearEffects(data);
 }
 static pinocchio::SE3 position(const Robot& self, const pinocchio::Data& data,
 const pinocchio::Model::JointIndex& index) {
 return self.position(data, index);
 }
 static pinocchio::Motion velocity(const Robot& self,
 const pinocchio::Data& data,
 const pinocchio::Model::JointIndex& index) {
 return self.velocity(data, index);
 }
 static pinocchio::Motion acceleration(
 const Robot& self, const pinocchio::Data& data,
 const pinocchio::Model::JointIndex& index) {
 return self.acceleration(data, index);
 }
 static pinocchio::SE3 framePosition(
 const Robot& self, const pinocchio::Data& data,
 const pinocchio::Model::FrameIndex& index) {
 return self.framePosition(data, index);
 }
 static pinocchio::Motion frameVelocity(
 const Robot& self, const pinocchio::Data& data,
 const pinocchio::Model::FrameIndex& index) {
 return self.frameVelocity(data, index);
 }
 static pinocchio::Motion frameAcceleration(
 const Robot& self, const pinocchio::Data& data,
 const pinocchio::Model::FrameIndex& index) {
 return self.frameAcceleration(data, index);
 }
 static pinocchio::Motion frameClassicAcceleration(
 const Robot& self, const pinocchio::Data& data,
 const pinocchio::Model::FrameIndex& index) {
 return self.frameClassicAcceleration(data, index);
 }
 static pinocchio::Motion frameVelocityWorldOriented(
 const Robot& self, const pinocchio::Data& data,
 const pinocchio::Model::FrameIndex& index) {
 return self.frameVelocityWorldOriented(data, index);
 }
 static pinocchio::Motion frameAccelerationWorldOriented(
 const Robot& self, const pinocchio::Data& data,
 const pinocchio::Model::FrameIndex& index) {
 return self.frameAccelerationWorldOriented(data, index);
 }
 static pinocchio::Motion frameClassicAccelerationWorldOriented(
 const Robot& self, const pinocchio::Data& data,
 const pinocchio::Model::FrameIndex& index) {
 return self.frameClassicAccelerationWorldOriented(data, index);
 }
 static Eigen::Vector3d angularMomentumTimeVariation(
 const Robot& self, const pinocchio::Data& data) {
 return self.angularMomentumTimeVariation(data);
 }
 static void setGravity(Robot& self, const pinocchio::Motion& gravity) {
 return self.setGravity(gravity);
 }
 static void expose(const std::string& class_name) {
 std::string doc = "Robot Wrapper info.";
 bp::class_<Robot>(class_name.c_str(), doc.c_str(), bp::no_init)
 .def(RobotPythonVisitor<Robot>());
 ;
 bp::enum_<robots::RobotWrapper::RootJointType>("RootJointType")
 .value("FIXED_BASE_SYSTEM", robots::RobotWrapper::FIXED_BASE_SYSTEM)
 .value("FLOATING_BASE_SYSTEM",
 robots::RobotWrapper::FLOATING_BASE_SYSTEM)
 .export_values();
 }
};
} // namespace python
} // namespace tsid

#endif // ifndef __tsid_python_robot_wrapper_hpp__