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__