[parser] Add sample manipulator (with unittest and bindings).

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 <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.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;
+      }
+      
+      /* --- 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."
+              );
+      
+    }
+    
+  }
+} // namespace se3::python
+
+#endif // ifndef __se3_python_sample_models_hpp__

src/parsers/sample-models.cpp

@@ -137,6 +137,64 @@ namespace se3
 
     }
 
+    void manipulator(Model & model)
+    {
+      typedef typename JointModelRX::ConfigVector_t CV;
+      typedef typename JointModelRX::TangentVector_t TV;
+
+      Model::JointIndex idx = 0;
+
+      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(),I4  ,"shoulder1_joint", vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Ijoint);
+      model.addJointFrame(idx);
+      model.addBodyFrame("shoulder1_body",idx);
+
+      idx = model.addJoint(idx,JointModelRY(),I4  ,"shoulder2_joint", vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Ijoint);
+      model.addJointFrame(idx);
+      model.addBodyFrame("shoulder2_body",idx);
+
+      idx = model.addJoint(idx,JointModelRZ(),I4  ,"shoulder3_joint", vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Iarm);
+      model.addJointFrame(idx);
+      model.addBodyFrame("upperarm_body",idx);
+
+      idx = model.addJoint(idx,JointModelRX(),Marm,"elbow_joint",     vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Iarm);
+      model.addJointFrame(idx);
+      model.addBodyFrame("lowerarm_body",idx);
+
+      idx = model.addJoint(idx,JointModelRX(),Marm,"wrist1_joint",    vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Ijoint);
+      model.addJointFrame(idx);
+      model.addBodyFrame("wrist1_body",idx);
+
+      idx = model.addJoint(idx,JointModelRZ(),I4  ,"wrist2_joint",    vmax,taumax,qmin,qmax);
+      model.appendBodyToJoint(idx,Iarm);
+      model.addJointFrame(idx);
+      model.addBodyFrame("effector_body",idx);
+
+    }
+
+    void manipulatorGeometries(const Model& model, GeometryModel & geom)
+    {
+      GeometryObject upperArm("upperarm_object",
+                              model.getBodyId("upperarm_body"),0,
+                              boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.1, 1)),
+                              SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(.5,0,0)) );
+      geom.addGeometryObject(upperArm,model,true);
+
+    }
+
+
+    
   } // namespace buildModels
   
 } // namespace se3

src/parsers/sample-models.hpp

@@ -20,6 +20,7 @@
 #define __se3_sample_models_hpp__
 
 #include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/geometry.hpp"
 
 namespace se3
 {
@@ -28,7 +29,9 @@ namespace se3
 
     void humanoid2d(Model& model);
     void humanoidSimple(Model& model, bool usingFF = true);
-
+    void manipulator(Model& model);
+    void manipulatorGeometries(const Model& model, GeometryModel & geom);
+    
   } // 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/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 )
+{
+  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_SUITE_END()