model.hpp

//
// Copyright (c) 2019 CNRS INRIA
//
 
#ifndef __pinocchio_algorithm_model_hpp__
#define __pinocchio_algorithm_model_hpp__
 
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/geometry.hpp"
 
namespace pinocchio
{
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  void
  appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
              const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
              const FrameIndex frameInModelA,
              const SE3Tpl<Scalar,Options> & aMb,
              ModelTpl<Scalar,Options,JointCollectionTpl> & model);
 
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  ModelTpl<Scalar,Options,JointCollectionTpl>
  appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
              const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
              const FrameIndex frameInModelA,
              const SE3Tpl<Scalar,Options> & aMb)
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    Model model;
    
    appendModel(modelA,modelB,frameInModelA,aMb,model);
    
    return model;
  }
 
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  void
  appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
              const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
              const GeometryModel & geomModelA,
              const GeometryModel & geomModelB,
              const FrameIndex frameInModelA,
              const SE3Tpl<Scalar,Options> & aMb,
              ModelTpl<Scalar,Options,JointCollectionTpl> & model,
              GeometryModel & geomModel);
 
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  void
  buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                    std::vector<JointIndex> list_of_joints_to_lock,
                    const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
                    ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model);
 
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  ModelTpl<Scalar,Options,JointCollectionTpl>
  buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                    const std::vector<JointIndex> & list_of_joints_to_lock,
                    const Eigen::MatrixBase<ConfigVectorType> & reference_configuration)
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    Model reduced_model;
    
    buildReducedModel(model,list_of_joints_to_lock,reference_configuration,reduced_model);
   
    return reduced_model;
  }
 
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  void
  buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                    const GeometryModel & geom_model,
                    const std::vector<JointIndex> & list_of_joints_to_lock,
                    const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
                    ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model,
                    GeometryModel & reduced_geom_model);
 
  template <typename Scalar, int Options,
            template <typename, int> class JointCollectionTpl,
            typename GeometryModelAllocator,
            typename ConfigVectorType>
  void buildReducedModel(
      const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
      const std::vector<GeometryModel,GeometryModelAllocator> &list_of_geom_models,
      const std::vector<JointIndex> &list_of_joints_to_lock,
      const Eigen::MatrixBase<ConfigVectorType> &reference_configuration,
      ModelTpl<Scalar, Options, JointCollectionTpl> &reduced_model,
      std::vector<GeometryModel,GeometryModelAllocator> &list_of_reduced_geom_models);
 
} // namespace pinocchio
 
#include "pinocchio/algorithm/model.hxx"
 
#endif // ifndef __pinocchio_algorithm_model_hpp__