center-of-mass-computation.hh

Go to the documentation of this file.

// Copyright (c) 2016, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-pinocchio.
// hpp-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.
//
// hpp-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
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.

#ifndef HPP_PINOCCHIO_CENTER_OF_MASS_COMPUTATION_HH
# define HPP_PINOCCHIO_CENTER_OF_MASS_COMPUTATION_HH

# include <list>

# include <pinocchio/multibody/data.hpp> // ::pinocchio::Data

# include <hpp/pinocchio/fwd.hh>
# include <hpp/pinocchio/device.hh>

namespace hpp {
  namespace pinocchio {
    class CenterOfMassComputation
    {
      public:
        typedef std::vector <std::size_t> JointRootIndexes_t;
        // This fixes an alignment issue of ::pinocchio::Data::hg
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      public:
        static CenterOfMassComputationPtr_t create (const DevicePtr_t& device);

        void add (const JointPtr_t& rootOfSubtree);

        void compute (const Computation_t& flag = COMPUTE_ALL)
        {
          compute (robot_->d(), flag);
        }

        void compute (DeviceData& data, const Computation_t& flag);

        const vector3_t&     com      () const { return com     (robot_->d()); }
        const value_type&    mass     () const { return mass    (robot_->d()); }
        const ComJacobian_t& jacobian () const { return jacobian(robot_->d()); }

        static const vector3_t&     com      (const DeviceData& d) { return d.data_->com [0]; }
        static const value_type&    mass     (const DeviceData& d) { return d.data_->mass[0]; }
        static const ComJacobian_t& jacobian (const DeviceData& d) { return d.data_->Jcom   ; }

        const JointRootIndexes_t & roots () const { return roots_; }

        ~CenterOfMassComputation ();

      protected:
        CenterOfMassComputation (const DevicePtr_t& device);

      private:
        DevicePtr_t robot_;
        // Root of the subtrees
        JointRootIndexes_t roots_;
    }; // class CenterOfMassComputation
  }  // namespace pinocchio
}  // namespace hpp
#endif // HPP_PINOCCHIO_CENTER_OF_MASS_COMPUTATION_HH