[Robots] Forward declaration of RobotWrapper + add namespace robots

CMakeLists.txt

@@ -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
   )
 

include/pininvdyn/contacts/contact-6d.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
 {

include/pininvdyn/contacts/contact-base.hpp

@@ -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);

include/pininvdyn/formulations/inverse-dynamics-formulation-acc-force.hpp

@@ -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

include/pininvdyn/formulations/inverse-dynamics-formulation-base.hpp

@@ -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,

include/pininvdyn/math/constraint-base.hpp

@@ -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

include/pininvdyn/math/constraint-bound.hpp

@@ -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
 {

include/pininvdyn/math/constraint-equality.hpp

@@ -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
 {

include/pininvdyn/math/constraint-inequality.hpp

@@ -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
 {

include/pininvdyn/math/fwd.hpp

@@ -42,6 +42,12 @@ namespace pininvdyn
     
     typedef std::size_t Index;
     
+    // Forward declaration of constraints
+    class ConstraintBase;
+    class ConstraintEquality;
+    class ConstraintInequality;
+    class ConstraintBound;
+    
   }
 }
 

include/pininvdyn/robots/fwd.hpp

+//
+// 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__

include/pininvdyn/robots/robot-wrapper.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__

include/pininvdyn/tasks/task-base.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;
     };

include/pininvdyn/tasks/task-com-equality.hpp

@@ -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);

include/pininvdyn/tasks/task-motion.hpp

@@ -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
 {

include/pininvdyn/tasks/task-se3-equality.hpp

@@ -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
 {

src/contacts/contact-base.cpp

@@ -15,7 +15,7 @@
 // <http://www.gnu.org/licenses/>.
 //
 
-#include <pininvdyn/contacts/contact-base.hpp>
+#include "pininvdyn/contacts/contact-base.hpp"
 
 namespace pininvdyn
 {

src/formulations/inverse-dynamics-formulation-acc-force.cpp

@@ -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;

src/formulations/inverse-dynamics-formulation-base.cpp

@@ -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

src/robots/robot-wrapper.cpp

@@ -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; }
-