GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contact-base.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 33 44 75.0%
Branches: 6 32 18.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "crocoddyl/core/utils/exception.hpp"
11 #include "crocoddyl/multibody/contact-base.hpp"
12
13 namespace crocoddyl {
14
15 template <typename Scalar>
16 2059 ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl(
17 boost::shared_ptr<StateMultibody> state,
18 const pinocchio::ReferenceFrame type, const std::size_t nc,
19 const std::size_t nu)
20 2059 : state_(state), nc_(nc), nu_(nu), id_(0), type_(type) {}
21
22 template <typename Scalar>
23 22 ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl(
24 boost::shared_ptr<StateMultibody> state,
25 const pinocchio::ReferenceFrame type, const std::size_t nc)
26 22 : state_(state), nc_(nc), nu_(state->get_nv()), id_(0), type_(type) {}
27
28 template <typename Scalar>
29 ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl(
30 boost::shared_ptr<StateMultibody> state, const std::size_t nc,
31 const std::size_t nu)
32 : state_(state),
33 nc_(nc),
34 nu_(nu),
35 id_(0),
36 type_(pinocchio::ReferenceFrame::LOCAL) {
37 std::cerr << "Deprecated: Use constructor that passes the type of contact, "
38 "this assumes is pinocchio::LOCAL."
39 << std::endl;
40 }
41
42 template <typename Scalar>
43 ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl(
44 boost::shared_ptr<StateMultibody> state, const std::size_t nc)
45 : state_(state),
46 nc_(nc),
47 nu_(state->get_nv()),
48 id_(0),
49 type_(pinocchio::ReferenceFrame::LOCAL) {
50 std::cerr << "Deprecated: Use constructor that passes the type of contact, "
51 "this assumes is pinocchio::LOCAL."
52 << std::endl;
53 }
54
55 template <typename Scalar>
56 4178 ContactModelAbstractTpl<Scalar>::~ContactModelAbstractTpl() {}
57
58 template <typename Scalar>
59 86442 void ContactModelAbstractTpl<Scalar>::updateForceDiff(
60 const boost::shared_ptr<ContactDataAbstract>& data, const MatrixXs& df_dx,
61 const MatrixXs& df_du) const {
62
3/6
✓ Branch 1 taken 86442 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 86442 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 86442 times.
 172884 if (static_cast<std::size_t>(df_dx.rows()) != nc_ ||
63 86442 static_cast<std::size_t>(df_dx.cols()) != state_->get_ndx())
64 ✗ throw_pretty("df_dx has wrong dimension");
65
66
2/4
✓ Branch 1 taken 86442 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 86442 times.
 172884 if (static_cast<std::size_t>(df_du.rows()) != nc_ ||
67
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 86442 times.
 86442 static_cast<std::size_t>(df_du.cols()) != nu_)
68 ✗ throw_pretty("df_du has wrong dimension");
69
70 86442 data->df_dx = df_dx;
71 86442 data->df_du = df_du;
72 86442 }
73
74 template <typename Scalar>
75 8 void ContactModelAbstractTpl<Scalar>::setZeroForce(
76 const boost::shared_ptr<ContactDataAbstract>& data) const {
77 8 data->f.setZero();
78 8 data->fext.setZero();
79 8 }
80
81 template <typename Scalar>
82 ✗ void ContactModelAbstractTpl<Scalar>::setZeroForceDiff(
83 const boost::shared_ptr<ContactDataAbstract>& data) const {
84 ✗ data->df_dx.setZero();
85 ✗ data->df_du.setZero();
86 }
87
88 template <typename Scalar>
89 boost::shared_ptr<ContactDataAbstractTpl<Scalar> >
90 ✗ ContactModelAbstractTpl<Scalar>::createData(
91 pinocchio::DataTpl<Scalar>* const data) {
92 return boost::allocate_shared<ContactDataAbstract>(
93 ✗ Eigen::aligned_allocator<ContactDataAbstract>(), this, data);
94 }
95
96 template <typename Scalar>
97 ✗ void ContactModelAbstractTpl<Scalar>::print(std::ostream& os) const {
98 ✗ os << boost::core::demangle(typeid(*this).name());
99 }
100
101 template <typename Scalar>
102 const boost::shared_ptr<StateMultibodyTpl<Scalar> >&
103 2412744 ContactModelAbstractTpl<Scalar>::get_state() const {
104 2412744 return state_;
105 }
106
107 template <typename Scalar>
108 1283322 std::size_t ContactModelAbstractTpl<Scalar>::get_nc() const {
109 1283322 return nc_;
110 }
111
112 template <typename Scalar>
113 167129 std::size_t ContactModelAbstractTpl<Scalar>::get_nu() const {
114 167129 return nu_;
115 }
116
117 template <typename Scalar>
118 164175 pinocchio::FrameIndex ContactModelAbstractTpl<Scalar>::get_id() const {
119 164175 return id_;
120 }
121
122 template <typename Scalar>
123 182486 pinocchio::ReferenceFrame ContactModelAbstractTpl<Scalar>::get_type() const {
124 182486 return type_;
125 }
126
127 template <typename Scalar>
128 9 void ContactModelAbstractTpl<Scalar>::set_id(const pinocchio::FrameIndex id) {
129 9 id_ = id;
130 9 }
131
132 template <typename Scalar>
133 ✗ void ContactModelAbstractTpl<Scalar>::set_type(
134 const pinocchio::ReferenceFrame type) {
135 ✗ type_ = type;
136 }
137
138 template <class Scalar>
139 50 std::ostream& operator<<(std::ostream& os,
140 const ContactModelAbstractTpl<Scalar>& model) {
141 50 model.print(os);
142 50 return os;
143 }
144
145 } // namespace crocoddyl
146