[wip/pinocchio] Handle partiallty 16.10 + add static method for creating shapes in Python

pinocchio/distinfo

@@ -3,3 +3,4 @@ RMD160 (pinocchio-1.2.4.tar.gz) = d47b71bbfaf05e6f254781915ff1a0f005a27f25
 Size (pinocchio-1.2.4.tar.gz) = 8992990 bytes
 SHA1 (patch-aa) = ef0ea5662acf1b226fee7232d6bbcc4fdae3abd9
 SHA1 (patch-ab) = e517d26d988713123d1d7cb6b1badd71402329e3
+SHA1 (patch-ac) = 126a46b21a39ff195bb1bad28efef51e28aed990

pinocchio/patches/patch-ac

+diff --git bindings/python/multibody/geometry-model.hpp bindings/python/multibody/geometry-model.hpp
+index 06c69b8..d311af5 100644
+--- bindings/python/multibody/geometry-model.hpp
++++ bindings/python/multibody/geometry-model.hpp
+@@ -80,6 +80,8 @@ namespace se3
+         ;
+       }
+       
++      
++
+       /* --- Expose --------------------------------------------------------- */
+       static void expose()
+       {
+diff --git bindings/python/multibody/geometry-object.hpp bindings/python/multibody/geometry-object.hpp
+index 1155c75..6e7ed0f 100644
+--- bindings/python/multibody/geometry-object.hpp
++++ bindings/python/multibody/geometry-object.hpp
+@@ -55,9 +55,20 @@ namespace se3
+         .def_readonly("meshPath", &GeometryObject::meshPath, "Absolute path to the mesh file")
+         .def_readonly("overrideMaterial", &GeometryObject::overrideMaterial, "Boolean that tells whether material information is stored in Geometry object")
+         .def_readonly("meshTexturePath", &GeometryObject::meshTexturePath, "Absolute path to the mesh texture file")
++
++          .def("CreateCapsule", &GeometryObjectPythonVisitor::maker_capsule)
++          .staticmethod("CreateCapsule")
+         ;
+       }
+ 
++      static GeometryObject maker_capsule( const double radius , const double length)
++      {
++        return GeometryObject("",FrameIndex(0),JointIndex(0),
++                              boost::shared_ptr<fcl::CollisionGeometry>(new fcl::Capsule (radius, length)),
++                              SE3::Identity());
++
++      }
++
+       static void expose()
+       {
+         bp::class_<GeometryObject>("GeometryObject",
+diff --git src/algorithm/frames.hpp src/algorithm/frames.hpp
+index 7ac9297..cda1fc3 100644
+--- src/algorithm/frames.hpp
++++ src/algorithm/frames.hpp
+@@ -55,6 +55,7 @@ namespace se3
+   /**
+    * @brief      Returns the jacobian of the frame expresssed in the world frame or
+      in the local frame depending on the template argument. 
++   *             You must first call se3::framesForwardKinematics and se3::computeJacobians to update values in data structure.
+    
+    * @remark Expressed in the local frame, the jacobian maps the joint velocity vector to the spatial velocity of the center of the frame, expressed in the frame coordinates system. Expressed in the global frame, the jacobian maps to the spatial velocity of the point coinciding with the center of the world and attached to the frame.
+    *
+@@ -129,20 +130,18 @@ namespace se3
+     
+     const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
+     
++    if(!local_frame)
++      getJacobian<local_frame>(model, data, parent, J);
++
+     // Lever between the joint center and the frame center expressed in the global frame
+     const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
+-    
+-    getJacobian<local_frame>(model, data, parent, J);
+-
+-    if (!frame.placement.isIdentity())
++      
++    for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
+     {
+-      for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
+-      {
+-        if(!local_frame)
+-          J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
+-        else
+-          J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
+-      }
++      if(!local_frame)
++        J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
++      else
++        J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
+     }
+   }
+ 
+diff --git src/parsers/urdf/model.cpp src/parsers/urdf/model.cpp
+index 6f0d65b..6c4bd5e 100644
+--- src/parsers/urdf/model.cpp
++++ src/parsers/urdf/model.cpp
+@@ -468,7 +468,7 @@ namespace se3
+             const Inertia YY = (!Y) ? Inertia::Zero() : convertFromUrdf(*Y);
+             std::cout << "Adding Body" << std::endl;
+             std::cout << "\"" << link_name << "\" connected to " << "\"" << parent_link_name << "\" throw joint " << "\"" << joint_name << "\"" << std::endl;
+-            std::cout << "joint type: " << joint_info << std::endl;
++            std::cout << "joint type: " << joint_info.str() << std::endl;
+             std::cout << "joint placement:\n" << jointPlacement;
+             std::cout << "body info: " << std::endl;
+             std::cout << "  " << "mass: " << YY.mass() << std::endl;
+diff --git unittest/frames.cpp unittest/frames.cpp
+index 0d3ac4d..0fa1334 100644
+--- unittest/frames.cpp
++++ unittest/frames.cpp
+@@ -64,46 +64,53 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
+   const SE3 & framePlacement = SE3::Random();
+   model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
+   se3::Data data(model);
++  se3::Data data_ref(model);
++  
+ 
+   VectorXd q = VectorXd::Ones(model.nq);
+   q.middleRows<4> (3).normalize();
+   VectorXd q_dot = VectorXd::Ones(model.nv);
+-  framesForwardKinematics(model, data, q);
+-
+-
+-  computeJacobians(model,data,q);
+ 
+ 
+   /// In global frame
+-
+   Data::Matrix6x Joj(6,model.nv); Joj.fill(0);
+   Data::Matrix6x Jof(6,model.nv); Jof.fill(0);
+   Model::Index idx = model.getFrameId(frame_name);
++  const Frame & frame = model.frames[idx];
++  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
+ 
++  computeJacobians(model,data,q);
++  computeJacobians(model,data_ref,q);
++  framesForwardKinematics(model,data);
+   getFrameJacobian<false>(model,data,idx,Jof);
+-  getJacobian<false>(model, data, parent_idx, Joj);
++  getJacobian<false>(model, data_ref, parent_idx, Joj);
+ 
+   Motion nu_frame(Jof*q_dot);
+   Motion nu_joint(Joj*q_dot);
++  
++  SE3 translation(SE3::Identity());
++  translation.translation(data.oMi[parent_idx].rotation()*frame.placement.translation());
++  
++  Motion nu_frame_from_nu_joint(translation.actInv(nu_joint));
+ 
+-  Motion nu_frame_from_nu_joint(nu_joint);
+-  nu_frame_from_nu_joint.linear() -= (data.oMi[parent_idx].rotation() *framePlacement.translation()).cross(nu_joint.angular());
+-
+-
+-  BOOST_CHECK(nu_frame.toVector().isApprox(nu_frame_from_nu_joint.toVector(), 1e-12));
+-
++  BOOST_CHECK(nu_frame.isApprox(nu_frame_from_nu_joint, 1e-12));
+ 
+   /// In local frame
+ 
+   Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
+   Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
+   getFrameJacobian<true>(model,data,idx,Jff);
+-  getJacobian<true>(model, data, parent_idx, Jjj);
+-
+-  nu_frame = Jff*q_dot;
+-  nu_joint = Jjj*q_dot;
+-
+-  BOOST_CHECK(nu_frame.toVector().isApprox(framePlacement.actInv(nu_joint).toVector(), 1e-12));
++  getJacobian<true>(model, data_ref, parent_idx, Jjj);
++
++  nu_frame = Motion(Jff*q_dot);
++  nu_joint = Motion(Jjj*q_dot);
++  
++  const SE3::ActionMatrix_t jXf = frame.placement.toActionMatrix();
++  Data::Matrix6x Jjj_from_frame(jXf * Jff);
++  BOOST_CHECK(Jjj_from_frame.isApprox(Jjj));
++  
++  nu_frame_from_nu_joint = frame.placement.act(nu_frame);
++  BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint), 1e-12));
+ }
+ 
+ BOOST_AUTO_TEST_SUITE_END ()