robot-wrapper.hpp

Go to the documentation of this file.

//
// Copyright (c) 2017 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 __invdyn_robot_wrapper_hpp__
#define __invdyn_robot_wrapper_hpp__

#include "tsid/deprecated.hh"
#include "tsid/math/fwd.hpp"
#include "tsid/robots/fwd.hpp"

#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/spatial/fwd.hpp>

#include <string>
#include <vector>

namespace tsid {
namespace robots {
class RobotWrapper {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef math::Scalar Scalar;
 typedef pinocchio::Model Model;
 typedef pinocchio::Data Data;
 typedef pinocchio::Motion Motion;
 typedef pinocchio::Frame Frame;
 typedef pinocchio::SE3 SE3;
 typedef math::Vector Vector;
 typedef math::Vector3 Vector3;
 typedef math::Vector6 Vector6;
 typedef math::Matrix Matrix;
 typedef math::Matrix3x Matrix3x;
 typedef math::RefVector RefVector;
 typedef math::ConstRefVector ConstRefVector;

 /* Possible root joints */
 typedef enum e_RootJointType {
 FIXED_BASE_SYSTEM = 0,
 FLOATING_BASE_SYSTEM = 1,
 } RootJointType;

 RobotWrapper(const std::string& filename,
 const std::vector<std::string>& package_dirs,
 bool verbose = false);

 RobotWrapper(const std::string& filename,
 const std::vector<std::string>& package_dirs,
 const pinocchio::JointModelVariant& rootJoint,
 bool verbose = false);

 TSID_DEPRECATED RobotWrapper(const Model& m, bool verbose = false);

 RobotWrapper(const Model& m, RootJointType rootJoint, bool verbose = false);

 virtual ~RobotWrapper() = default;

 virtual int nq() const;
 virtual int nq_actuated() const;
 virtual int nv() const;
 virtual int na() const;
 virtual bool is_fixed_base() const;

 const Model& model() const;
 Model& model();

 void computeAllTerms(Data& data, const Vector& q, const Vector& v) const;

 const Vector& rotor_inertias() const;
 const Vector& gear_ratios() const;

 bool rotor_inertias(ConstRefVector rotor_inertias);
 bool gear_ratios(ConstRefVector gear_ratios);

 void com(const Data& data, RefVector com_pos, RefVector com_vel,
 RefVector com_acc) const;

 const Vector3& com(const Data& data) const;

 const Vector3& com_vel(const Data& data) const;

 const Vector3& com_acc(const Data& data) const;

 const Matrix3x& Jcom(const Data& data) const;

 const Matrix& mass(const Data& data);

 const Vector& nonLinearEffects(const Data& data) const;

 const SE3& position(const Data& data, const Model::JointIndex index) const;

 const Motion& velocity(const Data& data, const Model::JointIndex index) const;

 const Motion& acceleration(const Data& data,
 const Model::JointIndex index) const;

 void jacobianWorld(const Data& data, const Model::JointIndex index,
 Data::Matrix6x& J) const;

 void jacobianLocal(const Data& data, const Model::JointIndex index,
 Data::Matrix6x& J) const;

 SE3 framePosition(const Data& data, const Model::FrameIndex index) const;

 void framePosition(const Data& data, const Model::FrameIndex index,
 SE3& framePosition) const;

 Motion frameVelocity(const Data& data, const Model::FrameIndex index) const;

 Motion frameVelocityWorldOriented(const Data& data,
 const Model::FrameIndex index) const;

 void frameVelocity(const Data& data, const Model::FrameIndex index,
 Motion& frameVelocity) const;

 Motion frameAcceleration(const Data& data,
 const Model::FrameIndex index) const;

 Motion frameAccelerationWorldOriented(const Data& data,
 const Model::FrameIndex index) const;

 void frameAcceleration(const Data& data, const Model::FrameIndex index,
 Motion& frameAcceleration) const;

 Motion frameClassicAcceleration(const Data& data,
 const Model::FrameIndex index) const;

 Motion frameClassicAccelerationWorldOriented(
 const Data& data, const Model::FrameIndex index) const;

 void frameClassicAcceleration(const Data& data, const Model::FrameIndex index,
 Motion& frameAcceleration) const;

 void frameJacobianWorld(Data& data, const Model::FrameIndex index,
 Data::Matrix6x& J) const;

 void frameJacobianLocal(Data& data, const Model::FrameIndex index,
 Data::Matrix6x& J) const;

 const Data::Matrix6x& momentumJacobian(const Data& data) const;

 Vector3 angularMomentumTimeVariation(const Data& data) const;

 void setGravity(const Motion& gravity);

 protected:
 void init();
 void updateMd();

 Model m_model;
 std::string m_model_filename;
 bool m_verbose;

 int m_nq_actuated;
 int m_na;
 bool m_is_fixed_base;
 Vector m_rotor_inertias;
 Vector m_gear_ratios;
 Vector m_Md;
 Matrix m_M;
};

} // namespace robots

} // namespace tsid

#endif // ifndef __invdyn_robot_wrapper_hpp__