Merge pull request #554 from nmansard/devel

[parser] Add two sample models 

bindings/python/CMakeLists.txt

@@ -70,6 +70,7 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
   multibody/joint/joint-derived.hpp
   algorithm/algorithms.hpp 
   parsers/parsers.hpp
+  parsers/sample-models.hpp
   )
 
 SET(${PROJECT_NAME}_PYTHON_SOURCES

bindings/python/parsers/expose-parsers.cpp

@@ -17,6 +17,7 @@
 
 #include "pinocchio/bindings/python/fwd.hpp"
 #include "pinocchio/bindings/python/parsers/parsers.hpp"
+#include "pinocchio/bindings/python/parsers/sample-models.hpp"
 
 namespace se3
 {
@@ -26,6 +27,7 @@ namespace se3
     void exposeParsers()
     {
       ParsersPythonVisitor::expose();
+      SampleModelsPythonVisitor::expose();
     }
     
   } // namespace python

bindings/python/parsers/sample-models.hpp

+//
+// Copyright (c) 2015-2018 CNRS
+//
+// This file is part of Pinocchio
+// 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 __se3_python_sample_models_hpp__
+#define __se3_python_sample_models_hpp__
+
+#include "pinocchio/parsers/sample-models.hpp"
+#include "pinocchio/bindings/python/multibody/data.hpp"
+#include "pinocchio/bindings/python/multibody/geometry-model.hpp"
+#include "pinocchio/bindings/python/multibody/geometry-data.hpp"
+
+namespace se3
+{
+  namespace python
+  {
+    struct SampleModelsPythonVisitor
+    {
+
+      static Model buildSampleModelHumanoidSimple(bool usingFF)
+      {
+        Model model;
+        buildModels::humanoidSimple(model,usingFF);
+        return model;
+      }
+
+      static Model buildSampleModelManipulator()
+      {
+        Model model;
+        buildModels::manipulator(model);
+        return model;
+      }
+      
+      static GeometryModel buildSampleGeometryModelManipulator(const Model& model)
+      {
+        GeometryModel geom;
+        buildModels::manipulatorGeometries(model,geom);
+        return geom;
+      }
+      
+      static Model buildSampleModelHumanoid()
+      {
+        Model model;
+        buildModels::humanoid(model);
+        return model;
+      }
+      
+      static GeometryModel buildSampleGeometryModelHumanoid(const Model& model)
+      {
+        GeometryModel geom;
+        buildModels::humanoidGeometries(model,geom);
+        return geom;
+      }
+      
+      /* --- Expose --------------------------------------------------------- */
+      static void expose();
+    }; // struct SampleModelsPythonVisitor
+
+    inline void SampleModelsPythonVisitor::expose()
+    {
+      bp::def("buildSampleModelHumanoidSimple",
+              static_cast <Model (*) (bool)> (&SampleModelsPythonVisitor::buildSampleModelHumanoidSimple),
+              bp::args("bool (usingFreeFlyer)"),
+              "Generate a (hard-coded) model of a simple humanoid robot."
+              );
+
+      bp::def("buildSampleModelManipulator",
+              static_cast <Model (*) ()> (&SampleModelsPythonVisitor::buildSampleModelManipulator),
+              "Generate a (hard-coded) model of a simple manipulator."
+              );
+
+      bp::def("buildSampleGeometryModelManipulator",
+              static_cast <GeometryModel (*) (const Model&)> (&SampleModelsPythonVisitor::buildSampleGeometryModelManipulator),
+              bp::args("Model (model)"),
+              "Generate a (hard-coded) geometry model of a simple manipulator."
+              );
+      
+      bp::def("buildSampleModelHumanoid",
+              static_cast <Model (*) ()> (&SampleModelsPythonVisitor::buildSampleModelHumanoid),
+              "Generate a (hard-coded) model of a simple humanoid."
+              );
+
+      bp::def("buildSampleGeometryModelHumanoid",
+              static_cast <GeometryModel (*) (const Model&)> (&SampleModelsPythonVisitor::buildSampleGeometryModelHumanoid),
+              bp::args("Model (model)"),
+              "Generate a (hard-coded) geometry model of a simple humanoid."
+              );
+      
+    }
+    
+  }
+} // namespace se3::python
+
+#endif // ifndef __se3_python_sample_models_hpp__

src/parsers/sample-models.cpp

@@ -84,7 +84,7 @@ namespace se3
 
     }
 
-    void humanoidSimple(Model & model, bool usingFF)
+    void humanoidRandom(Model & model, bool usingFF)
     {
       // root
       if(! usingFF )
@@ -137,6 +137,203 @@ namespace se3
 
     }
 
+    static void addManipulator(Model & model,
+                               Model::JointIndex rootJoint = 0,
+                               const SE3 & Mroot = SE3::Identity(),
+                               const std::string& pre = "")
+    {
+      typedef typename JointModelRX::ConfigVector_t CV;
+      typedef typename JointModelRX::TangentVector_t TV;
+
+      Model::JointIndex idx = rootJoint;
+
+      SE3 Marm(Eigen::Matrix3d::Identity(),Eigen::Vector3d(0,0,1));
+      SE3 I4 = SE3::Identity();
+      Inertia Ijoint = Inertia(.1,Eigen::Vector3d::Zero(),Eigen::Matrix3d::Identity()*.01);
+      Inertia Iarm   = Inertia(1.,Eigen::Vector3d(0,0,.5),Eigen::Matrix3d::Identity()*.1 );
+      CV qmin = CV::Constant(-3.14), qmax   = CV::Constant(3.14);
+      TV vmax = TV::Constant(-10),   taumax = TV::Constant(10);
+
+      idx = model.addJoint(idx,JointModelRX(),Mroot,pre+"shoulder1_joint", vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Ijoint);
+      model.addJointFrame(idx);
+      model.addBodyFrame(pre+"shoulder1_body",idx);
+
+      idx = model.addJoint(idx,JointModelRY(),I4   ,pre+"shoulder2_joint", vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Ijoint);
+      model.addJointFrame(idx);
+      model.addBodyFrame(pre+"shoulder2_body",idx);
+
+      idx = model.addJoint(idx,JointModelRZ(),I4   ,pre+"shoulder3_joint", vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Iarm);
+      model.addJointFrame(idx);
+      model.addBodyFrame(pre+"upperarm_body",idx);
+
+      idx = model.addJoint(idx,JointModelRY(),Marm ,pre+"elbow_joint",     vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Iarm);
+      model.addJointFrame(idx);
+      model.addBodyFrame(pre+"lowerarm_body",idx);
+      model.addBodyFrame(pre+"elbow_body",idx);
+
+      idx = model.addJoint(idx,JointModelRX(),Marm ,pre+"wrist1_joint",    vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Ijoint);
+      model.addJointFrame(idx);
+      model.addBodyFrame(pre+"wrist1_body",idx);
+
+      idx = model.addJoint(idx,JointModelRY(),I4   ,pre+"wrist2_joint",    vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Iarm);
+      model.addJointFrame(idx);
+      model.addBodyFrame(pre+"effector_body",idx);
+
+    }
+
+    /* Add a 6DOF manipulator shoulder-elbow-wrist geometries to an existing model. 
+     * <model> is the the kinematic chain, constant.
+     * <geom> is the geometry model where the new geoms are added.
+     * <pre> is the prefix (string) before every name in the model.
+     */
+    static void addManipulatorGeometries(const Model& model,
+                                         GeometryModel & geom,
+                                         const std::string & pre = "")
+    {
+      GeometryObject shoulderBall(pre+"shoulder_object",
+                                  model.getBodyId(pre+"shoulder1_body"),/*NR*/0,
+                                  boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
+                                  SE3::Identity());
+      geom.addGeometryObject(shoulderBall,model,true);
+      
+      GeometryObject elbowBall(pre+"elbow_object",
+                               model.getBodyId(pre+"elbow_body"),/*NR*/0,
+                               boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
+                               SE3::Identity());
+      geom.addGeometryObject(elbowBall,model,true);
+      
+      GeometryObject wristBall(pre+"wrist_object",
+                               model.getBodyId(pre+"wrist1_body"),/*NR*/0,
+                               boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
+                               SE3::Identity());
+      geom.addGeometryObject(wristBall,model,true);
+
+      GeometryObject upperArm(pre+"upperarm_object",
+                              model.getBodyId(pre+"upperarm_body"),/*NR*/0,
+                              boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)),
+                              SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
+      geom.addGeometryObject(upperArm,model,true);
+
+      GeometryObject lowerArm(pre+"lowerarm_object",
+                              model.getBodyId(pre+"lowerarm_body"),/*NR*/0,
+                              boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)),
+                              SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
+      geom.addGeometryObject(lowerArm,model,true);
+
+      GeometryObject effectorArm(pre+"effector_object",
+                                 model.getBodyId(pre+"effector_body"),/*NR*/0,
+                                 boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .2)),
+                                 SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.1)) );
+      geom.addGeometryObject(effectorArm,model,true);
+    }
+
+
+    void manipulator(Model& model) { addManipulator(model); }
+    void manipulatorGeometries(const Model& model, GeometryModel & geom)
+    { addManipulatorGeometries(model,geom); }
+
+    static Eigen::Matrix3d rotate(const double angle, const Eigen::Vector3d & axis)
+    { return Eigen::AngleAxisd(angle,axis).toRotationMatrix(); }
+    
+    void humanoid(Model & model, bool usingFF)
+    {
+      using namespace Eigen;
+      typedef typename JointModelRX::ConfigVector_t CV;
+      typedef typename JointModelRX::TangentVector_t TV;
+     
+      Model::JointIndex idx,chest,ffidx;
+
+      SE3 Marm(Eigen::Matrix3d::Identity(),Eigen::Vector3d(0,0,1));
+      SE3 I4 = SE3::Identity();
+      Inertia Ijoint = Inertia(.1,Eigen::Vector3d::Zero(),Eigen::Matrix3d::Identity()*.01);
+      Inertia Iarm   = Inertia(1.,Eigen::Vector3d(0,0,.5),Eigen::Matrix3d::Identity()*.1 );
+      CV qmin = CV::Constant(-3.14), qmax   = CV::Constant(3.14);
+      TV vmax = TV::Constant(-10),   taumax = TV::Constant(10);
+
+      /* --- Free flyer --- */
+      if(usingFF)
+          ffidx = model.addJoint(0,JointModelFreeFlyer(),SE3::Identity(),"freeflyer_joint");
+      else
+        {
+          JointModelComposite jff((JointModelTranslation()));
+          jff.addJoint(JointModelSphericalZYX());
+          ffidx = model.addJoint(0,jff,SE3::Identity(),"freeflyer_joint");
+        }
+      model.addJointFrame(ffidx);
+
+      /* --- Lower limbs --- */
+
+      AngleAxisd(M_PI,Vector3d(1,0,0)).toRotationMatrix();
+
+      addManipulator(model,ffidx,SE3(rotate(M_PI,Vector3d::UnitX()),Vector3d(0,-0.2,-.1)),"rleg");
+      addManipulator(model,ffidx,SE3(rotate(M_PI,Vector3d::UnitX()),Vector3d(0, 0.2,-.1)),"lleg");
+
+      model.jointPlacements[7 ].rotation() = rotate(M_PI/2,Vector3d::UnitY()); // rotate right foot
+      model.jointPlacements[13].rotation() = rotate(M_PI/2,Vector3d::UnitY()); // rotate left  foot
+      
+      /* --- Chest --- */
+      idx = model.addJoint(ffidx,JointModelRX(),I4 ,"chest1_joint",    vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Ijoint);
+      model.addJointFrame(idx);
+      model.addBodyFrame("chest1_body",idx);
+
+      idx = model.addJoint(idx,JointModelRY(),I4 ,"chest2_joint",    vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Iarm);
+      model.addJointFrame(idx);
+      model.addBodyFrame("chest2_body",idx);
+
+      chest = idx;
+
+      /* --- Head --- */
+      idx = model.addJoint(idx,JointModelRX(),
+                           SE3(Matrix3d::Identity(),Vector3d(0.,0.,1.)),
+                           "head1_joint",    vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Ijoint);
+      model.addJointFrame(idx);
+      model.addBodyFrame("head1_body",idx);
+
+      idx = model.addJoint(idx,JointModelRY(),I4 ,"head2_joint",    vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Iarm);
+      model.addJointFrame(idx);
+      model.addBodyFrame("head2_body",idx);
+
+      /* --- Upper Limbs --- */
+      addManipulator(model,chest,SE3(rotate(M_PI,Vector3d::UnitX()),Vector3d(0,-0.3, 1.)),"rarm");
+      addManipulator(model,chest,SE3(rotate(M_PI,Vector3d::UnitX()),Vector3d(0, 0.3, 1.)),"larm");
+    }
+
+    void humanoidGeometries(const Model& model, GeometryModel & geom)
+    {
+      addManipulatorGeometries(model,geom,"rleg");
+      addManipulatorGeometries(model,geom,"lleg");
+      addManipulatorGeometries(model,geom,"rarm");
+      addManipulatorGeometries(model,geom,"larm");
+
+      GeometryObject chestBall("chest_object",
+                               model.getBodyId("chest1_body"),/*NR*/0,
+                               boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
+                               SE3::Identity());
+      geom.addGeometryObject(chestBall,model,true);
+
+      GeometryObject headBall("head_object",
+                               model.getBodyId("head2_body"),/*NR*/0,
+                               boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.25)),
+                               SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
+      geom.addGeometryObject(headBall,model,true);
+
+      GeometryObject chestArm("chest2_object",
+                              model.getBodyId("chest2_body"),/*NR*/0,
+                              boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)),
+                              SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
+      geom.addGeometryObject(chestArm,model,true);
+    }
+
   } // namespace buildModels
   
 } // namespace se3

src/parsers/sample-models.hpp

@@ -20,15 +20,69 @@
 #define __se3_sample_models_hpp__
 
 #include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/geometry.hpp"
 
 namespace se3
 {
   namespace buildModels
-  {
+  {  
+    /** \brief Create a 6-DOF kinematic chain shoulder-elbow-wrist.
+     *
+     * \param model: model, typically given empty, where the kinematic chain is added.
+     */
+    void manipulator(Model& model);
+    /** \brief Create the geometries on top of the kinematic model created by manipulator function.
+     *
+     * \param model, const, kinematic chain typically produced by the function manipulator(model).
+     * \warning this method is expecting specific namings of the kinematic chain, use it with care
+     * not using after manipulator(model).
+     */
+    void manipulatorGeometries(const Model& model, GeometryModel & geom);
 
+    /** \brief Create a 28-DOF kinematic chain of a floating humanoid robot.
+     * 
+     * The kinematic chain has 4 limbs shoulder-elbow-wrist, one 2-dof torso, one
+     * 2-dof neck. The float joint is either a free-float joint JointModelFreeFloating
+     * (with nq=7 and nv=6), or a composite joint with 3 prismatic and 1 roll-pitch-yaw.
+     * Using a free-floating or a composite joint is decided by the boolean usingFF.
+     *
+     * \param model: model, typically given empty, where the kinematic chain is added.
+     * \param usingFF: if True, implement the chain with a plain JointModelFreeFloating; if False,
+     * uses a composite joint. This changes the size of the configuration space (35 vs 34).
+     */
+    void humanoid(Model& model,bool usingFF=true);
+    /** \brief Create the geometries on top of the kinematic model created by humanoid function.
+     *
+     * \param model, const, kinematic chain typically produced by the function humanoid(model).
+     * \warning this method is expecting specific namings of the kinematic chain, use it with care
+     * not using after humanoid(model).
+     */
+    void humanoidGeometries(const Model& model, GeometryModel & geom);
+    
+    /** \brief Create a humanoid kinematic tree with 6D limbs and random joint placement.
+     *
+     * This method is only meant to be used in unittest. Due to random placement and masses,
+     * the resulting model is likely to not correspond to any physically-plausible model. 
+     * You may want to consider se3::humanoid and se3::humanoidGeometries functions that
+     * rather define a plain and non-random model. 
+     * \param model: model, typically given empty, where the kinematic chain is added.
+     * \param usingFF: if True, implement the chain with a plain JointModelFreeFloating; if False,
+     * uses a composite joint. This changes the size of the configuration space (35 vs 34).
+     */
+    void humanoidRandom(Model& model, bool usingFF = true);
+
+    /** \brief Create a random humanoid tree with 2d limbs.
+     * \ deprecated This function has been replaced by the non-random se3::humanoid function.
+     */
+    PINOCCHIO_DEPRECATED
     void humanoid2d(Model& model);
-    void humanoidSimple(Model& model, bool usingFF = true);
 
+    /** \brief Alias of humanoidRandom, for compatibility reasons.
+     * \deprecated use se3::humanoid or se3::humanoidRandom instead. 
+     */
+    PINOCCHIO_DEPRECATED
+    inline void humanoidSimple(Model& model, bool usingFF = true) { humanoidRandom(model,usingFF); }
+   
   } // namespace buildModels
 } // namespace se3
 

unittest/CMakeLists.txt

@@ -59,6 +59,7 @@ ADD_PINOCCHIO_UNIT_TEST(com eigen3)
 ADD_PINOCCHIO_UNIT_TEST(jacobian eigen3)
 ADD_PINOCCHIO_UNIT_TEST(cholesky eigen3)
 ADD_PINOCCHIO_UNIT_TEST(dynamics eigen3)
+ADD_PINOCCHIO_UNIT_TEST(sample-models eigen3)
 
 IF(URDFDOM_FOUND)
   ADD_PINOCCHIO_UNIT_TEST(urdf "eigen3;urdfdom")

unittest/sample-models.cpp

+//
+// Copyright (c) 2015-2018 CNRS
+//
+// This file is part of Pinocchio
+// 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/>.
+
+#include <iostream>
+#include <fstream>
+#include <streambuf>
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/geometry.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+
+#include <boost/test/unit_test.hpp>
+
+
+
+BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
+
+BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid_simple )
+{
+  se3::Model model;
+  se3::buildModels::humanoidSimple(model,true);
+
+  BOOST_CHECK(model.nq == 33);
+  BOOST_CHECK(model.nv == 32);
+
+  se3::Model modelff;
+  se3::buildModels::humanoidSimple(modelff,false);
+
+  BOOST_CHECK(modelff.nq == 32);
+  BOOST_CHECK(modelff.nv == 32);
+}
+
+BOOST_AUTO_TEST_CASE ( build_model_sample_manipulator )
+{
+  se3::Model model;
+  se3::buildModels::manipulator(model);
+
+  BOOST_CHECK(model.nq == 6);
+  BOOST_CHECK(model.nv == 6);
+
+  se3::Data data(model);
+  se3::GeometryModel geom;
+  se3::buildModels::manipulatorGeometries(model,geom);
+}
+
+BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid )
+{
+  se3::Model model;
+  se3::buildModels::humanoid(model);
+  se3::Data data(model);
+
+  BOOST_CHECK(model.nq == 35);
+  BOOST_CHECK(model.nv == 34);
+
+  se3::GeometryModel geom;
+  se3::buildModels::humanoidGeometries(model,geom);
+  se3::GeometryData geomdata(geom);
+  
+  Eigen::VectorXd q = se3::neutral(model);
+  se3::forwardKinematics(model,data,q);
+  se3::updateGeometryPlacements(model,data,geom,geomdata,q);
+
+  /* We might want to check here the joint namings, and validate the 
+   * direct geometry with respect to reference values. */
+}
+
+
+
+BOOST_AUTO_TEST_SUITE_END()