hpp-core: include/hpp/core/steering-method/steering-kinodynamic.hh Source File

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

Loading...
Searching...
No Matches
steering-kinodynamic.hh
Go to the documentation of this file.
1// Copyright (c) 2016, LAAS-CNRS
2// Authors: Pierre Fernbach (pierre.fernbach@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_CORE_STEERING_METHOD_KINODYNAMIC_HH
30#define HPP_CORE_STEERING_METHOD_KINODYNAMIC_HH
31
32#include <hpp/core/config.hh>
33#include <hpp/core/fwd.hh>
34#include <hpp/core/steering-method.hh>
35#include <hpp/util/debug.hh>
36#include <hpp/util/pointer.hh>
37
38namespace hpp {
39namespace core {
40namespace steeringMethod {
41
42 typedef Eigen::Matrix<value_type, 3, 1> Vector3;
43
46
56 class HPP_CORE_DLLAPI Kinodynamic : public SteeringMethod {
57 public:
59 static KinodynamicPtr_t create(const ProblemConstPtr_t& problem) {
60 Kinodynamic* ptr = new Kinodynamic(problem);
61 KinodynamicPtr_t shPtr(ptr);
62 ptr->init(shPtr);
63 return shPtr;
64 }
65
67 static KinodynamicPtr_t createCopy(const KinodynamicPtr_t& other) {
68 Kinodynamic* ptr = new Kinodynamic(*other);
69 KinodynamicPtr_t shPtr(ptr);
70 ptr->init(shPtr);
71 return shPtr;
72 }
73
75 virtual SteeringMethodPtr_t copy() const { return createCopy(weak_.lock()); }
76
78 virtual PathPtr_t impl_compute(ConfigurationIn_t q1,
79 ConfigurationIn_t q2) const;
80
91 double computeMinTime(int index, double p1, double p2, double v1, double v2,
92 interval_t* infInterval) const;
93
109 virtual void fixedTimeTrajectory(int index, double T, double p1, double p2,
110 double v1, double v2, double* a1, double* t0,
111 double* t1, double* tv, double* t2,
112 double* vLim) const;
113
114 void setAmax(Vector3 aMax) { aMax_ = aMax; }
115
116 void setVmax(Vector3 vMax) { vMax_ = vMax; }
117
118 protected:
120 Kinodynamic(const ProblemConstPtr_t& problem);
121
123 Kinodynamic(const Kinodynamic& other);
124
126 void init(KinodynamicWkPtr_t weak) {
127 core::SteeringMethod::init(weak);
128 weak_ = weak;
129 }
130
131 Vector3 aMax_;
132 Vector3 vMax_;
133 double aMaxFixed_;
134 double aMaxFixed_Z_;
135 bool synchronizeVerticalAxis_;
136 bool orientedPath_;
137 bool orientationIgnoreZValue_;
138
139 private:
140 DeviceWkPtr_t device_;
141 KinodynamicWkPtr_t weak_;
142}; // Kinodynamic
144} // namespace steeringMethod
145} // namespace core
146} // namespace hpp
147#endif // HPP_CORE_STEERING_METHOD_KINODYNAMIC_HH
hpp::core::SteeringMethod
Definition steering-method.hh:48
hpp::core::steeringMethod::Kinodynamic
Definition steering-kinodynamic.hh:56
hpp::core::steeringMethod::Kinodynamic::create
static KinodynamicPtr_t create(const ProblemConstPtr_t &problem)
Create an instance.
Definition steering-kinodynamic.hh:59
hpp::core::steeringMethod::Kinodynamic::Kinodynamic
Kinodynamic(const Kinodynamic &other)
Copy constructor.
hpp::core::steeringMethod::Kinodynamic::aMaxFixed_
double aMaxFixed_
Definition steering-kinodynamic.hh:133
hpp::core::steeringMethod::Kinodynamic::setAmax
void setAmax(Vector3 aMax)
Definition steering-kinodynamic.hh:114
hpp::core::steeringMethod::Kinodynamic::aMaxFixed_Z_
double aMaxFixed_Z_
Definition steering-kinodynamic.hh:134
hpp::core::steeringMethod::Kinodynamic::setVmax
void setVmax(Vector3 vMax)
Definition steering-kinodynamic.hh:116
hpp::core::steeringMethod::Kinodynamic::aMax_
Vector3 aMax_
Definition steering-kinodynamic.hh:131
hpp::core::steeringMethod::Kinodynamic::orientedPath_
bool orientedPath_
Definition steering-kinodynamic.hh:136
hpp::core::steeringMethod::Kinodynamic::Kinodynamic
Kinodynamic(const ProblemConstPtr_t &problem)
Constructor.
hpp::core::steeringMethod::Kinodynamic::synchronizeVerticalAxis_
bool synchronizeVerticalAxis_
Definition steering-kinodynamic.hh:135
hpp::core::steeringMethod::Kinodynamic::createCopy
static KinodynamicPtr_t createCopy(const KinodynamicPtr_t &other)
Copy instance and return shared pointer.
Definition steering-kinodynamic.hh:67
hpp::core::steeringMethod::Kinodynamic::orientationIgnoreZValue_
bool orientationIgnoreZValue_
Definition steering-kinodynamic.hh:137
hpp::core::steeringMethod::Kinodynamic::copy
virtual SteeringMethodPtr_t copy() const
Copy instance and return shared pointer.
Definition steering-kinodynamic.hh:75
hpp::core::steeringMethod::Kinodynamic::init
void init(KinodynamicWkPtr_t weak)
Store weak pointer to itself.
Definition steering-kinodynamic.hh:126
hpp::core::steeringMethod::Kinodynamic::impl_compute
virtual PathPtr_t impl_compute(ConfigurationIn_t q1, ConfigurationIn_t q2) const
create a path between two configurations
hpp::core::steeringMethod::Kinodynamic::fixedTimeTrajectory
virtual void fixedTimeTrajectory(int index, double T, double p1, double p2, double v1, double v2, double *a1, double *t0, double *t1, double *tv, double *t2, double *vLim) const
fixedTimeTrajectory compute the minimum acceleration trajectory for desired time T (1 dimension)
hpp::core::steeringMethod::Kinodynamic::vMax_
Vector3 vMax_
Definition steering-kinodynamic.hh:132
hpp::core::steeringMethod::Kinodynamic::computeMinTime
double computeMinTime(int index, double p1, double p2, double v1, double v2, interval_t *infInterval) const
computeMinTime compute the minimum time required to go from state (p1,v1) to (p2,v2)
config.hh
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
debug.hh
fwd.hh
hpp
hpp::core::steeringMethod::Vector3
Eigen::Matrix< value_type, 3, 1 > Vector3
Definition steering-kinodynamic.hh:42
hpp::core::steeringMethod::KinodynamicPtr_t
shared_ptr< Kinodynamic > KinodynamicPtr_t
Definition fwd.hh:354
hpp::core::DeviceWkPtr_t
pinocchio::DeviceWkPtr_t DeviceWkPtr_t
Definition fwd.hh:135
hpp::core::SteeringMethodPtr_t
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition fwd.hh:213
hpp::core::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::core::interval_t
std::pair< value_type, value_type > interval_t
Definition fwd.hh:175
hpp::core::ProblemConstPtr_t
shared_ptr< const Problem > ProblemConstPtr_t
Definition fwd.hh:197
hpp::core::PathPtr_t
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187
pointer.hh
steering-method.hh