Merge pull request #357 from jcarpent/devel

Multiple fixes

bindings/python/multibody/joint/joint.hpp

@@ -22,6 +22,7 @@
 #include <eigenpy/eigenpy.hpp>
 
 #include "pinocchio/multibody/joint/joint.hpp"
+#include "pinocchio/bindings/python/utils/printable.hpp"
 
 namespace se3
 {
@@ -48,6 +49,7 @@ namespace se3
         .add_property("nv",&JointModelPythonVisitor::getNv)
 
         .def("setIndexes",&JointModel::setIndexes)
+        .def("shortname",&JointModel::shortname)
         ;
       }
 
@@ -64,6 +66,7 @@ namespace se3
                                bp::no_init)
         .def(bp::init<se3::JointModelVariant>())
         .def(JointModelPythonVisitor())
+        .def(PrintableVisitor<JointModel>())
         ;
       }
 

bindings/python/multibody/model.hpp

@@ -27,6 +27,7 @@
 #include "pinocchio/parsers/sample-models.hpp"
 #include "pinocchio/bindings/python/utils/eigen_container.hpp"
 #include "pinocchio/bindings/python/utils/printable.hpp"
+#include "pinocchio/bindings/python/utils/copyable.hpp"
 
 EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(se3::Model)
 
@@ -214,7 +215,8 @@ namespace se3
                           "Articulated rigid body model (const)",
                           bp::no_init)
         .def(ModelPythonVisitor())
-        .def(PrintableVisitor<SE3>())
+        .def(PrintableVisitor<Model>())
+        .def(CopyableVisitor<Model>())
         ;
       
       }

bindings/python/scripts/robot_wrapper.py

@@ -58,12 +58,8 @@ class RobotWrapper(object):
         self.q0 = utils.zero(self.nq)
 
     def increment(self, q, dq):
-        M = se3.SE3(se3.Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).matrix(), q[:3])
-        dM = exp(dq[:6])
-        M = M * dM
-        q[:3] = M.translation
-        q[3:7] = se3.Quaternion(M.rotation).coeffs()
-        q[7:] += dq[6:]
+        q_next = se3.integrate(self.model,q,dq)
+        q[:] = q_next[:]
 
     @property
     def nq(self):

src/algorithm/frames.hpp

@@ -84,7 +84,9 @@ namespace se3
                                       Data & data
                                       )
   {
-    for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nframes; ++i)
+    // The following for loop starts by index 1 because the first frame is fixed
+    // and corresponds to the universe.s
+    for (Model::FrameIndex i=1; i < (Model::FrameIndex) model.nframes; ++i)
     {
       const Frame & frame = model.frames[i];
       const Model::JointIndex & parent = frame.parent;

src/algorithm/kinematics.hpp

@@ -33,6 +33,18 @@ namespace se3
   inline void emptyForwardPass(const Model & model,
                                Data & data);
   
+  ///
+  /// \brief Update the global placement of the joints oMi according to the relative
+  ///        placements of the joints.
+  ///
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  ///
+  /// \remark This algorithm may be useful to call to update global joint placement
+  ///         after calling se3::rnea, se3::aba, etc for example.
+  ///
+  inline void updateGlobalPlacements(const Model & model, Data & data);
+  
   ///
   /// \brief Update the joint placement according to the current joint configuration.
   ///

src/algorithm/kinematics.hxx

@@ -53,6 +53,19 @@ namespace se3
     }
   }
   
+  inline void updateGlobalPlacements(const Model & model, Data & data)
+  {
+    for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i)
+    {
+      const Model::JointIndex & parent = model.parents[i];
+      
+      if (parent>0)
+        data.oMi[i] = data.oMi[parent] * data.liMi[i];
+      else
+        data.oMi[i] = data.liMi[i];
+    }
+  }
+  
   struct ForwardKinematicZeroStep : public fusion::JointVisitor<ForwardKinematicZeroStep>
   {
     typedef boost::fusion::vector<const se3::Model &,

src/multibody/model.hxx

@@ -291,6 +291,7 @@ namespace se3
     f[0].setZero();
     oMi[0].setIdentity();
     liMi[0].setIdentity();
+    oMf[0].setIdentity();
   }
 
   inline void Data::computeLastChild (const Model & model)

src/spatial/fcl-pinocchio-conversions.hpp

 //
-// Copyright (c) 2015 CNRS
+// Copyright (c) 2015-2016 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -18,10 +18,8 @@
 #ifndef __se3_fcl_convertion_hpp__
 #define __se3_fcl_convertion_hpp__
 
-#include <Eigen/Geometry>
-
+#include <hpp/fcl/math/transform.h>
 #include "pinocchio/spatial/se3.hpp"
-# include <hpp/fcl/math/transform.h>
 
 namespace se3
 {
@@ -45,27 +43,24 @@ namespace se3
 
   inline fcl::Vec3f toFclVec3f(const Eigen::Vector3d & vec)
   {
-    return fcl::Vec3f( vec(0),vec(1), vec(2));
+    return fcl::Vec3f(vec(0),vec(1),vec(2));
   }
 
   inline Eigen::Vector3d toVector3d(const fcl::Vec3f & vec)
   {
-    Eigen::Vector3d res;
-    res << vec[0],vec[1], vec[2];
-    return res;
+    return Eigen::Vector3d(vec[0],vec[1],vec[2]);
   }
 
-  inline fcl::Transform3f toFclTransform3f(const se3::SE3 & m)
+  inline fcl::Transform3f toFclTransform3f(const SE3 & m)
   {
     return fcl::Transform3f(toFclMatrix3f(m.rotation()), toFclVec3f(m.translation()));
   }
 
-  inline se3::SE3 toPinocchioSE3(const fcl::Transform3f & tf)
+  inline SE3 toPinocchioSE3(const fcl::Transform3f & tf)
   {
-    return se3::SE3(toMatrix3d(tf.getRotation()), toVector3d(tf.getTranslation()));
+    return SE3(toMatrix3d(tf.getRotation()), toVector3d(tf.getTranslation()));
   }
 
 } // namespace se3
 
 #endif // ifndef __se3_fcl_convertion_hpp__
-

src/spatial/se3.hpp

@@ -277,8 +277,8 @@ namespace se3
       return d.se3ActionInverse(*this);
     }
 
-    Vector3 act_impl   (const Vector3& p) const { return rot*p+trans; }
-    Vector3 actInv_impl(const Vector3& p) const { return rot.transpose()*(p-trans); }
+    Vector3 act_impl   (const Vector3& p) const { return Vector3(rot*p+trans); }
+    Vector3 actInv_impl(const Vector3& p) const { return Vector3(rot.transpose()*(p-trans)); }
 
     SE3Tpl act_impl    (const SE3Tpl& m2) const { return SE3Tpl( rot*m2.rot,trans+rot*m2.trans);}
     SE3Tpl actInv_impl (const SE3Tpl& m2) const { return SE3Tpl( rot.transpose()*m2.rot, rot.transpose()*(m2.trans-trans));}