hpp-core: include/hpp/core/dubins-path.hh Source File

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
dubins-path.hh
Go to the documentation of this file.
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_DUBINS_PATH_HH
31#define HPP_CORE_DUBINS_PATH_HH
32
33#include <hpp/core/config.hh>
34#include <hpp/core/fwd.hh>
35#include <hpp/core/path-vector.hh>
36#include <hpp/pinocchio/device.hh>
37
38namespace hpp {
39namespace core {
42
56 class DubinsPath : public PathVector {
57 public:
58 typedef core::PathVector parent_t;
59
61 virtual ~DubinsPath() {}
62
72 static DubinsPathPtr_t create(const DevicePtr_t& device,
73 ConfigurationIn_t init, ConfigurationIn_t end,
74 value_type extraLength, value_type rho,
75 size_type xyId, size_type rzId,
76 const std::vector<JointPtr_t> wheels);
87 static DubinsPathPtr_t create(const DevicePtr_t& device,
88 ConfigurationIn_t init, ConfigurationIn_t end,
89 value_type extraLength, value_type rho,
90 size_type xyId, size_type rzId,
91 const std::vector<JointPtr_t> wheels,
92 ConstraintSetPtr_t constraints);
93
96 static DubinsPathPtr_t createCopy(const DubinsPathPtr_t& path) {
97 DubinsPath* ptr = new DubinsPath(*path);
98 DubinsPathPtr_t shPtr(ptr);
99 ptr->init(shPtr);
100 return shPtr;
101 }
102
106 static DubinsPathPtr_t createCopy(const DubinsPathPtr_t& path,
107 const ConstraintSetPtr_t& constraints) {
108 DubinsPath* ptr = new DubinsPath(*path, constraints);
109 DubinsPathPtr_t shPtr(ptr);
110 ptr->init(shPtr);
111 return shPtr;
112 }
113
115 virtual PathPtr_t copy() const { return createCopy(weak_.lock()); }
116
121 virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
122 return createCopy(weak_.lock(), constraints);
123 }
124
126 inline DevicePtr_t device() const { return device_; }
127
129 inline Configuration_t initial() const { return initial_; }
130
132 inline Configuration_t end() const { return end_; }
133
134 protected:
136 virtual std::ostream& print(std::ostream& os) const {
137 os << "DubinsPath:" << std::endl;
138 Path::print(os);
139 os << "initial configuration: " << initial_.transpose() << std::endl;
140 os << "final configuration: " << end_.transpose() << std::endl;
141 return os;
142 }
144 DubinsPath(const DevicePtr_t& robot, ConfigurationIn_t init,
145 ConfigurationIn_t end, value_type extraLength, value_type rho,
146 size_type xyId, size_type rzId,
147 const std::vector<JointPtr_t> wheels);
148
150 DubinsPath(const DevicePtr_t& robot, ConfigurationIn_t init,
151 ConfigurationIn_t end, value_type extraLength, value_type rho,
152 size_type xyId, size_type rzId,
153 const std::vector<JointPtr_t> wheels,
154 ConstraintSetPtr_t constraints);
155
157 DubinsPath(const DubinsPath& path);
158
160 DubinsPath(const DubinsPath& path, const ConstraintSetPtr_t& constraints);
161
162 void init(DubinsPathPtr_t self);
163
165 DubinsPath() : xyId_(0), rzId_(0) {}
166
167 private:
168 void dubins_init_normalised(double alpha, double beta, double d);
169 void dubins_init(vector3_t q0, vector3_t q1);
170 typedef Eigen::Matrix<value_type, 3, 1> Lengths_t;
171
172 DevicePtr_t device_;
173 Configuration_t initial_;
174 Configuration_t end_;
175 const size_type xyId_, rzId_;
176 size_type dxyId_, drzId_;
177 std::vector<JointPtr_t> wheels_;
178 std::size_t typeId_;
179 Lengths_t lengths_;
180 value_type extraLength_, rho_;
181
182 vector3_t qi_; // the initial configuration
183 DubinsPathWkPtr_t weak_;
184
185 HPP_SERIALIZABLE();
186}; // class DubinsPath
187
189} // namespace core
190} // namespace hpp
191
192BOOST_CLASS_EXPORT_KEY(hpp::core::DubinsPath)
193
194#endif // HPP_CORE_DUBINS_PATH_HH
hpp::core::DubinsPath
Definition dubins-path.hh:56
hpp::core::DubinsPath::copy
virtual PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition dubins-path.hh:121
hpp::core::DubinsPath::init
void init(DubinsPathPtr_t self)
hpp::core::DubinsPath::print
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition dubins-path.hh:136
hpp::core::DubinsPath::device
DevicePtr_t device() const
Return the internal robot.
Definition dubins-path.hh:126
hpp::core::DubinsPath::~DubinsPath
virtual ~DubinsPath()
Destructor.
Definition dubins-path.hh:61
hpp::core::DubinsPath::initial
Configuration_t initial() const
Get the initial configuration.
Definition dubins-path.hh:129
hpp::core::DubinsPath::DubinsPath
DubinsPath(const DevicePtr_t &robot, ConfigurationIn_t init, ConfigurationIn_t end, value_type extraLength, value_type rho, size_type xyId, size_type rzId, const std::vector< JointPtr_t > wheels)
Constructor.
hpp::core::DubinsPath::DubinsPath
DubinsPath(const DubinsPath &path)
Copy constructor.
hpp::core::DubinsPath::end
Configuration_t end() const
Get the final configuration.
Definition dubins-path.hh:132
hpp::core::DubinsPath::parent_t
core::PathVector parent_t
Definition dubins-path.hh:58
hpp::core::DubinsPath::copy
virtual PathPtr_t copy() const
Return a shared pointer to a copy of this object.
Definition dubins-path.hh:115
hpp::core::DubinsPath::create
static DubinsPathPtr_t create(const DevicePtr_t &device, ConfigurationIn_t init, ConfigurationIn_t end, value_type extraLength, value_type rho, size_type xyId, size_type rzId, const std::vector< JointPtr_t > wheels, ConstraintSetPtr_t constraints)
hpp::core::DubinsPath::createCopy
static DubinsPathPtr_t createCopy(const DubinsPathPtr_t &path)
Definition dubins-path.hh:96
hpp::core::DubinsPath::DubinsPath
DubinsPath(const DevicePtr_t &robot, ConfigurationIn_t init, ConfigurationIn_t end, value_type extraLength, value_type rho, size_type xyId, size_type rzId, const std::vector< JointPtr_t > wheels, ConstraintSetPtr_t constraints)
Constructor with constraints.
hpp::core::DubinsPath::createCopy
static DubinsPathPtr_t createCopy(const DubinsPathPtr_t &path, const ConstraintSetPtr_t &constraints)
Definition dubins-path.hh:106
hpp::core::DubinsPath::DubinsPath
DubinsPath(const DubinsPath &path, const ConstraintSetPtr_t &constraints)
Copy constructor with constraints.
hpp::core::DubinsPath::DubinsPath
DubinsPath()
For serialization only.
Definition dubins-path.hh:165
hpp::core::DubinsPath::create
static DubinsPathPtr_t create(const DevicePtr_t &device, ConfigurationIn_t init, ConfigurationIn_t end, value_type extraLength, value_type rho, size_type xyId, size_type rzId, const std::vector< JointPtr_t > wheels)
hpp::core::PathVector
Concatenation of several paths.
Definition path-vector.hh:43
hpp::core::Path::print
virtual std::ostream & print(std::ostream &os) const
hpp::core::Path::constraints
const ConstraintSetPtr_t & constraints() const
Get constraints the path is subject to.
Definition path.hh:204
config.hh
device.hh
fwd.hh
hpp
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::vector3_t
pinocchio::vector3_t vector3_t
Definition fwd.hh:165
hpp::core::DubinsPathPtr_t
shared_ptr< DubinsPath > DubinsPathPtr_t
Definition fwd.hh:204
hpp::core::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
hpp::core::Configuration_t
pinocchio::Configuration_t Configuration_t
Definition fwd.hh:107
hpp::core::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:134
hpp::core::ConstraintSetPtr_t
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition fwd.hh:130
hpp::core::PathPtr_t
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187
path-vector.hh