GCC Code Coverage Report

Directory: ./
File: src/configuration-constraint.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 39 54 72.2%
Branches: 58 130 44.6%
Line Branch Exec Source
1 // Copyright (c) 2015, Joseph Mirabel
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/configuration-constraint.hh>
30 #include <hpp/pinocchio/configuration.hh>
31 #include <hpp/pinocchio/device.hh>
32 #include <hpp/pinocchio/joint-collection.hh>
33 #include <hpp/pinocchio/joint.hh>
34 #include <hpp/pinocchio/liegroup-element.hh>
35 #include <hpp/pinocchio/util.hh>
36 #include <hpp/util/debug.hh>
37 #include <hpp/util/indent.hh>
38 #include <pinocchio/multibody/liegroup/liegroup.hpp>
39 #include <pinocchio/multibody/model.hpp>
40
41 namespace hpp {
42 namespace constraints {
43
44 2 ConfigurationConstraintPtr_t ConfigurationConstraint::create(
45 const std::string& name, const DevicePtr_t& robot, ConfigurationIn_t goal,
46 std::vector<bool> mask) {
47
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 vector_t ws(vector_t::Ones(robot->numberDof()));
48
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 for (std::size_t i = 0; i < mask.size(); ++i) {
49 ✗ if (!mask[i]) ws[i] = 0;
50 }
51
52 ConfigurationConstraint* ptr =
53
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 new ConfigurationConstraint(name, robot, goal, ws);
54
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 return ConfigurationConstraintPtr_t(ptr);
55 2 }
56
57 ✗ ConfigurationConstraintPtr_t ConfigurationConstraint::create(
58 const std::string& name, const DevicePtr_t& robot, ConfigurationIn_t goal,
59 const vector_t& weights) {
60 ConfigurationConstraint* ptr =
61 ✗ new ConfigurationConstraint(name, robot, goal, weights);
62 ✗ return ConfigurationConstraintPtr_t(ptr);
63 }
64
65 2 ConfigurationConstraint::ConfigurationConstraint(const std::string& name,
66 const DevicePtr_t& robot,
67 ConfigurationIn_t goal,
68 2 const vector_t& ws)
69 : DifferentiableFunction(robot->configSize(), robot->numberDof(),
70
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 LiegroupSpace::R1(), name),
71 2 robot_(robot),
72
5/10
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
 6 weights_() {
73
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 weights(ws);
74
1/2
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 LiegroupSpacePtr_t s(LiegroupSpace::createCopy(robot->configSpace()));
75
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 s->mergeVectorSpaces();
76
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 goal_ = LiegroupElement(goal, s);
77 2 }
78
79 2 void ConfigurationConstraint::weights(const vector_t& ws) {
80
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
 2 if (ws.size() != robot_->numberDof())
81 ✗ throw std::invalid_argument(
82 "Size of weights vector should be the same "
83 ✗ "as the robot number DoFs.");
84 2 weights_ = ws;
85
86
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 activeParameters_ = ArrayXb::Constant(inputSize(), true);
87
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 activeDerivativeParameters_ = ArrayXb::Constant(weights_.size(), true);
88
89 2 const pinocchio::Model& model = robot_->model();
90
2/2
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
 56 for (int i = 1; i < model.njoints; ++i) {
91
3/6
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 54 times.
✗ Branch 9 not taken.
 54 if ((weights_.segment(model.joints[i].idx_v(), model.joints[i].nv())
92
2/4
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
 108 .array() == 0)
93
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 54 times.
 54 .all()) {
94 ✗ activeParameters_.segment(model.joints[i].idx_q(), model.joints[i].nq())
95 ✗ .setConstant(false);
96 ✗ activeDerivativeParameters_
97 ✗ .segment(model.joints[i].idx_v(), model.joints[i].nv())
98 ✗ .setConstant(false);
99 }
100 }
101 2 }
102
103 ✗ std::ostream& ConfigurationConstraint::print(std::ostream& o) const {
104 ✗ o << "ConfigurationConstraint: " << name() << incindent << iendl
105 ✗ << "weights: " << one_line(weights_);
106 ✗ return o << decindent;
107 }
108
109 413 void ConfigurationConstraint::impl_compute(LiegroupElementRef result,
110 ConfigurationIn_t argument) const {
111 using namespace hpp::pinocchio;
112
1/2
✓ Branch 2 taken 412 times.
✗ Branch 3 not taken.
 413 LiegroupElementConstRef a(argument, goal_.space());
113
4/8
✓ Branch 1 taken 414 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 419 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 412 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 414 times.
✗ Branch 12 not taken.
 412 result.vector()[0] = 0.5 * weights_.dot((goal_ - a).cwiseAbs2());
114 418 }
115
116 102 void ConfigurationConstraint::impl_jacobian(matrixOut_t jacobian,
117 ConfigurationIn_t argument) const {
118 using namespace hpp::pinocchio;
119
120
1/2
✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
 102 LiegroupElementConstRef a(argument, goal_.space());
121
6/12
✓ Branch 1 taken 99 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 95 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 101 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 104 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 97 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 94 times.
✗ Branch 18 not taken.
 102 jacobian.leftCols(robot_->numberDof()).noalias() = (goal_ - a).transpose();
122
123 // Apply jacobian of the difference on the right.
124
3/6
✓ Branch 3 taken 102 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 102 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 98 times.
✗ Branch 10 not taken.
 310 goal_.space()->dDifference_dq0<pinocchio::InputTimesDerivative>(
125
4/8
✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 102 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 98 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 103 times.
✗ Branch 12 not taken.
 203 argument, goal_.vector(), jacobian.leftCols(robot_->numberDof()));
126
127
3/6
✓ Branch 2 taken 104 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 101 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 95 times.
✗ Branch 9 not taken.
 99 jacobian.leftCols(robot_->numberDof()).array() *=
128
3/6
✓ Branch 1 taken 98 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 99 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 98 times.
✗ Branch 8 not taken.
 199 weights_.array().transpose();
129 98 }
130 } // namespace constraints
131 } // namespace hpp
132