contact-fwddyn.hpp

// BSD 3-Clause License
//
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh, CTU, INRIA,
//                          University of Oxford, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
#define CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
 
#include <stdexcept>
 
#include "crocoddyl/core/actuation-base.hpp"
#include "crocoddyl/core/constraints/constraint-manager.hpp"
#include "crocoddyl/core/costs/cost-sum.hpp"
#include "crocoddyl/core/diff-action-base.hpp"
#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
#include "crocoddyl/multibody/data/contacts.hpp"
#include "crocoddyl/multibody/fwd.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"
 
namespace crocoddyl {
 
template <typename _Scalar>
class DifferentialActionModelContactFwdDynamicsTpl
    : public DifferentialActionModelAbstractTpl<_Scalar> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef DifferentialActionModelAbstractTpl<Scalar> Base;
  typedef DifferentialActionDataContactFwdDynamicsTpl<Scalar> Data;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef StateMultibodyTpl<Scalar> StateMultibody;
  typedef CostModelSumTpl<Scalar> CostModelSum;
  typedef ConstraintModelManagerTpl<Scalar> ConstraintModelManager;
  typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
  typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
  typedef DifferentialActionDataAbstractTpl<Scalar>
      DifferentialActionDataAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  DifferentialActionModelContactFwdDynamicsTpl(
      boost::shared_ptr<StateMultibody> state,
      boost::shared_ptr<ActuationModelAbstract> actuation,
      boost::shared_ptr<ContactModelMultiple> contacts,
      boost::shared_ptr<CostModelSum> costs,
      const Scalar JMinvJt_damping = Scalar(0.),
      const bool enable_force = false);
 
  DifferentialActionModelContactFwdDynamicsTpl(
      boost::shared_ptr<StateMultibody> state,
      boost::shared_ptr<ActuationModelAbstract> actuation,
      boost::shared_ptr<ContactModelMultiple> contacts,
      boost::shared_ptr<CostModelSum> costs,
      boost::shared_ptr<ConstraintModelManager> constraints,
      const Scalar JMinvJt_damping = Scalar(0.),
      const bool enable_force = false);
  virtual ~DifferentialActionModelContactFwdDynamicsTpl();
 
  virtual void calc(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data,
      const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
 
  virtual void calc(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data,
      const Eigen::Ref<const VectorXs>& x);
 
  virtual void calcDiff(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data,
      const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
 
  virtual void calcDiff(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data,
      const Eigen::Ref<const VectorXs>& x);
 
  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
 
  virtual bool checkData(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data);
 
  virtual void quasiStatic(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data,
      Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
      const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
 
  virtual std::size_t get_ng() const;
 
  virtual std::size_t get_nh() const;
 
  virtual const VectorXs& get_g_lb() const;
 
  virtual const VectorXs& get_g_ub() const;
 
  const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;
 
  const boost::shared_ptr<ContactModelMultiple>& get_contacts() const;
 
  const boost::shared_ptr<CostModelSum>& get_costs() const;
 
  const boost::shared_ptr<ConstraintModelManager>& get_constraints() const;
 
  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
 
  const VectorXs& get_armature() const;
 
  const Scalar get_damping_factor() const;
 
  void set_armature(const VectorXs& armature);
 
  void set_damping_factor(const Scalar damping);
 
  virtual void print(std::ostream& os) const;
 
 protected:
  using Base::g_lb_;   
  using Base::g_ub_;   
  using Base::nu_;     
  using Base::state_;  
 
 private:
  void init();
  boost::shared_ptr<ActuationModelAbstract> actuation_;    
  boost::shared_ptr<ContactModelMultiple> contacts_;       
  boost::shared_ptr<CostModelSum> costs_;                  
  boost::shared_ptr<ConstraintModelManager> constraints_;  
  pinocchio::ModelTpl<Scalar>& pinocchio_;                 
  bool with_armature_;      
  VectorXs armature_;       
  Scalar JMinvJt_damping_;  
  bool enable_force_;  
};
 
template <typename _Scalar>
struct DifferentialActionDataContactFwdDynamicsTpl
    : public DifferentialActionDataAbstractTpl<_Scalar> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef DifferentialActionDataAbstractTpl<Scalar> Base;
  typedef JointDataAbstractTpl<Scalar> JointDataAbstract;
  typedef DataCollectorJointActMultibodyInContactTpl<Scalar>
      DataCollectorJointActMultibodyInContact;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  template <template <typename Scalar> class Model>
  explicit DifferentialActionDataContactFwdDynamicsTpl(
      Model<Scalar>* const model)
      : Base(model),
        pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
        multibody(
            &pinocchio, model->get_actuation()->createData(),
            boost::make_shared<JointDataAbstract>(
                model->get_state(), model->get_actuation(), model->get_nu()),
            model->get_contacts()->createData(&pinocchio)),
        costs(model->get_costs()->createData(&multibody)),
        Kinv(model->get_state()->get_nv() +
                 model->get_contacts()->get_nc_total(),
             model->get_state()->get_nv() +
                 model->get_contacts()->get_nc_total()),
        df_dx(model->get_contacts()->get_nc_total(),
              model->get_state()->get_ndx()),
        df_du(model->get_contacts()->get_nc_total(), model->get_nu()),
        tmp_xstatic(model->get_state()->get_nx()),
        tmp_Jstatic(model->get_state()->get_nv(),
                    model->get_nu() + model->get_contacts()->get_nc_total()) {
    multibody.joint->dtau_du.diagonal().setOnes();
    costs->shareMemory(this);
    if (model->get_constraints() != nullptr) {
      constraints = model->get_constraints()->createData(&multibody);
      constraints->shareMemory(this);
    }
    Kinv.setZero();
    df_dx.setZero();
    df_du.setZero();
    tmp_xstatic.setZero();
    tmp_Jstatic.setZero();
    pinocchio.lambda_c.resize(model->get_contacts()->get_nc_total());
    pinocchio.lambda_c.setZero();
  }
 
  pinocchio::DataTpl<Scalar> pinocchio;
  DataCollectorJointActMultibodyInContact multibody;
  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
  boost::shared_ptr<ConstraintDataManagerTpl<Scalar> > constraints;
  MatrixXs Kinv;
  MatrixXs df_dx;
  MatrixXs df_du;
  VectorXs tmp_xstatic;
  MatrixXs tmp_Jstatic;
 
  using Base::cost;
  using Base::Fu;
  using Base::Fx;
  using Base::Lu;
  using Base::Luu;
  using Base::Lx;
  using Base::Lxu;
  using Base::Lxx;
  using Base::r;
  using Base::xout;
};
 
}  // namespace crocoddyl
 
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include <crocoddyl/multibody/actions/contact-fwddyn.hxx>
 
#endif  // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_