Merge pull request #104 from jmirabel/devel

Add thread safe API to CenterOfMassComputation.

include/hpp/pinocchio/center-of-mass-computation.hh

@@ -61,14 +61,36 @@ namespace hpp {
         void add (const JointPtr_t& rootOfSubtree);
 
         /// Compute the center of mass and Jacobian of the sub-trees.
-        void compute (const Computation_t& flag = COMPUTE_ALL);
+        /// \param flag select which values must be computed.
+        void compute (const Computation_t& flag = COMPUTE_ALL)
+        {
+          compute (robot_->d(), flag);
+        }
+
+        /// \copydoc compute(const Computation_t&)
+        /// \param data where to write results.
+        void compute (DeviceData& data, const Computation_t& flag);
 
         /// Get center of mass of the subtree.
-        const vector3_t&     com         () const { return data.com [0]; }
+        const vector3_t&     com      () const { return com     (robot_->d()); }
         /// Get mass of the sub-tree.
-        const value_type&    mass        () const { return data.mass[0]; }
+        const value_type&    mass     () const { return mass    (robot_->d()); }
         /// Get Jacobian of center of mass of the sub-tree.
-        const ComJacobian_t& jacobian    () const { return data.Jcom   ; }
+        const ComJacobian_t& jacobian () const { return jacobian(robot_->d()); }
+
+        /// \copydoc com
+        /// \param d the data where results were written
+        ///          (passed to compute(const DeviceData&, const Computation_t&)).
+        static const vector3_t&     com      (const DeviceData& d) { return d.data_->com [0]; }
+        /// \copydoc mass
+        /// \param d the data where results were written
+        ///          (passed to compute(const DeviceData&, const Computation_t&)).
+        static const value_type&    mass     (const DeviceData& d) { return d.data_->mass[0]; }
+        /// \copydoc jacobian
+        /// \param d the data where results were written
+        ///          (passed to compute(const DeviceData&, const Computation_t&)).
+        static const ComJacobian_t& jacobian (const DeviceData& d) { return d.data_->Jcom   ; }
+
         /// Get const reference to the vector of sub-tree roots.
         const JointRootIndexes_t & roots () const { return roots_; }
 
@@ -81,13 +103,6 @@ namespace hpp {
         DevicePtr_t robot_;
         // Root of the subtrees
         JointRootIndexes_t roots_;
-        // Specific pinocchio Data to store the computation results
-        Data data;
-
-        // value_type mass_;
-        // vector3_t com_;
-        // ComJacobian_t jacobianCom_;
-
     }; // class CenterOfMassComputation
   }  // namespace pinocchio
 }  // namespace hpp

include/hpp/pinocchio/device.hh

@@ -54,6 +54,7 @@ namespace hpp {
       friend class Frame;
       friend class DeviceSync;
       friend class CollisionObject;
+      friend class CenterOfMassComputation;
     public:
       /// Collision pairs between bodies
       typedef std::pair <JointPtr_t, JointPtr_t> CollisionPair_t;

src/center-of-mass-computation.cc

@@ -36,7 +36,7 @@ namespace hpp {
       return CenterOfMassComputationPtr_t (new CenterOfMassComputation (d));
     }
 
-    void CenterOfMassComputation::compute (const Computation_t& flag)
+    void CenterOfMassComputation::compute (DeviceData& d, const Computation_t& flag)
     {
       const Model& model = robot_->model();
 
@@ -45,8 +45,7 @@ namespace hpp {
       assert(computeCOM && "This does nothing");
       assert (!(computeJac && !computeCOM)); // JACOBIAN => COM
 
-      // update kinematics
-      ::pinocchio::copy(model,robot_->data(),data,0);
+      Data& data = *d.data_;
 
       data.mass[0] = 0;
       if(computeCOM) data.com[0].setZero();
@@ -146,12 +145,12 @@ namespace hpp {
     }
 
     CenterOfMassComputation::CenterOfMassComputation (const DevicePtr_t& d) :
-      robot_(d), roots_ (), //mass_ (0), jacobianCom_ (3, d->numberDof ())
-      data(d->model())
+      robot_(d), roots_ ()
     { assert (d->modelPtr()); }
 
     void CenterOfMassComputation::add (const JointPtr_t& j)
     {
+      const Data& data = robot_->data();
       JointIndex jid = j->index();
       BOOST_FOREACH( const JointIndex rootId,  roots_ )
         {