joint.hpp

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

#ifndef CROCODDYL_CORE_DATA_JOINT_HPP_
#define CROCODDYL_CORE_DATA_JOINT_HPP_

#include "crocoddyl/core/data-collector-base.hpp"
#include "crocoddyl/core/data/actuation.hpp"
#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/state-base.hpp"

namespace crocoddyl {

template <typename _Scalar>
struct JointDataAbstractTpl {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef StateAbstractTpl<Scalar> StateAbstract;
 typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;

 JointDataAbstractTpl(std::shared_ptr<StateAbstract> state,
 std::shared_ptr<ActuationModelAbstract> actuation,
 const std::size_t nu)
 : tau(actuation->get_nu()),
 a(state->get_nv()),
 dtau_dx(actuation->get_nu(), state->get_ndx()),
 dtau_du(actuation->get_nu(), nu),
 da_dx(state->get_nv(), state->get_ndx()),
 da_du(state->get_nv(), nu) {
 tau.setZero();
 a.setZero();
 dtau_dx.setZero();
 dtau_du.setZero();
 da_dx.setZero();
 da_du.setZero();
 }
 virtual ~JointDataAbstractTpl() {}

 VectorXs tau;
 VectorXs a;
 MatrixXs dtau_dx;
 MatrixXs dtau_du;
 MatrixXs da_dx;
 MatrixXs da_du;
};

template <typename Scalar>
struct DataCollectorJointTpl : virtual DataCollectorAbstractTpl<Scalar> {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 DataCollectorJointTpl(std::shared_ptr<JointDataAbstractTpl<Scalar> > joint)
 : DataCollectorAbstractTpl<Scalar>(), joint(joint) {}
 virtual ~DataCollectorJointTpl() {}

 std::shared_ptr<JointDataAbstractTpl<Scalar> > joint;
};

template <typename Scalar>
struct DataCollectorJointActuationTpl : DataCollectorActuationTpl<Scalar> {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 DataCollectorJointActuationTpl(
 std::shared_ptr<ActuationDataAbstractTpl<Scalar> > actuation,
 std::shared_ptr<JointDataAbstractTpl<Scalar> > joint)
 : DataCollectorActuationTpl<Scalar>(actuation), joint(joint) {}
 virtual ~DataCollectorJointActuationTpl() {}

 std::shared_ptr<JointDataAbstractTpl<Scalar> > joint;
};

} // namespace crocoddyl

CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::JointDataAbstractTpl)
CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::DataCollectorJointTpl)
CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
 crocoddyl::DataCollectorJointActuationTpl)

#endif // CROCODDYL_CORE_DATA_JOINT_HPP_