Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2017 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 |
|
|
#ifndef HPP_CORE_STEERING_METHOD_CONSTANT_CURVATURE_HH |
31 |
|
|
#define HPP_CORE_STEERING_METHOD_CONSTANT_CURVATURE_HH |
32 |
|
|
|
33 |
|
|
#include <hpp/core/fwd.hh> |
34 |
|
|
#include <hpp/core/path.hh> |
35 |
|
|
#include <hpp/core/steering-method/fwd.hh> |
36 |
|
|
|
37 |
|
|
namespace hpp { |
38 |
|
|
namespace core { |
39 |
|
|
namespace steeringMethod { |
40 |
|
|
/// Path of constant curvature for a carlike robot |
41 |
|
|
class ConstantCurvature : public Path { |
42 |
|
|
public: |
43 |
|
|
typedef Path parent_t; |
44 |
|
52 |
virtual ~ConstantCurvature() {} |
45 |
|
|
/// Create instance and return shared pointer. |
46 |
|
|
/// \param robot the carlike robot, |
47 |
|
|
/// \param init, end Initial and final configurations of the path, |
48 |
|
|
/// \param curveLength distance traveled by the middle of the rear |
49 |
|
|
/// wheel axis, negative values correspond to backward motions |
50 |
|
|
/// \param pathLength length of the interval of definition. Should be |
51 |
|
|
/// positive, |
52 |
|
|
/// \param curvature curvature of the path, |
53 |
|
|
/// \param xyId id of degrees of freedom corresponding to (x,y) |
54 |
|
|
/// coordinates of robot, |
55 |
|
|
/// \param rzId id of degrees of freedom corresponding to orientation |
56 |
|
|
/// of robot. |
57 |
|
|
/// \param rz joint corresponding to orientation of robot, |
58 |
|
|
/// \param wheels vector of joints corresponding to wheels, |
59 |
|
|
/// \param constraints set of contraints the path is suject to. |
60 |
|
|
static ConstantCurvaturePtr_t create( |
61 |
|
|
const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end, |
62 |
|
|
value_type curveLength, value_type pathLength, value_type curvature, |
63 |
|
|
size_type xyId, size_type rzId, const JointPtr_t rz, |
64 |
|
|
const std::vector<JointPtr_t> wheels, |
65 |
|
|
const ConstraintSetPtr_t& constraints); |
66 |
|
|
|
67 |
|
|
/// Create instance and return shared pointer. |
68 |
|
|
static ConstantCurvaturePtr_t createCopy(const ConstantCurvaturePtr_t& other); |
69 |
|
|
|
70 |
|
|
/// Create instance and return shared pointer. |
71 |
|
|
static ConstantCurvaturePtr_t createCopy( |
72 |
|
|
const ConstantCurvaturePtr_t& other, |
73 |
|
|
const ConstraintSetPtr_t& constraints); |
74 |
|
|
|
75 |
|
|
/// Return a shared pointer to a copy of this |
76 |
|
|
virtual PathPtr_t copy() const; |
77 |
|
|
|
78 |
|
|
/// Return a shared pointer to a copy of this with constraints |
79 |
|
|
/// \param constraints constraints to apply to the copy |
80 |
|
|
/// \pre *this should not have constraints. |
81 |
|
✗ |
virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const { |
82 |
|
✗ |
return createCopy(weak_.lock(), constraints); |
83 |
|
|
} |
84 |
|
|
|
85 |
|
|
/// Get the initial configuration |
86 |
|
✗ |
inline Configuration_t initial() const { return initial_; } |
87 |
|
|
|
88 |
|
|
/// Get the final configuration |
89 |
|
18 |
inline Configuration_t end() const { return end_; } |
90 |
|
|
|
91 |
|
|
virtual PathPtr_t reverse() const; |
92 |
|
|
|
93 |
|
|
struct Wheels_t { |
94 |
|
|
value_type value; // Constant value of the wheel angle |
95 |
|
|
JointPtr_t j; |
96 |
|
2 |
Wheels_t() : j() {} |
97 |
|
|
}; |
98 |
|
|
|
99 |
|
|
protected: |
100 |
|
|
/// Print path in a stream |
101 |
|
|
virtual std::ostream& print(std::ostream& os) const; |
102 |
|
|
|
103 |
|
|
/// Constructor |
104 |
|
|
/// \param robot the carlike robot, |
105 |
|
|
/// \param init, end Initial and final configurations of the path, |
106 |
|
|
/// \param curveLength distance traveled by the middle of the rear |
107 |
|
|
/// wheel axis, negative values correspond to backward motions |
108 |
|
|
/// \param pathLength length of the interval of definition. Should be |
109 |
|
|
/// positive, |
110 |
|
|
/// \param curvature curvature of the path, |
111 |
|
|
/// \param xyId id of degrees of freedom corresponding to (x,y) |
112 |
|
|
/// coordinates of robot, |
113 |
|
|
/// \param rzId id of degrees of freedom corresponding to orientation |
114 |
|
|
/// of robot. |
115 |
|
|
/// \param rz joint corresponding to orientation of robot, |
116 |
|
|
/// \param wheels vector of joints corresponding to wheels. |
117 |
|
|
ConstantCurvature(const DevicePtr_t& robot, ConfigurationIn_t init, |
118 |
|
|
ConfigurationIn_t end, value_type curveLength, |
119 |
|
|
value_type pathLength, value_type curvature, size_type xyId, |
120 |
|
|
size_type rzId, const JointPtr_t rz, |
121 |
|
|
const std::vector<JointPtr_t> wheels); |
122 |
|
|
|
123 |
|
|
/// Constructor |
124 |
|
|
/// \param robot the carlike robot, |
125 |
|
|
/// \param init, end Initial and final configurations of the path, |
126 |
|
|
/// \param curveLength distance traveled by the middle of the rear |
127 |
|
|
/// wheel axis, negative values correspond to backward motions |
128 |
|
|
/// \param pathLength length of the interval of definition. Should be |
129 |
|
|
/// positive, |
130 |
|
|
/// \param curvature curvature of the path, |
131 |
|
|
/// \param xyId id of degrees of freedom corresponding to (x,y) |
132 |
|
|
/// coordinates of robot, |
133 |
|
|
/// \param rzId id of degrees of freedom corresponding to orientation |
134 |
|
|
/// of robot. |
135 |
|
|
/// \param rz joint corresponding to orientation of robot, |
136 |
|
|
/// \param wheels vector of joints corresponding to wheels, |
137 |
|
|
/// \param constraints set of contraints the path is suject to. |
138 |
|
|
ConstantCurvature(const DevicePtr_t& robot, ConfigurationIn_t init, |
139 |
|
|
ConfigurationIn_t end, value_type curveLength, |
140 |
|
|
value_type pathLength, value_type curvature, size_type xyId, |
141 |
|
|
size_type rzId, const JointPtr_t rz, |
142 |
|
|
const std::vector<JointPtr_t> wheels, |
143 |
|
|
ConstraintSetPtr_t constraints); |
144 |
|
|
|
145 |
|
|
/// Copy constructor |
146 |
|
|
ConstantCurvature(const ConstantCurvature& other); |
147 |
|
|
|
148 |
|
|
/// Copy constructor with constraints |
149 |
|
|
ConstantCurvature(const ConstantCurvature& other, |
150 |
|
|
const ConstraintSetPtr_t& constraints); |
151 |
|
|
|
152 |
|
|
virtual bool impl_compute(ConfigurationOut_t result, value_type param) const; |
153 |
|
|
/// Virtual implementation of derivative |
154 |
|
|
virtual void impl_derivative(vectorOut_t result, const value_type& param, |
155 |
|
|
size_type order) const; |
156 |
|
|
/// Virtual implementation of velocity bound |
157 |
|
|
virtual void impl_velocityBound(vectorOut_t bound, const value_type& param0, |
158 |
|
|
const value_type& param1) const; |
159 |
|
|
|
160 |
|
|
/// Virtual implementation of path extraction |
161 |
|
|
virtual PathPtr_t impl_extract(const interval_t& paramInterval) const; |
162 |
|
|
|
163 |
|
|
/// store weak pointer to itself |
164 |
|
29 |
void init(const ConstantCurvatureWkPtr_t& weak) { |
165 |
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
29 |
parent_t::init(weak); |
166 |
|
29 |
weak_ = weak; |
167 |
|
29 |
} |
168 |
|
|
|
169 |
|
|
/// For serialization only. |
170 |
|
✗ |
ConstantCurvature() : curvature_(0), xyId_(0), rzId_(0) {} |
171 |
|
|
|
172 |
|
|
private: |
173 |
|
|
/// Set the wheel joints for a car-like vehicle. |
174 |
|
|
/// |
175 |
|
|
/// \param rz joint from which the turning radius was computed. |
176 |
|
|
/// \param wheels bounded rotation joints. |
177 |
|
|
void setWheelJoints(const JointPtr_t rz, |
178 |
|
|
const std::vector<JointPtr_t> wheels); |
179 |
|
|
|
180 |
|
|
DevicePtr_t robot_; |
181 |
|
|
Configuration_t initial_; |
182 |
|
|
Configuration_t end_; |
183 |
|
|
value_type curveLength_; |
184 |
|
|
const value_type curvature_; |
185 |
|
|
const size_type xyId_, rzId_; |
186 |
|
|
size_type dxyId_, drzId_; |
187 |
|
|
value_type forward_; |
188 |
|
|
std::vector<Wheels_t> wheels_; |
189 |
|
|
ConstantCurvatureWkPtr_t weak_; |
190 |
|
|
|
191 |
|
|
HPP_SERIALIZABLE(); |
192 |
|
|
}; // class ConstantCurvature |
193 |
|
|
} // namespace steeringMethod |
194 |
|
|
} // namespace core |
195 |
|
|
} // namespace hpp |
196 |
|
|
|
197 |
|
|
#endif // HPP_CORE_STEERING_METHOD_CONSTANT_CURVATURE_HH |
198 |
|
|
|