reachable-workspace.hpp

//
// Copyright (c) 2016-2023 CNRS INRIA
//
 
#ifndef __pinocchio_extra_reachable_workspace_hpp__
#define __pinocchio_extra_reachable_workspace_hpp__
 
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/extra/config.hpp"
 
#ifdef PINOCCHIO_WITH_HPP_FCL
  #include "pinocchio/collision/collision.hpp"
#endif // PINOCCHIO_WITH_HPP_FCL
 
#include <Eigen/Core>
 
namespace pinocchio
{
  struct ReachableSetResults
  {
    Eigen::MatrixXd vertex;
    Eigen::MatrixXi faces;
  };
 
  struct ReachableSetParams
  {
    int n_samples = 5;
    int facet_dims = 3;
  };
 
 
 
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType>
  void reachableWorkspace(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q0,
    const double time_horizon,
    const int frame_id,
    Eigen::MatrixXd & vertex,
    const ReachableSetParams & params = ReachableSetParams());
 
 
 
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType>
  void reachableWorkspaceHull(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q0,
    const double time_horizon,
    const int frame_id,
    ReachableSetResults & res,
    const ReachableSetParams & params = ReachableSetParams());
 
#ifdef PINOCCHIO_WITH_HPP_FCL
 
 
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType>
  void reachableWorkspaceWithCollisions(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const GeometryModel & geom_model,
    const Eigen::MatrixBase<ConfigVectorType> & q0,
    const double time_horizon,
    const int frame_id,
    Eigen::MatrixXd & vertex,
    const ReachableSetParams & params = ReachableSetParams());
 
 
 
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType>
  void reachableWorkspaceWithCollisionsHull(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const GeometryModel & geom_model,
    const Eigen::MatrixBase<ConfigVectorType> & q0,
    const double time_horizon,
    const int frame_id,
    ReachableSetResults & res,
    const ReachableSetParams & params = ReachableSetParams());
#endif // PINOCCHIO_WITH_HPP_FCL
 
  namespace internal
  {
 
    template<
      typename Scalar,
      int Options,
      template<typename, int> class JointCollectionTpl,
      typename ConfigVectorType,
      class FilterFunction>
    void computeVertex(
      const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
      const Eigen::MatrixBase<ConfigVectorType> & q0,
      const double time_horizon,
      const int frame_id,
      FilterFunction config_filter,
      Eigen::MatrixXd & vertex,
      const ReachableSetParams & params = ReachableSetParams());
 
    PINOCCHIO_EXTRA_DLLAPI void buildConvexHull(ReachableSetResults & res);
 
 
    void computeJointVel(
      const Eigen::VectorXd & res1,
      const Eigen::VectorXd & res2,
      const Eigen::VectorXi & comb,
      Eigen::VectorXd & qv);
 
 
    void generateCombination(const int n, const int k, Eigen::VectorXi & indices);
 
 
    void productCombination(
      const Eigen::VectorXd & element,
      const int repeat,
      Eigen::VectorXi & indices,
      Eigen::VectorXd & combination);
  } // namespace internal
} // namespace pinocchio
 
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/extra/reachable-workspace.hxx"
 
#endif // ifndef __pinocchio_extra_reachable_workspace_hpp__