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 |