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