GCC Code Coverage Report

Directory: ./
File: include/hpp/constraints/symbolic-function.hh
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 59 65 90.8%
Branches: 48 96 50.0%
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 #ifndef HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH
30 #define HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH
31
32 #include <hpp/constraints/config.hh>
33 #include <hpp/constraints/differentiable-function.hh>
34 #include <hpp/constraints/fwd.hh>
35 #include <hpp/constraints/symbolic-calculus.hh>
36 #include <hpp/pinocchio/device.hh>
37 #include <hpp/pinocchio/liegroup-element.hh>
38
39 namespace hpp {
40 namespace constraints {
41
42 /// Wrapper of a CalculusBaseAbstract derived object into a
43 /// DifferentiableFunction \note At the moment, it is not possible to write a
44 /// CalculusBaseAbstract
45 /// derived object that uses the extra config space of the robot.
46 template <typename Expression>
47 class HPP_CONSTRAINTS_DLLAPI SymbolicFunction : public DifferentiableFunction {
48 public:
49 typedef shared_ptr<SymbolicFunction> Ptr_t;
50 typedef weak_ptr<SymbolicFunction> WkPtr_t;
51
52 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
53
54 /// Return a shared pointer to a new instance
55 1 static Ptr_t create(const std::string& name, const DevicePtr_t& robot,
56 const typename Traits<Expression>::Ptr_t expr) {
57
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 std::vector<bool> mask(expr->value().size(), true);
58
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 return create(name, robot, expr, mask);
59 1 }
60
61 /// Return a shared pointer to a new instance
62 1 static Ptr_t create(const std::string& name, const DevicePtr_t& robot,
63 const typename Traits<Expression>::Ptr_t expr,
64 std::vector<bool> mask) {
65
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 assert(mask.size() == (std::size_t)expr->value().size());
66
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 Ptr_t ptr(new SymbolicFunction(name, robot, expr, mask));
67 1 ptr->init(ptr);
68 1 return ptr;
69 }
70
71 /// Return a shared pointer to a new instance
72 52 static Ptr_t create(const std::string& name, size_type sizeInput,
73 size_type sizeInputDerivative, size_type sizeOutput,
74 const typename Traits<Expression>::Ptr_t expr) {
75
1/2
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
 52 std::vector<bool> mask(sizeOutput, true);
76
4/8
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 26 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
 104 Ptr_t ptr(new SymbolicFunction(name, sizeInput, sizeInputDerivative,
77 sizeOutput, expr, mask));
78 52 ptr->init(ptr);
79 104 return ptr;
80 52 }
81
82 108 virtual ~SymbolicFunction() {}
83
84 1 SymbolicFunction(const std::string& name, const DevicePtr_t& robot,
85 const typename Traits<Expression>::Ptr_t expr,
86 std::vector<bool> mask)
87 : DifferentiableFunction(robot->configSize(), robot->numberDof(),
88
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 LiegroupSpace::Rn(expr->value().size()), name),
89 1 robot_(robot),
90 1 expr_(expr),
91
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 3 mask_(mask) {
92 1 size_type d = robot_->extraConfigSpace().dimension();
93
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 activeParameters_.tail(d).setConstant(false);
94
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 activeDerivativeParameters_.tail(d).setConstant(false);
95 1 }
96
97 52 SymbolicFunction(const std::string& name, size_type sizeInput,
98 size_type sizeInputDerivative, size_type sizeOutput,
99 const typename Traits<Expression>::Ptr_t expr,
100 std::vector<bool> mask)
101 : DifferentiableFunction(sizeInput, sizeInputDerivative,
102 LiegroupSpace::Rn(sizeOutput), name),
103 52 robot_(),
104 52 expr_(expr),
105
3/6
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
 104 mask_(mask) {}
106
107 protected:
108 /// Compute value of error
109 ///
110 /// \param argument configuration of the robot,
111 /// \retval result error vector
112 2718 virtual void impl_compute(LiegroupElementRef result,
113 ConfigurationIn_t argument) const {
114
2/2
✓ Branch 1 taken 100 times.
✓ Branch 2 taken 1309 times.
 2718 if (robot_) {
115
1/2
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
 100 robot_->currentConfiguration(argument);
116 100 robot_->computeForwardKinematics(pinocchio::JOINT_POSITION);
117 }
118 2718 expr_->invalidate();
119
1/2
✓ Branch 3 taken 1409 times.
✗ Branch 4 not taken.
 2718 expr_->computeValue(argument);
120 2718 size_t index = 0;
121
2/2
✓ Branch 1 taken 1909 times.
✓ Branch 2 taken 1409 times.
 5936 for (std::size_t i = 0; i < mask_.size(); i++) {
122
1/2
✓ Branch 1 taken 1909 times.
✗ Branch 2 not taken.
 3218 if (mask_[i]) result.vector()[index++] = expr_->value()[i];
123 }
124 2718 }
125
126 1312 virtual void impl_jacobian(matrixOut_t jacobian,
127 ConfigurationIn_t arg) const {
128
2/2
✓ Branch 1 taken 100 times.
✓ Branch 2 taken 606 times.
 1312 if (robot_) {
129
1/2
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
 100 robot_->currentConfiguration(arg);
130 100 robot_->computeForwardKinematics(pinocchio::JOINT_POSITION |
131 pinocchio::JACOBIAN);
132 }
133 1312 expr_->invalidate();
134
1/2
✓ Branch 3 taken 706 times.
✗ Branch 4 not taken.
 1312 expr_->computeJacobian(arg);
135 1312 size_t index = 0;
136
137 1312 const typename Expression::JacobianType_t& Je(expr_->jacobian());
138 size_type nv;
139
2/2
✓ Branch 1 taken 100 times.
✓ Branch 2 taken 606 times.
 1312 if (robot_) {
140 100 size_type d = robot_->extraConfigSpace().dimension();
141 100 nv = Je.cols();
142
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 100 times.
 100 assert(robot_->numberDof() == Je.cols() + d);
143
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 100 jacobian.rightCols(d).setZero();
144 } else {
145 1212 nv = jacobian.cols();
146 }
147
148
2/2
✓ Branch 1 taken 1206 times.
✓ Branch 2 taken 706 times.
 3124 for (std::size_t i = 0; i < mask_.size(); i++) {
149
4/8
✓ Branch 1 taken 1206 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1206 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1206 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1206 times.
✗ Branch 12 not taken.
 1812 if (mask_[i]) jacobian.row(index++).head(nv) = Je.row(i);
150 }
151 1312 }
152
153 53 void init(const Ptr_t& self) { wkPtr_ = self; }
154
155 13 bool isEqual(const DifferentiableFunction& other) const {
156 ✗ const SymbolicFunction& castother =
157
0/2
✗ Branch 0 not taken.
✗ Branch 1 not taken.
 13 dynamic_cast<const SymbolicFunction&>(other);
158 ✗ if (!DifferentiableFunction::isEqual(other)) return false;
159
160 ✗ if (robot_ != castother.robot_) return false;
161 ✗ if (expr_ != castother.expr_) return false;
162 ✗ if (mask_ != castother.mask_) return false;
163
164 ✗ return true;
165 }
166
167 private:
168 WkPtr_t wkPtr_;
169 DevicePtr_t robot_;
170 typename Traits<Expression>::Ptr_t expr_;
171 std::vector<bool> mask_;
172 }; // class SymbolicFunction
173 } // namespace constraints
174 } // namespace hpp
175 #endif // HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH
176