relative-motion.hh

Go to the documentation of this file.

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

#ifndef HPP_CORE_RELATIVE_MOTION_HH
#define HPP_CORE_RELATIVE_MOTION_HH

#include <Eigen/Core>

#include <hpp/pinocchio/fwd.hh>
#include <hpp/core/fwd.hh>
#include <hpp/pinocchio/joint.hh>

namespace hpp {
  namespace core {
    struct RelativeMotion {
      enum RelativeMotionType {
        Constrained = 0,
        Parameterized = 1,
        Unconstrained = 2
      };

      typedef Eigen::Matrix<RelativeMotionType, Eigen::Dynamic, Eigen::Dynamic> matrix_type;

      static matrix_type matrix (const DevicePtr_t& robot);

      static void fromConstraint (
          matrix_type& matrix,
          const DevicePtr_t& robot,
          const ConstraintSetPtr_t& constraint);

      static void recurseSetRelMotion(matrix_type& matrix,
          const size_type& i1, const size_type& i2,
          const RelativeMotionType& type);

      static inline size_type idx(const JointConstPtr_t& joint)
      {
        return (joint ? joint->index() : 0);
      }
    };
  } // namespace core
} // namespace hpp

namespace Eigen {
  template<> struct NumTraits<hpp::core::RelativeMotion::RelativeMotionType>
    : NumTraits<int> {};
} // namespace Eigen

#endif // HPP_CORE_RELATIVE_MOTION_HH