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

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

Loading...
Searching...
No Matches
hermite.hh
Go to the documentation of this file.
1// Copyright (c) 2016 CNRS
2// Authors: Joseph Mirabel
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_PATH_HERMITE_HH
30#define HPP_CORE_PATH_HERMITE_HH
31
32#include <hpp/core/config.hh>
33#include <hpp/core/fwd.hh>
34#include <hpp/core/path/spline.hh>
35
36namespace hpp {
37namespace core {
38namespace path {
41
42 class HPP_CORE_DLLAPI Hermite : public Spline<BernsteinBasis, 3> {
43 public:
44 typedef Spline<BernsteinBasis, 3> parent_t;
45
47 virtual ~Hermite() {}
48
49 static HermitePtr_t create(const DevicePtr_t& device, ConfigurationIn_t init,
50 ConfigurationIn_t end,
51 ConstraintSetPtr_t constraints) {
52 Hermite* ptr = new Hermite(device, init, end, constraints);
53 HermitePtr_t shPtr(ptr);
54 ptr->init(shPtr);
55 return shPtr;
56 }
57
60 static HermitePtr_t createCopy(const HermitePtr_t& path) {
61 Hermite* ptr = new Hermite(*path);
62 HermitePtr_t shPtr(ptr);
63 ptr->init(shPtr);
64 return shPtr;
65 }
66
70 static HermitePtr_t createCopy(const HermitePtr_t& path,
71 const ConstraintSetPtr_t& constraints) {
72 Hermite* ptr = new Hermite(*path, constraints);
73 HermitePtr_t shPtr(ptr);
74 ptr->init(shPtr);
75 return shPtr;
76 }
77
82 virtual PathPtr_t copy() const { return createCopy(weak_.lock()); }
83
88 virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
89 return createCopy(weak_.lock(), constraints);
90 }
91
93 DevicePtr_t device() const;
94
95 void v0(const vectorIn_t& speed) {
96 parameters_.row(1) = parameters_.row(0) + speed.transpose() / 3;
97 hermiteLength_ = -1;
98 }
99
100 void v1(const vectorIn_t& speed) {
101 parameters_.row(2) = parameters_.row(3) - speed.transpose() / 3;
102 hermiteLength_ = -1;
103 }
104
105 vector_t v0() const {
106 return 3 * (parameters_.row(1) - parameters_.row(0));
107 // TODO Should be equivalent to
108 // vector_t res (outputDerivativeSize());
109 // derivative (res, timeRange().first, 1);
110 // return res;
111 }
112
113 vector_t v1() const { return 3 * (parameters_.row(3) - parameters_.row(2)); }
114
115 virtual Configuration_t initial() const { return init_; }
116
117 virtual Configuration_t end() const { return end_; }
118
119 const value_type& hermiteLength() const { return hermiteLength_; }
120
121 void computeHermiteLength();
122
123 vector_t velocity(const value_type& t) const;
124
125 protected:
127 virtual std::ostream& print(std::ostream& os) const {
128 os << "Hermite:" << std::endl;
129 Path::print(os);
130 os << "initial configuration: " << initial().transpose() << std::endl;
131 os << "final configuration: " << end().transpose() << std::endl;
132 return os;
133 }
134
136 Hermite(const DevicePtr_t& robot, ConfigurationIn_t init,
137 ConfigurationIn_t end);
138
140 Hermite(const DevicePtr_t& robot, ConfigurationIn_t init,
141 ConfigurationIn_t end, ConstraintSetPtr_t constraints);
142
144 Hermite(const Hermite& path);
145
147 Hermite(const Hermite& path, const ConstraintSetPtr_t& constraints);
148
149 void init(HermitePtr_t self);
150
151 private:
152 // void computeVelocities ();
153 void projectVelocities(ConfigurationIn_t qi, ConfigurationIn_t qe);
154
155 DevicePtr_t device_;
156 Configuration_t init_, end_;
157 value_type hermiteLength_;
158
159 HermiteWkPtr_t weak_;
160}; // class Hermite
162} // namespace path
163} // namespace core
164} // namespace hpp
165#endif // HPP_CORE_PATH_HERMITE_HH
hpp::core::path::Hermite
Definition hermite.hh:42
hpp::core::path::Hermite::v0
vector_t v0() const
Definition hermite.hh:105
hpp::core::path::Hermite::Hermite
Hermite(const DevicePtr_t &robot, ConfigurationIn_t init, ConfigurationIn_t end)
Constructor.
hpp::core::path::Hermite::init
void init(HermitePtr_t self)
hpp::core::path::Hermite::computeHermiteLength
void computeHermiteLength()
hpp::core::path::Hermite::v1
void v1(const vectorIn_t &speed)
Definition hermite.hh:100
hpp::core::path::Hermite::velocity
vector_t velocity(const value_type &t) const
hpp::core::path::Hermite::createCopy
static HermitePtr_t createCopy(const HermitePtr_t &path, const ConstraintSetPtr_t &constraints)
Definition hermite.hh:70
hpp::core::path::Hermite::create
static HermitePtr_t create(const DevicePtr_t &device, ConfigurationIn_t init, ConfigurationIn_t end, ConstraintSetPtr_t constraints)
Definition hermite.hh:49
hpp::core::path::Hermite::device
DevicePtr_t device() const
Return the internal robot.
hpp::core::path::Hermite::hermiteLength
const value_type & hermiteLength() const
Definition hermite.hh:119
hpp::core::path::Hermite::print
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition hermite.hh:127
hpp::core::path::Hermite::Hermite
Hermite(const Hermite &path, const ConstraintSetPtr_t &constraints)
Copy constructor with constraints.
hpp::core::path::Hermite::createCopy
static HermitePtr_t createCopy(const HermitePtr_t &path)
Definition hermite.hh:60
hpp::core::path::Hermite::initial
virtual Configuration_t initial() const
Get the initial configuration.
Definition hermite.hh:115
hpp::core::path::Hermite::~Hermite
virtual ~Hermite()
Destructor.
Definition hermite.hh:47
hpp::core::path::Hermite::copy
virtual PathPtr_t copy() const
Definition hermite.hh:82
hpp::core::path::Hermite::v1
vector_t v1() const
Definition hermite.hh:113
hpp::core::path::Hermite::end
virtual Configuration_t end() const
Get the final configuration.
Definition hermite.hh:117
hpp::core::path::Hermite::copy
virtual PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition hermite.hh:88
hpp::core::path::Hermite::v0
void v0(const vectorIn_t &speed)
Definition hermite.hh:95
hpp::core::path::Hermite::Hermite
Hermite(const DevicePtr_t &robot, ConfigurationIn_t init, ConfigurationIn_t end, ConstraintSetPtr_t constraints)
Constructor with constraints.
hpp::core::path::Hermite::parent_t
Spline< BernsteinBasis, 3 > parent_t
Definition hermite.hh:44
hpp::core::path::Hermite::Hermite
Hermite(const Hermite &path)
Copy constructor.
hpp::core::path::Spline
Definition spline.hh:91
config.hh
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
fwd.hh
hpp
hpp::core::path::HermitePtr_t
shared_ptr< Hermite > HermitePtr_t
Definition fwd.hh:246
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition fwd.hh:221
hpp::core::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:220
hpp::core::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
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
spline.hh