GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-cop-position.hxx
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 56 0.0%
Branches: 0 70 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2025, University of Duisburg-Essen,
5 // University of Edinburgh, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 namespace crocoddyl {
11
12 template <typename _Scalar>
13 ✗ ResidualModelContactCoPPositionTpl<_Scalar>::ResidualModelContactCoPPositionTpl(
14 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
15 const CoPSupport& cref, const std::size_t nu, const bool fwddyn)
16 : Base(state, 4, nu, fwddyn ? true : false, fwddyn ? true : false, true),
17 ✗ fwddyn_(fwddyn),
18 ✗ update_jacobians_(true),
19 ✗ id_(id),
20 ✗ cref_(cref) {}
21
22 template <typename _Scalar>
23 ✗ ResidualModelContactCoPPositionTpl<_Scalar>::ResidualModelContactCoPPositionTpl(
24 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
25 const CoPSupport& cref)
26 : Base(state, 4),
27 ✗ fwddyn_(true),
28 ✗ update_jacobians_(true),
29 ✗ id_(id),
30 ✗ cref_(cref) {}
31
32 template <typename Scalar>
33 ✗ void ResidualModelContactCoPPositionTpl<Scalar>::calc(
34 const std::shared_ptr<ResidualDataAbstract>& data,
35 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
36 ✗ Data* d = static_cast<Data*>(data.get());
37
38 // Compute the residual residual r = A * f
39 ✗ data->r.noalias() = cref_.get_A() * d->contact->f.toVector();
40 }
41
42 template <typename Scalar>
43 ✗ void ResidualModelContactCoPPositionTpl<Scalar>::calc(
44 const std::shared_ptr<ResidualDataAbstract>& data,
45 const Eigen::Ref<const VectorXs>&) {
46 ✗ data->r.setZero();
47 }
48
49 template <typename Scalar>
50 ✗ void ResidualModelContactCoPPositionTpl<Scalar>::calcDiff(
51 const std::shared_ptr<ResidualDataAbstract>& data,
52 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
53 ✗ if (fwddyn_ || update_jacobians_) {
54 ✗ updateJacobians(data);
55 }
56 }
57
58 template <typename Scalar>
59 ✗ void ResidualModelContactCoPPositionTpl<Scalar>::calcDiff(
60 const std::shared_ptr<ResidualDataAbstract>& data,
61 const Eigen::Ref<const VectorXs>&) {
62 ✗ data->Rx.setZero();
63 }
64
65 template <typename Scalar>
66 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
67 ✗ ResidualModelContactCoPPositionTpl<Scalar>::createData(
68 DataCollectorAbstract* const data) {
69 ✗ std::shared_ptr<ResidualDataAbstract> d =
70 ✗ std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, data);
71 ✗ if (!fwddyn_) {
72 ✗ updateJacobians(d);
73 }
74 ✗ return d;
75 }
76
77 template <typename Scalar>
78 ✗ void ResidualModelContactCoPPositionTpl<Scalar>::updateJacobians(
79 const std::shared_ptr<ResidualDataAbstract>& data) {
80 ✗ Data* d = static_cast<Data*>(data.get());
81
82 ✗ const MatrixXs& df_dx = d->contact->df_dx;
83 ✗ const MatrixXs& df_du = d->contact->df_du;
84 ✗ const Matrix46& A = cref_.get_A();
85 ✗ data->Rx.noalias() = A * df_dx;
86 ✗ data->Ru.noalias() = A * df_du;
87 ✗ update_jacobians_ = false;
88 }
89
90 template <typename Scalar>
91 template <typename NewScalar>
92 ResidualModelContactCoPPositionTpl<NewScalar>
93 ✗ ResidualModelContactCoPPositionTpl<Scalar>::cast() const {
94 typedef ResidualModelContactCoPPositionTpl<NewScalar> ReturnType;
95 typedef StateMultibodyTpl<NewScalar> StateType;
96 ✗ ReturnType ret(
97 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
98 ✗ id_, cref_.template cast<NewScalar>(), nu_, fwddyn_);
99 ✗ return ret;
100 }
101
102 template <typename Scalar>
103 ✗ void ResidualModelContactCoPPositionTpl<Scalar>::print(std::ostream& os) const {
104 ✗ std::shared_ptr<StateMultibody> s =
105 ✗ std::static_pointer_cast<StateMultibody>(state_);
106 ✗ const Eigen::IOFormat fmt(2, Eigen::DontAlignCols, ", ", ";\n", "", "", "[",
107 "]");
108 os << "ResidualModelContactCoPPosition {frame="
109 ✗ << s->get_pinocchio()->frames[id_].name
110 ✗ << ", box=" << cref_.get_box().transpose().format(fmt) << "}";
111 }
112
113 template <typename Scalar>
114 ✗ bool ResidualModelContactCoPPositionTpl<Scalar>::is_fwddyn() const {
115 ✗ return fwddyn_;
116 }
117
118 template <typename Scalar>
119 ✗ pinocchio::FrameIndex ResidualModelContactCoPPositionTpl<Scalar>::get_id()
120 const {
121 ✗ return id_;
122 }
123
124 template <typename Scalar>
125 const CoPSupportTpl<Scalar>&
126 ✗ ResidualModelContactCoPPositionTpl<Scalar>::get_reference() const {
127 ✗ return cref_;
128 }
129
130 template <typename Scalar>
131 ✗ void ResidualModelContactCoPPositionTpl<Scalar>::set_id(
132 const pinocchio::FrameIndex id) {
133 ✗ id_ = id;
134 }
135
136 template <typename Scalar>
137 ✗ void ResidualModelContactCoPPositionTpl<Scalar>::set_reference(
138 const CoPSupport& reference) {
139 ✗ cref_ = reference;
140 ✗ update_jacobians_ = true;
141 }
142
143 } // namespace crocoddyl
144