GCC Code Coverage Report

Directory: ./
File: src/locked-joint.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 75 144 52.1%
Branches: 65 278 23.4%
Line Branch Exec Source
1 // Copyright (c) 2015, LAAS-CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include "hpp/constraints/locked-joint.hh"
30
31 #include <boost/serialization/weak_ptr.hpp>
32 #include <hpp/constraints/affine-function.hh>
33 #include <hpp/constraints/explicit/implicit-function.hh>
34 #include <hpp/pinocchio/configuration.hh>
35 #include <hpp/pinocchio/device.hh>
36 #include <hpp/pinocchio/joint.hh>
37 #include <hpp/util/debug.hh>
38 #include <hpp/util/serialization.hh>
39 #include <pinocchio/multibody/model.hpp>
40 #include <sstream>
41
42 namespace hpp {
43 namespace constraints {
44 namespace {
45 template <typename T>
46 12 std::string numToStr(const T& v) {
47
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 std::stringstream ss;
48
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 ss << v;
49
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 24 return ss.str();
50 }
51 } // namespace
52
53 /// Copy object and return shared pointer to copy
54
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 ImplicitPtr_t LockedJoint::copy() const { return createCopy(weak_.lock()); }
55
56 6 LockedJointPtr_t LockedJoint::create(const JointPtr_t& joint,
57 const LiegroupElement& value) {
58
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 LockedJoint* ptr = new LockedJoint(joint, value);
59 6 LockedJointPtr_t shPtr(ptr);
60
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 ptr->init(shPtr);
61 6 return shPtr;
62 }
63
64 ✗ LockedJointPtr_t LockedJoint::create(const JointPtr_t& joint,
65 const size_type index, vectorIn_t value) {
66 ✗ LockedJoint* ptr = new LockedJoint(joint, index, value);
67 ✗ LockedJointPtr_t shPtr(ptr);
68 ✗ ptr->init(shPtr);
69 ✗ return shPtr;
70 }
71
72 ✗ LockedJointPtr_t LockedJoint::create(const DevicePtr_t& dev,
73 const size_type index, vectorIn_t value) {
74 ✗ LockedJoint* ptr = new LockedJoint(dev, index, value);
75 ✗ LockedJointPtr_t shPtr(ptr);
76 ✗ ptr->init(shPtr);
77 ✗ return shPtr;
78 }
79
80 2 LockedJointPtr_t LockedJoint::createCopy(LockedJointConstPtr_t other) {
81
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 LockedJoint* ptr = new LockedJoint(*other);
82 2 LockedJointPtr_t shPtr(ptr);
83
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 ptr->init(shPtr);
84 2 return shPtr;
85 }
86
87 20 size_type LockedJoint::rankInConfiguration() const {
88 20 return outputConf_[0].first;
89 }
90
91 20 size_type LockedJoint::rankInVelocity() const {
92 20 return outputVelocity_[0].first;
93 }
94
95 ✗ size_type LockedJoint::configSize() const { return configSpace_->nq(); }
96
97 ✗ size_type LockedJoint::numberDof() const { return configSpace_->nv(); }
98
99 ✗ const LiegroupSpacePtr_t& LockedJoint::configSpace() const {
100 ✗ return configSpace_;
101 }
102
103 6 LockedJoint::LockedJoint(const JointPtr_t& joint, const LiegroupElement& value)
104 : Explicit(
105 12 joint->robot()->configSpace(),
106
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 ConstantFunction::create(value, 0, 0,
107
4/8
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
 12 "LockedJoint " + joint->name() + ' ' +
108
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 12 numToStr(value.vector().transpose())),
109 12 segments_t(), // input conf
110
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 {segment_t(joint->rankInConfiguration(),
111
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 joint->configSize())}, // output conf
112 6 segments_t(), // input vel
113
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 {segment_t(joint->rankInVelocity(),
114
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 joint->numberDof())}, // output vel
115
2/4
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
 12 ComparisonTypes_t(joint->numberDof(), Equality),
116
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 std::vector<bool>(joint->numberDof(), true)),
117
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 jointName_(joint->name()),
118 6 joint_(joint),
119
4/8
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 26 taken 6 times.
✗ Branch 27 not taken.
 54 configSpace_(joint->configurationSpace()) {
120
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
 6 assert(HPP_DYNAMIC_PTR_CAST(explicit_::ImplicitFunction, functionPtr()));
121
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 6 times.
 6 assert(rightHandSideSize() == joint->numberDof());
122
2/4
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
 6 assert(*(value.space()) == *configSpace_);
123 6 }
124
125 ✗ LockedJoint::LockedJoint(const JointPtr_t& joint, const size_type index,
126 ✗ vectorIn_t value)
127 ✗ : Explicit(joint->robot()->configSpace(),
128 ✗ ConstantFunction::create(
129 ✗ LiegroupElement(
130 ✗ value, LiegroupSpace::Rn(joint->configSize() - index)),
131 ✗ 0, 0, "partial " + joint->name()),
132 ✗ segments_t(), // input conf
133 ✗ segments_t(), // input vel
134 ✗ {segment_t(joint->rankInConfiguration(),
135 ✗ joint->configSize() - index)}, // output conf
136 ✗ {segment_t(joint->rankInVelocity(),
137 ✗ joint->numberDof() - index)}, // output vel
138 ✗ ComparisonTypes_t(joint->numberDof() - index, Equality),
139 ✗ std::vector<bool>(joint->numberDof(), true)),
140 ✗ jointName_("partial_" + joint->name()),
141 ✗ joint_(joint),
142 ✗ configSpace_(LiegroupSpace::Rn(joint->configSize() - index)) {
143 ✗ assert(HPP_DYNAMIC_PTR_CAST(explicit_::ImplicitFunction, functionPtr()));
144 ✗ assert(joint->numberDof() == joint->configSize());
145 // rightHandSide (value);
146 ✗ assert(rightHandSideSize() == value.size());
147 }
148
149 ✗ LockedJoint::LockedJoint(const DevicePtr_t& dev, const size_type index,
150 ✗ vectorIn_t value)
151 ✗ : Explicit(dev->configSpace(),
152 ✗ ConstantFunction::create(
153 ✗ LiegroupElement(value, LiegroupSpace::Rn(value.size())), 0,
154 ✗ 0, dev->name() + "_extraDof" + numToStr(index)),
155 ✗ segments_t(), // input conf
156 ✗ segments_t(), // input vel
157 ✗ {segment_t(dev->configSize() -
158 ✗ dev->extraConfigSpace().dimension() + index,
159 ✗ value.size())}, // output conf
160 ✗ {segment_t(dev->numberDof() -
161 ✗ dev->extraConfigSpace().dimension() + index,
162 ✗ value.size())}, // output vel
163 ✗ ComparisonTypes_t(value.size(), Equality),
164 ✗ std::vector<bool>(value.size(), true)),
165 ✗ jointName_(dev->name() + "_extraDof" + numToStr(index)),
166 ✗ joint_(JointPtr_t()),
167 ✗ configSpace_(LiegroupSpace::Rn(value.size())) {
168 ✗ assert(HPP_DYNAMIC_PTR_CAST(explicit_::ImplicitFunction, functionPtr()));
169 ✗ assert(value.size() > 0);
170 ✗ assert(rankInConfiguration() + value.size() <= dev->configSize());
171 // rightHandSide (value);
172 ✗ assert(rightHandSideSize() == value.size());
173 }
174
175 8 void LockedJoint::init(const LockedJointPtr_t& self) {
176
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 Explicit::init(self);
177 8 weak_ = self;
178 8 }
179
180 ✗ std::ostream& LockedJoint::print(std::ostream& os) const {
181 ✗ LiegroupElement v;
182 ✗ vector_t empty;
183 ✗ function().value(v, empty);
184 ✗ os << "Locked joint " << jointName_
185 ✗ << ", value = " << pinocchio::displayConfig(v.vector())
186 ✗ << ": rank in configuration = " << rankInConfiguration()
187 ✗ << ": rank in velocity = " << rankInVelocity() << std::endl;
188 ✗ return os;
189 }
190
191 2 LockedJoint::LockedJoint(const LockedJoint& other)
192 : Explicit(other),
193 2 jointName_(other.jointName_),
194 2 joint_(other.joint_),
195 2 configSpace_(other.configSpace_),
196
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 weak_() {}
197
198 16 bool LockedJoint::isEqual(const Implicit& other, bool swapAndTest) const {
199 try {
200
2/2
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 5 times.
 16 const LockedJoint& lj = dynamic_cast<const LockedJoint&>(other);
201
3/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 10 times.
 11 if (!Implicit::isEqual(other, false)) return false;
202
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 10 times.
 10 if (jointName_ != lj.jointName_) return false;
203
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 10 times.
 10 if (rankInConfiguration() != lj.rankInConfiguration()) return false;
204
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 10 times.
 10 if (rankInVelocity() != lj.rankInVelocity()) return false;
205
2/4
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 10 times.
 10 if (*configSpace_ != *(lj.configSpace_)) return false;
206
3/4
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 5 times.
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 10 if (swapAndTest) return lj.isEqual(*this, false);
207 5 return true;
208
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 } catch (const std::bad_cast& err) {
209 5 return false;
210 5 }
211 }
212
213 std::pair<JointConstPtr_t, JointConstPtr_t>
214 1 LockedJoint::doesConstrainRelPoseBetween(DeviceConstPtr_t robot) const {
215
2/4
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
 1 if (!robot->model().existJointName(jointName())) {
216 // Extra dofs and partial locked joints have a name that won't be
217 // recognized by Device::getJointByName. So they can be filtered
218 // this way.
219 ✗ return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
220 }
221
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 JointConstPtr_t j1 = joint_->parentJoint();
222 1 size_type index1 = Joint::index(j1); // parent joint may be universe
223 1 size_type index2 = joint_->index();
224
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (index1 <= index2) {
225 1 return std::pair<JointConstPtr_t, JointConstPtr_t>(j1, joint_);
226 } else {
227 ✗ return std::pair<JointConstPtr_t, JointConstPtr_t>(joint_, j1);
228 }
229 1 }
230
231 template <class Archive>
232 4 void LockedJoint::serialize(Archive& ar, const unsigned int version) {
233 (void)version;
234
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 ar& boost::serialization::make_nvp(
235 4 "base", boost::serialization::base_object<Explicit>(*this));
236
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 ar& BOOST_SERIALIZATION_NVP(jointName_);
237
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 ar& BOOST_SERIALIZATION_NVP(joint_);
238
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 ar& BOOST_SERIALIZATION_NVP(configSpace_);
239
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 ar& BOOST_SERIALIZATION_NVP(weak_);
240 }
241
242 HPP_SERIALIZATION_IMPLEMENT(LockedJoint);
243 } // namespace constraints
244 } // namespace hpp
245
246 BOOST_CLASS_EXPORT_IMPLEMENT(hpp::constraints::LockedJoint)
247