GCC Code Coverage Report


Directory: ./
File: src/distance/reeds-shepp.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 22 69 31.9%
Branches: 11 94 11.7%

Line Branch Exec Source
1 //
2 // Copyright (c) 2016 CNRS
3 // Authors: Florent Lamiraux
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #include <hpp/core/distance/reeds-shepp.hh>
31 #include <hpp/core/problem.hh>
32 #include <hpp/core/steering-method/reeds-shepp.hh>
33 #include <hpp/core/weighed-distance.hh>
34 #include <hpp/pinocchio/device.hh>
35 #include <hpp/pinocchio/joint-collection.hh>
36 #include <hpp/pinocchio/joint.hh>
37 #include <pinocchio/multibody/joint/joint-generic.hpp>
38
39 namespace hpp {
40 namespace core {
41 namespace distance {
42
43 DistancePtr_t ReedsShepp::clone() const { return createCopy(weak_.lock()); }
44
45 1 ReedsSheppPtr_t ReedsShepp::create(const ProblemConstPtr_t& problem) {
46
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
1 ReedsShepp* ptr(new ReedsShepp(problem));
47 1 ReedsSheppPtr_t shPtr(ptr);
48
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
1 ptr->init(shPtr);
49 1 return shPtr;
50 }
51 ReedsSheppPtr_t ReedsShepp::create(const ProblemConstPtr_t& problem,
52 const value_type& turningRadius,
53 JointPtr_t xyJoint, JointPtr_t rzJoint,
54 std::vector<JointPtr_t> wheels) {
55 ReedsShepp* ptr(
56 new ReedsShepp(problem, turningRadius, xyJoint, rzJoint, wheels));
57 ReedsSheppPtr_t shPtr(ptr);
58 ptr->init(shPtr);
59 return shPtr;
60 }
61
62 ReedsSheppPtr_t ReedsShepp::createCopy(const ReedsSheppPtr_t& distance) {
63 ReedsShepp* ptr(new ReedsShepp(*distance));
64 ReedsSheppPtr_t shPtr(ptr);
65 ptr->init(shPtr);
66 return shPtr;
67 }
68
69 1 ReedsShepp::ReedsShepp(const ProblemConstPtr_t& problem)
70 : Distance(),
71
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
1 weighedDistance_(WeighedDistance::create(problem->robot())),
72 1 device_(problem->robot()),
73 1 xyId_(0),
74 2 rzId_(2) {
75 1 DevicePtr_t d(device_.lock());
76
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
1 xy_ = d->getJointAtConfigRank(xyId_);
77
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
1 rz_ = d->getJointAtConfigRank(rzId_);
78
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
1 wheels_ = steeringMethod::getWheelsFromParameter(problem, rz_);
79
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 turningRadius(problem->getParameter("SteeringMethod/Carlike/turningRadius")
80
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
1 .floatValue());
81 1 }
82
83 ReedsShepp::ReedsShepp(const ProblemConstPtr_t& problem,
84 const value_type& turningRadius, JointPtr_t xyJoint,
85 JointPtr_t rzJoint, std::vector<JointPtr_t> wheels)
86 : Distance(),
87 weighedDistance_(WeighedDistance::create(problem->robot())),
88 device_(problem->robot()),
89 rho_(turningRadius),
90 xy_(xyJoint),
91 rz_(rzJoint),
92 xyId_(xy_->rankInConfiguration()),
93 wheels_(wheels),
94 weak_() {
95 if (rz_->jointModel().shortname() == "JointModelPlanar") {
96 rzId_ = rz_->rankInConfiguration() + 2;
97 } else {
98 rzId_ = rz_->rankInConfiguration();
99 }
100 }
101
102 ReedsShepp::ReedsShepp(const ReedsShepp& other)
103 : Distance(other),
104 weighedDistance_(WeighedDistance::createCopy(other.weighedDistance_)),
105 device_(other.device_),
106 rho_(other.rho_),
107 xy_(other.xy_),
108 rz_(other.rz_),
109 xyId_(other.xyId_),
110 rzId_(other.rzId_),
111 wheels_(other.wheels_),
112 weak_() {}
113
114 1 void ReedsShepp::turningRadius(const value_type& rho) {
115
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
1 if (rho <= 0)
116 throw std::invalid_argument("Turning radius must be strictly positive.");
117 1 rho_ = rho;
118 1 }
119
120 value_type ReedsShepp::impl_distance(ConfigurationIn_t q1,
121 ConfigurationIn_t q2) const {
122 // TODO this should not be done here.
123 // See todo in class ConstantCurvature
124 Configuration_t qEnd(q2);
125 qEnd.segment<2>(xyId_) = q1.segment<2>(xyId_);
126 qEnd.segment<2>(rzId_) = q1.segment<2>(rzId_);
127 // Do not take into account wheel joints in additional distance.
128 for (std::vector<JointPtr_t>::const_iterator it = wheels_.begin();
129 it != wheels_.end(); ++it) {
130 size_type i = (*it)->rankInConfiguration();
131 qEnd[i] = q1[i];
132 }
133 // The length corresponding to the non RS DoF
134 value_type extraL = (*weighedDistance_)(q1, qEnd);
135
136 value_type distance;
137 PathVectorPtr_t path(steeringMethod::reedsSheppPathOrDistance(
138 device_.lock(), q1, q2, extraL, rho_, xyId_, rzId_, wheels_,
139 ConstraintSetPtr_t(), true, distance));
140 return distance;
141 }
142
143 1 void ReedsShepp::init(const ReedsSheppWkPtr_t& weak) { weak_ = weak; }
144
145 } // namespace distance
146 } // namespace core
147 } // namespace hpp
148