reachable-workspace.hpp

//
// Copyright (c) 2016-2023 CNRS INRIA
//
 
#pragma once
 
// IWYU pragma: begin_keep
#include <Eigen/Core>
#include <Eigen/SparseCore>
 
// #include <vector>
#include <cmath>
 
#include <boost/math/special_functions/factorials.hpp>
 
#include "pinocchio/macros.hpp"
 
#include "pinocchio/multibody.hpp"
#include "pinocchio/multibody/liegroup.hpp"
 
#include "pinocchio/algorithm/frames.hpp"
 
#include "pinocchio/extra/config.hpp"
 
#ifdef PINOCCHIO_WITH_COLLISION
  #include "pinocchio/collision/collision.hpp"
#endif // PINOCCHIO_WITH_COLLISION
// IWYU pragma: end_keep
 
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_COLLISION
  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_COLLISION
 
  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);
 
 
    inline void computeJointVel(
      const Eigen::VectorXd & res1,
      const Eigen::VectorXd & res2,
      const Eigen::VectorXi & comb,
      Eigen::VectorXd & qv);
 
 
    inline void generateCombination(const int n, const int k, Eigen::VectorXi & indices);
 
 
    inline void productCombination(
      const Eigen::VectorXd & element,
      const int repeat,
      Eigen::VectorXi & indices,
      Eigen::VectorXd & combination);
  } // namespace internal
} // namespace pinocchio
 
// IWYU pragma: begin_exports
#include "pinocchio/src/extra/reachable-workspace.hxx"
// IWYU pragma: end_exports