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);

} // namespace pinocchio

#include "pinocchio/algorithm/model.hxx"

#endif // ifndef __pinocchio_algorithm_model_hpp__