Commit e9d8bb6c authored by jcarpent's avatar jcarpent
Browse files

[Robots] Forward declaration of RobotWrapper + add namespace robots

parent a43fa52b
......@@ -154,6 +154,7 @@ SET(${PROJECT_NAME}_SOLVERS_HEADERS
)
SET(${PROJECT_NAME}_ROBOTS_HEADERS
include/pininvdyn/robots/fwd.hpp
include/pininvdyn/robots/robot-wrapper.hpp
)
......
......@@ -18,8 +18,10 @@
#ifndef __invdyn_contact_6d_hpp__
#define __invdyn_contact_6d_hpp__
#include <pininvdyn/contacts/contact-base.hpp>
#include <pininvdyn/tasks/task-se3-equality.hpp>
#include "pininvdyn/contacts/contact-base.hpp"
#include "pininvdyn/tasks/task-se3-equality.hpp"
#include "pininvdyn/math/constraint-inequality.hpp"
#include "pininvdyn/math/constraint-equality.hpp"
namespace pininvdyn
{
......
......@@ -18,12 +18,10 @@
#ifndef __invdyn_contact_base_hpp__
#define __invdyn_contact_base_hpp__
#include <pininvdyn/math/fwd.hpp>
#include <pininvdyn/robots/robot-wrapper.hpp>
#include <pininvdyn/tasks/task-motion.hpp>
#include <pininvdyn/math/constraint-base.hpp>
#include <pininvdyn/math/constraint-equality.hpp>
#include <pininvdyn/math/constraint-inequality.hpp>
#include "pininvdyn/math/fwd.hpp"
#include "pininvdyn/robots/fwd.hpp"
#include "pininvdyn/tasks/task-motion.hpp"
namespace pininvdyn
{
......@@ -45,6 +43,7 @@ namespace pininvdyn
typedef math::Matrix Matrix;
typedef tasks::TaskMotion TaskMotion;
typedef se3::Data Data;
typedef robots::RobotWrapper RobotWrapper;
ContactBase(const std::string & name,
RobotWrapper & robot);
......
......@@ -18,7 +18,9 @@
#ifndef __invdyn_inverse_dynamics_formulation_acc_force_hpp__
#define __invdyn_inverse_dynamics_formulation_acc_force_hpp__
#include <pininvdyn/formulations/inverse-dynamics-formulation-base.hpp>
#include "pininvdyn/formulations/inverse-dynamics-formulation-base.hpp"
#include "pininvdyn/math/constraint-equality.hpp"
#include <vector>
namespace pininvdyn
......
......@@ -49,6 +49,7 @@ namespace pininvdyn
typedef contacts::ContactBase ContactBase;
typedef solvers::HqpData HqpData;
typedef solvers::HqpOutput HqpOutput;
typedef robots::RobotWrapper RobotWrapper;
InverseDynamicsFormulationBase(const std::string & name,
......
......@@ -18,7 +18,7 @@
#ifndef __invdyn_math_constraint_base_hpp__
#define __invdyn_math_constraint_base_hpp__
#include <pininvdyn/math/fwd.hpp>
#include "pininvdyn/math/fwd.hpp"
#include <string>
namespace pininvdyn
......
......@@ -18,7 +18,7 @@
#ifndef __invdyn_math_constraint_bound_hpp__
#define __invdyn_math_constraint_bound_hpp__
#include <pininvdyn/math/constraint-base.hpp>
#include "pininvdyn/math/constraint-base.hpp"
namespace pininvdyn
{
......
......@@ -18,7 +18,7 @@
#ifndef __invdyn_math_constraint_equality_hpp__
#define __invdyn_math_constraint_equality_hpp__
#include <pininvdyn/math/constraint-base.hpp>
#include "pininvdyn/math/constraint-base.hpp"
namespace pininvdyn
{
......
......@@ -18,7 +18,7 @@
#ifndef __invdyn_math_constraint_inequality_hpp__
#define __invdyn_math_constraint_inequality_hpp__
#include <pininvdyn/math/constraint-base.hpp>
#include "pininvdyn/math/constraint-base.hpp"
namespace pininvdyn
{
......
......@@ -42,6 +42,12 @@ namespace pininvdyn
typedef std::size_t Index;
// Forward declaration of constraints
class ConstraintBase;
class ConstraintEquality;
class ConstraintInequality;
class ConstraintBound;
}
}
......
//
// Copyright (c) 2017 CNRS
//
// This file is part of PinInvDyn
// pinocchio 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.
// pinocchio 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
// pinocchio If not, see
// <http://www.gnu.org/licenses/>.
//
#ifndef __invdyn_robots_fwd_hpp__
#define __invdyn_robots_fwd_hpp__
namespace pininvdyn
{
namespace robots
{
class RobotWrapper;
}
}
#endif // ifndef __invdyn_robots_fwd_hpp__
......@@ -19,155 +19,158 @@
#define __invdyn_robot_wrapper_hpp__
#include "pininvdyn/math/fwd.hpp"
#include "pininvdyn/robots/fwd.hpp"
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/spatial/fwd.hpp>
#include <string>
#include <vector>
namespace pininvdyn
{
///
/// \brief Wrapper for a robot based on pinocchio
///
class RobotWrapper
namespace robots
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Scalar Scalar;
typedef se3::Model Model;
typedef se3::Data Data;
typedef se3::Motion Motion;
typedef se3::Frame Frame;
typedef se3::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;
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 se3::JointModelVariant & rootJoint,
bool verbose=false);
virtual int nq() const;
virtual int nv() const;
///
/// \brief Accessor to model.
///
/// \returns a const reference on the model.
/// \brief Wrapper for a robot based on pinocchio
///
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;
void frameVelocity(const Data & data,
const Model::FrameIndex index,
Motion & frameVelocity) const;
Motion frameAcceleration(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;
void frameClassicAcceleration(const Data & data,
const Model::FrameIndex index,
Motion & frameAcceleration) const;
void frameJacobianWorld(const Data & data,
const Model::FrameIndex index,
Data::Matrix6x & J) const;
void frameJacobianLocal(const Data & data,
const Model::FrameIndex index,
Data::Matrix6x & J) const;
protected:
void updateMd();
/// \brief Robot model.
Model m_model;
std::string m_model_filename;
bool m_verbose;
Vector m_rotor_inertias;
Vector m_gear_ratios;
Vector m_Md; /// diagonal part of inertia matrix due to rotor inertias
Matrix m_M; /// inertia matrix including rotor inertias
};
}
class RobotWrapper
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Scalar Scalar;
typedef se3::Model Model;
typedef se3::Data Data;
typedef se3::Motion Motion;
typedef se3::Frame Frame;
typedef se3::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;
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 se3::JointModelVariant & rootJoint,
bool verbose=false);
virtual int nq() const;
virtual int nv() const;
///
/// \brief Accessor to model.
///
/// \returns a const reference on the model.
///
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;
void frameVelocity(const Data & data,
const Model::FrameIndex index,
Motion & frameVelocity) const;
Motion frameAcceleration(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;
void frameClassicAcceleration(const Data & data,
const Model::FrameIndex index,
Motion & frameAcceleration) const;
void frameJacobianWorld(const Data & data,
const Model::FrameIndex index,
Data::Matrix6x & J) const;
void frameJacobianLocal(const Data & data,
const Model::FrameIndex index,
Data::Matrix6x & J) const;
protected:
void updateMd();
/// \brief Robot model.
Model m_model;
std::string m_model_filename;
bool m_verbose;
Vector m_rotor_inertias;
Vector m_gear_ratios;
Vector m_Md; /// diagonal part of inertia matrix due to rotor inertias
Matrix m_M; /// inertia matrix including rotor inertias
};
} // namespace robots
} // namespace pininvdyn
#endif // ifndef __invdyn_robot_wrapper_hpp__
......@@ -19,9 +19,11 @@
#define __invdyn_task_base_hpp__
#include "pininvdyn/math/fwd.hpp"
#include "pininvdyn/robots/robot-wrapper.hpp"
#include "pininvdyn/robots/fwd.hpp"
#include "pininvdyn/math/constraint-base.hpp"
#include <pinocchio/multibody/fwd.hpp>
namespace pininvdyn
{
namespace tasks
......@@ -39,6 +41,7 @@ namespace pininvdyn
typedef math::ConstraintBase ConstraintBase;
typedef math::ConstRefVector ConstRefVector;
typedef se3::Data Data;
typedef robots::RobotWrapper RobotWrapper;
TaskBase(const std::string & name,
RobotWrapper & robot);
......@@ -60,6 +63,7 @@ namespace pininvdyn
protected:
std::string m_name;
/// \brief Reference on the robot model.
RobotWrapper & m_robot;
};
......
......@@ -18,9 +18,10 @@
#ifndef __invdyn_task_com_equality_hpp__
#define __invdyn_task_com_equality_hpp__
#include <pininvdyn/tasks/task-motion.hpp>
#include <pininvdyn/trajectories/trajectory-base.hpp>
#include <pininvdyn/math/constraint-equality.hpp>
#include "pininvdyn/math/fwd.hpp"
#include "pininvdyn/tasks/task-motion.hpp"
#include "pininvdyn/trajectories/trajectory-base.hpp"
#include "pininvdyn/math/constraint-equality.hpp"
namespace pininvdyn
{
......@@ -35,10 +36,6 @@ namespace pininvdyn
typedef math::Vector Vector;
typedef math::Vector3 Vector3;
typedef math::ConstraintEquality ConstraintEquality;
typedef se3::Data Data;
typedef se3::Data::Matrix6x Matrix6x;
typedef se3::Motion Motion;
typedef se3::SE3 SE3;
TaskComEquality(const std::string & name,
RobotWrapper & robot);
......
......@@ -18,8 +18,8 @@
#ifndef __invdyn_task_motion_hpp__
#define __invdyn_task_motion_hpp__
#include <pininvdyn/tasks/task-base.hpp>
#include <pininvdyn/trajectories/trajectory-base.hpp>
#include "pininvdyn/tasks/task-base.hpp"
#include "pininvdyn/trajectories/trajectory-base.hpp"
namespace pininvdyn
{
......
......@@ -18,9 +18,11 @@
#ifndef __invdyn_task_se3_equality_hpp__
#define __invdyn_task_se3_equality_hpp__
#include <pininvdyn/tasks/task-motion.hpp>
#include <pininvdyn/trajectories/trajectory-base.hpp>
#include <pininvdyn/math/constraint-equality.hpp>
#include "pininvdyn/tasks/task-motion.hpp"
#include "pininvdyn/trajectories/trajectory-base.hpp"
#include "pininvdyn/math/constraint-equality.hpp"
#include <pinocchio/multibody/model.hpp>
namespace pininvdyn
{
......
......@@ -15,7 +15,7 @@
// <http://www.gnu.org/licenses/>.
//
#include <pininvdyn/contacts/contact-base.hpp>
#include "pininvdyn/contacts/contact-base.hpp"
namespace pininvdyn
{
......
......@@ -15,8 +15,9 @@
// <http://www.gnu.org/licenses/>.
//
#include <pininvdyn/formulations/inverse-dynamics-formulation-acc-force.hpp>
#include <pininvdyn/math/constraint-bound.hpp>
#include "pininvdyn/formulations/inverse-dynamics-formulation-acc-force.hpp"
#include "pininvdyn/math/constraint-bound.hpp"
#include "pininvdyn/math/constraint-inequality.hpp"
using namespace std;
using namespace pininvdyn;
......
......@@ -15,7 +15,7 @@
// <http://www.gnu.org/licenses/>.
//
#include <pininvdyn/formulations/inverse-dynamics-formulation-base.hpp>
#include "pininvdyn/formulations/inverse-dynamics-formulation-base.hpp"
namespace pininvdyn
......
......@@ -29,240 +29,243 @@ using namespace pininvdyn::math;
namespace pininvdyn
{
RobotWrapper::RobotWrapper(const std::string & filename,
const std::vector<std::string> & package_dirs,
bool verbose)
: m_verbose(verbose)
namespace robots
{
se3::urdf::buildModel(filename, m_model, m_verbose);
m_model_filename = filename;
m_rotor_inertias.setZero(m_model.nv);
m_gear_ratios.setZero(m_model.nv);
m_Md.setZero(m_model.nv);
m_M.setZero(m_model.nv, m_model.nv);
}
RobotWrapper::RobotWrapper(const std::string & filename,
const std::vector<std::string> & package_dirs,
const se3::JointModelVariant & rootJoint,
bool verbose)
RobotWrapper::RobotWrapper(const std::string & filename,
const std::vector<std::string> & package_dirs,
bool verbose)
: m_verbose(verbose)
{
se3::urdf::buildModel(filename, rootJoint, m_model, m_verbose);
m_model_filename = filename;
m_rotor_inertias.setZero(m_model.nv-6);
m_gear_ratios.setZero(m_model.nv-6);
m_Md.setZero(m_model.nv-6);
m_M.setZero(m_model.nv, m_model.nv);
}
int RobotWrapper::nq() const { return m_model.nq; }
int RobotWrapper::nv() const { return m_model.nv; }