multibody.hpp

Go to the documentation of this file.

#pragma once
 
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
 
#include "aligator/modelling/spaces/tangent-bundle.hpp"
 
namespace aligator {

template <typename _Scalar>
struct MultibodyConfiguration : public ManifoldAbstractTpl<_Scalar> {
public:
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Self = MultibodyConfiguration<Scalar>;
  using ModelType = pinocchio::ModelTpl<Scalar>;
  using Base = ManifoldAbstractTpl<Scalar>;
 
  MultibodyConfiguration(const ModelType &model)
      : model_(model) {};
  MultibodyConfiguration(const MultibodyConfiguration &) = default;
  MultibodyConfiguration &operator=(const MultibodyConfiguration &) = default;
  MultibodyConfiguration(MultibodyConfiguration &&) = default;
  MultibodyConfiguration &operator=(MultibodyConfiguration &&) = default;
 
  const ModelType &getModel() const { return model_; }
 
  bool isNormalized(const ConstVectorRef &x) const {
    return pinocchio::isNormalized(model_, x);
  }
 
  inline int nx() const { return model_.nq; }
  inline int ndx() const { return model_.nv; }
 
protected:
  ModelType model_;

 
  void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
                      VectorRef xout) const {
    pinocchio::integrate(model_, x, v, xout);
  }
 
  void Jintegrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
                       MatrixRef Jout, int arg) const {
    switch (arg) {
    case 0:
      pinocchio::dIntegrate(model_, x, v, Jout, pinocchio::ARG0);
      break;
    case 1:
      pinocchio::dIntegrate(model_, x, v, Jout, pinocchio::ARG1);
      break;
    }
  }
 
  void JintegrateTransport_impl(const ConstVectorRef &x,
                                const ConstVectorRef &v, MatrixRef Jout,
                                int arg) const {
    switch (arg) {
    case 0:
      pinocchio::dIntegrateTransport(model_, x, v, Jout, pinocchio::ARG0);
      break;
    case 1:
      pinocchio::dIntegrateTransport(model_, x, v, Jout, pinocchio::ARG1);
      break;
    default:
      break;
    }
  }
 
  void difference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
                       VectorRef vout) const {
    pinocchio::difference(model_, x0, x1, vout);
  }
 
  void Jdifference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
                        MatrixRef Jout, int arg) const {
    switch (arg) {
    case 0:
      pinocchio::dDifference(model_, x0, x1, Jout, pinocchio::ARG0);
      break;
    case 1:
      pinocchio::dDifference(model_, x0, x1, Jout, pinocchio::ARG1);
      break;
    }
  }
 
  void interpolate_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
                        const Scalar &u, VectorRef out) const {
    pinocchio::interpolate(model_, x0, x1, u, out);
  }
 
  void neutral_impl(VectorRef out) const { pinocchio::neutral(model_, out); }
 
  void rand_impl(VectorRef out) const {
    pinocchio::randomConfiguration(model_, model_.lowerPositionLimit,
                                   model_.upperPositionLimit, out);
  }

};

template <typename Scalar>
struct MultibodyPhaseSpace : TangentBundleTpl<MultibodyConfiguration<Scalar>> {
  using ConfigSpace = MultibodyConfiguration<Scalar>;
  using ModelType = typename ConfigSpace::ModelType;
 
  const ModelType &getModel() const { return this->base_.getModel(); }
 
  MultibodyPhaseSpace(const ModelType &model)
      : TangentBundleTpl<ConfigSpace>(ConfigSpace(model)) {}
  MultibodyPhaseSpace(const MultibodyPhaseSpace &) = default;
  MultibodyPhaseSpace &operator=(const MultibodyPhaseSpace &) = default;
  MultibodyPhaseSpace(MultibodyPhaseSpace &&) = default;
  MultibodyPhaseSpace &operator=(MultibodyPhaseSpace &&) = default;
};
 
} // namespace aligator