Crocoddyl
 
Loading...
Searching...
No Matches
joint-effort.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2022-2025, Heriot-Watt University, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_CORE_RESIDUALS_JOINT_TORQUE_HPP_
10#define CROCODDYL_CORE_RESIDUALS_JOINT_TORQUE_HPP_
11
12#include "crocoddyl/core/actuation-base.hpp"
13#include "crocoddyl/core/data/joint.hpp"
14#include "crocoddyl/core/fwd.hpp"
15#include "crocoddyl/core/residual-base.hpp"
16
17namespace crocoddyl {
18
35template <typename _Scalar>
37 public:
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39 CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelJointEffortTpl)
40
41 typedef _Scalar Scalar;
49 typedef typename MathBase::VectorXs VectorXs;
50 typedef typename MathBase::MatrixXs MatrixXs;
51
62 ResidualModelJointEffortTpl(std::shared_ptr<StateAbstract> state,
63 std::shared_ptr<ActuationModelAbstract> actuation,
64 const VectorXs& uref, const std::size_t nu,
65 const bool fwddyn = false);
66
76 ResidualModelJointEffortTpl(std::shared_ptr<StateAbstract> state,
77 std::shared_ptr<ActuationModelAbstract> actuation,
78 const VectorXs& uref);
79
90 ResidualModelJointEffortTpl(std::shared_ptr<StateAbstract> state,
91 std::shared_ptr<ActuationModelAbstract> actuation,
92 const std::size_t nu);
93
105 std::shared_ptr<StateAbstract> state,
106 std::shared_ptr<ActuationModelAbstract> actuation);
107
108 virtual ~ResidualModelJointEffortTpl() = default;
109
117 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
118 const Eigen::Ref<const VectorXs>& x,
119 const Eigen::Ref<const VectorXs>& u) override;
120
125 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
126 const Eigen::Ref<const VectorXs>& x) override;
127
135 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
136 const Eigen::Ref<const VectorXs>& x,
137 const Eigen::Ref<const VectorXs>& u) override;
138
144 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
145 const Eigen::Ref<const VectorXs>& x) override;
146
150 virtual std::shared_ptr<ResidualDataAbstract> createData(
151 DataCollectorAbstract* const data) override;
152
162 template <typename NewScalar>
164
168 const VectorXs& get_reference() const;
169
173 void set_reference(const VectorXs& reference);
174
180 virtual void print(std::ostream& os) const override;
181
182 protected:
183 using Base::nr_;
184 using Base::nu_;
185 using Base::q_dependent_;
186 using Base::state_;
187 using Base::v_dependent_;
188
189 private:
190 std::shared_ptr<ActuationModelAbstract> actuation_;
191 VectorXs uref_;
192 bool fwddyn_;
193};
194
195template <typename _Scalar>
197 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
198
199 typedef _Scalar Scalar;
203
204 template <template <typename Scalar> class Model>
205 ResidualDataJointEffortTpl(Model<Scalar>* const model,
206 DataCollectorAbstract* const data)
207 : Base(model, data) {
208 // Check that proper shared data has been passed
211 if (d == nullptr) {
212 throw_pretty(
213 "Invalid argument: the shared data should be derived from "
214 "DataCollectorJoint");
215 }
216 joint = d->joint;
217 }
218 virtual ~ResidualDataJointEffortTpl() = default;
219
220 std::shared_ptr<JointDataAbstractTpl<Scalar> > joint;
221 using Base::r;
222 using Base::Ru;
223 using Base::Rx;
224 using Base::shared;
225};
226
227} // namespace crocoddyl
228
229/* --- Details -------------------------------------------------------------- */
230/* --- Details -------------------------------------------------------------- */
231/* --- Details -------------------------------------------------------------- */
232#include "crocoddyl/core/residuals/joint-effort.hxx"
233
234CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ResidualModelJointEffortTpl)
235CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ResidualDataJointEffortTpl)
236
237#endif // CROCODDYL_CORE_RESIDUALS_JOINT_TORQUE_HPP_
Abstract class for the actuation-mapping model.
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
std::size_t nr_
Residual vector dimension.
Define a joint-effort residual.
ResidualModelJointEffortTpl(std::shared_ptr< StateAbstract > state, std::shared_ptr< ActuationModelAbstract > actuation, const VectorXs &uref)
Initialize the joint-effort residual model.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the residual vector for nodes that depends only on the state.
ResidualModelJointEffortTpl(std::shared_ptr< StateAbstract > state, std::shared_ptr< ActuationModelAbstract > actuation)
Initialize the joint-effort residual model.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the derivatives of the joint-effort residual.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the joint-effort residual data.
void set_reference(const VectorXs &reference)
Modify the reference joint-effort vector.
ResidualModelJointEffortTpl(std::shared_ptr< StateAbstract > state, std::shared_ptr< ActuationModelAbstract > actuation, const std::size_t nu)
Initialize the joint-effort residual model.
ResidualModelJointEffortTpl(std::shared_ptr< StateAbstract > state, std::shared_ptr< ActuationModelAbstract > actuation, const VectorXs &uref, const std::size_t nu, const bool fwddyn=false)
Initialize the joint-effort residual model.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the joint-effort residual.
ResidualModelJointEffortTpl< NewScalar > cast() const
Cast the joint-effort residual model to a different scalar type.
virtual void print(std::ostream &os) const override
Print relevant information of the joint-effort residual.
const VectorXs & get_reference() const
Return the reference joint-effort vector.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Abstract class for the state representation.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
DataCollectorAbstract * shared
Shared data allocated by the action model.
std::shared_ptr< JointDataAbstractTpl< Scalar > > joint
Joint data.
DataCollectorAbstract * shared
Shared data allocated by the action model.