GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/residuals/joint-effort.hxx
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 55 0.0%
Branches: 0 96 0.0%
Line Branch Exec Source
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.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "crocoddyl/core/residuals/joint-effort.hpp"
10
11 namespace crocoddyl {
12
13 template <typename Scalar>
14 ✗ ResidualModelJointEffortTpl<Scalar>::ResidualModelJointEffortTpl(
15 std::shared_ptr<StateAbstract> state,
16 std::shared_ptr<ActuationModelAbstract> actuation, const VectorXs& uref,
17 const std::size_t nu, const bool fwddyn)
18 : Base(state, actuation->get_nu(), nu, fwddyn ? false : true,
19 fwddyn ? false : true, true),
20 ✗ actuation_(actuation),
21 ✗ uref_(uref),
22 ✗ fwddyn_(fwddyn) {
23 ✗ if (nu_ == 0) {
24 ✗ throw_pretty("Invalid argument: "
25 << "it seems to be an autonomous system, if so, don't add "
26 "this residual function");
27 }
28 }
29
30 template <typename Scalar>
31 ✗ ResidualModelJointEffortTpl<Scalar>::ResidualModelJointEffortTpl(
32 std::shared_ptr<StateAbstract> state,
33 std::shared_ptr<ActuationModelAbstract> actuation, const VectorXs& uref)
34 : Base(state, actuation->get_nu(), state->get_nv(), true, true, true),
35 ✗ actuation_(actuation),
36 ✗ uref_(uref),
37 ✗ fwddyn_(false) {}
38
39 template <typename Scalar>
40 ✗ ResidualModelJointEffortTpl<Scalar>::ResidualModelJointEffortTpl(
41 std::shared_ptr<StateAbstract> state,
42 std::shared_ptr<ActuationModelAbstract> actuation, const std::size_t nu)
43 : Base(state, actuation->get_nu(), nu, true, true, true),
44 ✗ actuation_(actuation),
45 ✗ uref_(VectorXs::Zero(actuation->get_nu())),
46 ✗ fwddyn_(false) {
47 ✗ if (nu_ == 0) {
48 ✗ throw_pretty("Invalid argument: "
49 << "it seems to be an autonomous system, if so, don't add "
50 "this residual function");
51 }
52 }
53
54 template <typename Scalar>
55 ✗ ResidualModelJointEffortTpl<Scalar>::ResidualModelJointEffortTpl(
56 std::shared_ptr<StateAbstract> state,
57 std::shared_ptr<ActuationModelAbstract> actuation)
58 : Base(state, actuation->get_nu(), state->get_nv(), true, true, true),
59 ✗ actuation_(actuation),
60 ✗ uref_(VectorXs::Zero(actuation->get_nu())) {}
61
62 template <typename Scalar>
63 ✗ void ResidualModelJointEffortTpl<Scalar>::calc(
64 const std::shared_ptr<ResidualDataAbstract>& data,
65 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
66 ✗ Data* d = static_cast<Data*>(data.get());
67 ✗ data->r = d->joint->tau - uref_;
68 }
69
70 template <typename Scalar>
71 ✗ void ResidualModelJointEffortTpl<Scalar>::calc(
72 const std::shared_ptr<ResidualDataAbstract>& data,
73 const Eigen::Ref<const VectorXs>&) {
74 ✗ if (fwddyn_) {
75 ✗ data->r.setZero();
76 } else {
77 ✗ Data* d = static_cast<Data*>(data.get());
78 ✗ data->r = d->joint->tau - uref_;
79 }
80 }
81
82 template <typename Scalar>
83 ✗ void ResidualModelJointEffortTpl<Scalar>::calcDiff(
84 const std::shared_ptr<ResidualDataAbstract>& data,
85 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
86 ✗ Data* d = static_cast<Data*>(data.get());
87 ✗ if (q_dependent_ || v_dependent_) {
88 ✗ data->Rx = d->joint->dtau_dx;
89 }
90 ✗ data->Ru = d->joint->dtau_du;
91 }
92
93 template <typename Scalar>
94 ✗ void ResidualModelJointEffortTpl<Scalar>::calcDiff(
95 const std::shared_ptr<ResidualDataAbstract>& data,
96 const Eigen::Ref<const VectorXs>&) {
97 ✗ if (fwddyn_) {
98 ✗ data->Rx.setZero();
99 } else {
100 ✗ Data* d = static_cast<Data*>(data.get());
101 ✗ data->Rx = d->joint->dtau_dx;
102 ✗ data->Ru = d->joint->dtau_du;
103 }
104 }
105
106 template <typename Scalar>
107 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
108 ✗ ResidualModelJointEffortTpl<Scalar>::createData(
109 DataCollectorAbstract* const data) {
110 ✗ std::shared_ptr<ResidualDataAbstract> d =
111 ✗ std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, data);
112 ✗ return d;
113 }
114
115 template <typename Scalar>
116 template <typename NewScalar>
117 ResidualModelJointEffortTpl<NewScalar>
118 ✗ ResidualModelJointEffortTpl<Scalar>::cast() const {
119 typedef ResidualModelJointEffortTpl<NewScalar> ReturnType;
120 ✗ ReturnType ret(state_->template cast<NewScalar>(),
121 ✗ actuation_->template cast<NewScalar>(),
122 ✗ uref_.template cast<NewScalar>(), nu_, fwddyn_);
123 ✗ return ret;
124 }
125
126 template <typename Scalar>
127 ✗ void ResidualModelJointEffortTpl<Scalar>::print(std::ostream& os) const {
128 ✗ os << "ResidualModelJointEffort";
129 }
130
131 template <typename Scalar>
132 const typename MathBaseTpl<Scalar>::VectorXs&
133 ✗ ResidualModelJointEffortTpl<Scalar>::get_reference() const {
134 ✗ return uref_;
135 }
136
137 template <typename Scalar>
138 ✗ void ResidualModelJointEffortTpl<Scalar>::set_reference(
139 const VectorXs& reference) {
140 ✗ if (static_cast<std::size_t>(reference.size()) != nr_) {
141 ✗ throw_pretty("Invalid argument: "
142 << "the joint-effort reference has wrong dimension ("
143 << reference.size()
144 << " provided - it should be " + std::to_string(nr_) + ")")
145 }
146 ✗ uref_ = reference;
147 }
148
149 } // namespace crocoddyl
150