full.hpp

// BSD 3-Clause License
//
// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
// Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.

#ifndef CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
#define CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_

#include "crocoddyl/core/actuation-base.hpp"
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/multibody/fwd.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"

namespace crocoddyl {

template <typename _Scalar>
class ActuationModelFullTpl : public ActuationModelAbstractTpl<_Scalar> {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 CROCODDYL_DERIVED_CAST(ActuationModelBase, ActuationModelFullTpl)

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef ActuationModelAbstractTpl<Scalar> Base;
 typedef ActuationDataAbstractTpl<Scalar> Data;
 typedef StateAbstractTpl<Scalar> StateAbstract;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;

 explicit ActuationModelFullTpl(std::shared_ptr<StateAbstract> state)
 : Base(state, state->get_nv()) {
 StateMultibodyTpl<Scalar>* s =
 dynamic_cast<StateMultibodyTpl<Scalar>*>(state.get());
 if (s != NULL) {
 Base::u_lb_ = Scalar(-1.) * s->get_pinocchio()->effortLimit;
 Base::u_ub_ = s->get_pinocchio()->effortLimit;
 }
 };
 virtual ~ActuationModelFullTpl() = default;

 virtual void calc(const std::shared_ptr<Data>& data,
 const Eigen::Ref<const VectorXs>& /*x*/,
 const Eigen::Ref<const VectorXs>& u) override {
 if (static_cast<std::size_t>(u.size()) != nu_) {
 throw_pretty(
 "Invalid argument: " << "u has wrong dimension (it should be " +
 std::to_string(nu_) + ")");
 }
 data->tau = u;
 };

#ifndef NDEBUG
 virtual void calcDiff(const std::shared_ptr<Data>& data,
 const Eigen::Ref<const VectorXs>& /*x*/,
 const Eigen::Ref<const VectorXs>&) override {
#else
 virtual void calcDiff(const std::shared_ptr<Data>&,
 const Eigen::Ref<const VectorXs>& /*x*/,
 const Eigen::Ref<const VectorXs>&) override {
#endif
 // The derivatives has constant values which were set in createData.
 assert_pretty(data->dtau_dx.isZero(), "dtau_dx has wrong value");
 assert_pretty(MatrixXs(data->dtau_du)
 .isApprox(MatrixXs::Identity(state_->get_nv(), nu_)),
 "dtau_du has wrong value");
 };

 virtual void commands(const std::shared_ptr<Data>& data,
 const Eigen::Ref<const VectorXs>&,
 const Eigen::Ref<const VectorXs>& tau) override {
 if (static_cast<std::size_t>(tau.size()) != nu_) {
 throw_pretty(
 "Invalid argument: " << "tau has wrong dimension (it should be " +
 std::to_string(nu_) + ")");
 }
 data->u = tau;
 }

#ifndef NDEBUG
 virtual void torqueTransform(const std::shared_ptr<Data>& data,
 const Eigen::Ref<const VectorXs>&,
 const Eigen::Ref<const VectorXs>&) override {
#else
 virtual void torqueTransform(const std::shared_ptr<Data>&,
 const Eigen::Ref<const VectorXs>&,
 const Eigen::Ref<const VectorXs>&) override {
#endif
 // The torque transform has constant values which were set in createData.
 assert_pretty(MatrixXs(data->Mtau).isApprox(MatrixXs::Identity(nu_, nu_)),
 "Mtau has wrong value");
 }

 virtual std::shared_ptr<Data> createData() override {
 std::shared_ptr<Data> data =
 std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
 data->dtau_du.diagonal().setOnes();
 data->Mtau.setIdentity();
 return data;
 };

 template <typename NewScalar>
 ActuationModelFullTpl<NewScalar> cast() const {
 typedef ActuationModelFullTpl<NewScalar> ReturnType;
 ReturnType ret(state_->template cast<NewScalar>());
 return ret;
 }

 virtual void print(std::ostream& os) const override {
 os << "ActuationModelFull {nu=" << nu_ << ", nv=" << state_->get_nv()
 << "}";
 }

 protected:
 using Base::nu_;
 using Base::state_;
};

} // namespace crocoddyl

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ActuationModelFullTpl)

#endif // CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_