| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// Copyright (c) 2019, LAAS-CNRS |
| 2 |
|
|
// Authors: Joseph Mirabel (joseph.mirabel@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_MANIPULATION_STEERING_METHOD_END_EFFECTOR_TRAJECTORY_HH |
| 30 |
|
|
#define HPP_MANIPULATION_STEERING_METHOD_END_EFFECTOR_TRAJECTORY_HH |
| 31 |
|
|
|
| 32 |
|
|
#include <hpp/core/steering-method.hh> |
| 33 |
|
|
#include <hpp/manipulation/config.hh> |
| 34 |
|
|
#include <hpp/manipulation/fwd.hh> |
| 35 |
|
|
|
| 36 |
|
|
namespace hpp { |
| 37 |
|
|
namespace manipulation { |
| 38 |
|
|
namespace steeringMethod { |
| 39 |
|
|
HPP_PREDEF_CLASS(EndEffectorTrajectory); |
| 40 |
|
|
typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t; |
| 41 |
|
|
|
| 42 |
|
|
using core::PathPtr_t; |
| 43 |
|
|
|
| 44 |
|
|
/// \addtogroup steering_method |
| 45 |
|
|
/// \{ |
| 46 |
|
|
|
| 47 |
|
|
/// Build piecewise straight paths for a robot end-effector |
| 48 |
|
|
/// |
| 49 |
|
|
/// To use this steering method, the user needs to provide |
| 50 |
|
|
/// \li a constraint with value in \f$SE(3)\f$. An easy way to create such a |
| 51 |
|
|
/// constraint is to use method hpp::manipulation::Handle::createGrasp. The |
| 52 |
|
|
/// constraint is passed to the steering method using method \link |
| 53 |
|
|
/// EndEffectorTrajectory::trajectoryConstraint trajectoryConstraint \endlink. |
| 54 |
|
|
/// \li the time-varying right hand side of this constraint along the path |
| 55 |
|
|
/// the user wants to create in the form of a hpp::core::Path instance |
| 56 |
|
|
/// with values in \f$SE(3)\f$. For that, \link |
| 57 |
|
|
/// EndEffectorTrajectory::makePiecewiseLinearTrajectory |
| 58 |
|
|
/// makePiecewiseLinearTrajectory \endlink method may be useful. |
| 59 |
|
|
/// |
| 60 |
|
|
/// \warning the constraint should also be inserted in the \link |
| 61 |
|
|
/// hpp::core::SteeringMethod::constraints set of constraints \endlink |
| 62 |
|
|
/// of the steering method. |
| 63 |
|
|
/// |
| 64 |
|
|
/// Once the steering method has been initialized, it can be called between |
| 65 |
|
|
/// two configurations \c q1 and \c q2. The interval of definition \f$[0,T]\f$ |
| 66 |
|
|
/// of the output path is the same as the one of the path provided as the right |
| 67 |
|
|
/// hand side of the constraint. |
| 68 |
|
|
/// Note that \c q1 and \c q2 should satisfy the constraint at times 0 and |
| 69 |
|
|
/// \f$T\f$ respectively. The output path is a \link hpp::core::StraightPath |
| 70 |
|
|
/// linear interpolation \endlink between \c q1 and \c q2 projected on the |
| 71 |
|
|
/// steering method constraints. |
| 72 |
|
|
class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory |
| 73 |
|
|
: public core::SteeringMethod { |
| 74 |
|
|
public: |
| 75 |
|
|
typedef core::interval_t interval_t; |
| 76 |
|
|
|
| 77 |
|
✗ |
static EndEffectorTrajectoryPtr_t create( |
| 78 |
|
|
const core::ProblemConstPtr_t& problem) { |
| 79 |
|
✗ |
EndEffectorTrajectoryPtr_t ptr(new EndEffectorTrajectory(problem)); |
| 80 |
|
✗ |
ptr->init(ptr); |
| 81 |
|
✗ |
return ptr; |
| 82 |
|
|
} |
| 83 |
|
|
|
| 84 |
|
|
/** Build a trajectory in SE(3). |
| 85 |
|
|
\param points a Nx7 matrix whose rows corresponds to poses. |
| 86 |
|
|
\param weights a 6D vector, weights to be applied when computing |
| 87 |
|
|
the distance between two SE3 points. |
| 88 |
|
|
|
| 89 |
|
|
The trajectory \f$T\f$ is defined as follows. Let \f$N\f$ be the number of |
| 90 |
|
|
lines of matrix \c points, \f$p_i\f$ be the i-th line of \c points and |
| 91 |
|
|
let \f$W\f$ be the |
| 92 |
|
|
diagonal matrix with the coefficients of \c weights: |
| 93 |
|
|
\f[ |
| 94 |
|
|
W = \left(\begin{array}{cccccc} |
| 95 |
|
|
w_1 & 0 & 0 & 0 & 0 & 0\\ |
| 96 |
|
|
0 & w_2 & 0 & 0 & 0 & 0\\ |
| 97 |
|
|
0 & 0 & w_3 & 0 & 0 & 0\\ |
| 98 |
|
|
0 & 0 & 0 & w_4 & 0 & 0\\ |
| 99 |
|
|
0 & 0 & 0 & 0 & w_5 & 0\\ |
| 100 |
|
|
0 & 0 & 0 & 0 & 0 & w_6\\ |
| 101 |
|
|
\end{array}\right) |
| 102 |
|
|
\f] |
| 103 |
|
|
|
| 104 |
|
|
\f{eqnarray*}{ |
| 105 |
|
|
f(t) = \mathbf{p}_i \oplus \frac{t-t_i}{t_{i+1}-t_i} |
| 106 |
|
|
(\mathbf{p}_{i+1}-\mathbf{p}_i) && \mbox{ for } t \in [t_i,t_{i+1}] \f} |
| 107 |
|
|
|
| 108 |
|
|
where \f$t_0 = 0\f$ and |
| 109 |
|
|
\f{eqnarray*}{ |
| 110 |
|
|
t_{i+1}-t_i = \|W(\mathbf{p}_{i+1}-\mathbf{p}_i)\| && \mbox{for } i |
| 111 |
|
|
\mbox{ such that } 1 \leq i \leq N-1 |
| 112 |
|
|
\f} */ |
| 113 |
|
|
static PathPtr_t makePiecewiseLinearTrajectory(matrixIn_t points, |
| 114 |
|
|
vectorIn_t weights); |
| 115 |
|
|
|
| 116 |
|
|
/// Set the constraint whose right hand side will vary. |
| 117 |
|
|
void trajectoryConstraint(const constraints::ImplicitPtr_t& ic); |
| 118 |
|
|
|
| 119 |
|
✗ |
const constraints::ImplicitPtr_t& trajectoryConstraint() { |
| 120 |
|
✗ |
return constraint_; |
| 121 |
|
|
} |
| 122 |
|
|
|
| 123 |
|
|
/// Set the right hand side of the function from a path |
| 124 |
|
|
/// \param se3Output set to True if the output of path must be |
| 125 |
|
|
/// understood as SE3. |
| 126 |
|
|
void trajectory(const PathPtr_t& eeTraj, bool se3Output); |
| 127 |
|
|
|
| 128 |
|
|
/// Set the right hand side of the function from another function. |
| 129 |
|
|
/// \param eeTraj a function whose input space is of dimension 1. |
| 130 |
|
|
/// \param timeRange the input range of eeTraj. |
| 131 |
|
|
void trajectory(const DifferentiableFunctionPtr_t& eeTraj, |
| 132 |
|
|
const interval_t& timeRange); |
| 133 |
|
|
|
| 134 |
|
✗ |
const DifferentiableFunctionPtr_t& trajectory() const { return eeTraj_; } |
| 135 |
|
|
|
| 136 |
|
✗ |
const interval_t& timeRange() const { return timeRange_; } |
| 137 |
|
|
|
| 138 |
|
✗ |
core::SteeringMethodPtr_t copy() const { |
| 139 |
|
✗ |
EndEffectorTrajectoryPtr_t ptr(new EndEffectorTrajectory(*this)); |
| 140 |
|
✗ |
ptr->init(ptr); |
| 141 |
|
✗ |
return ptr; |
| 142 |
|
|
} |
| 143 |
|
|
|
| 144 |
|
|
/// Computes an core::InterpolatedPath from the provided interpolation |
| 145 |
|
|
/// points. |
| 146 |
|
|
/// \param times the time of each configuration |
| 147 |
|
|
/// \param configs each column correspond to a configuration |
| 148 |
|
|
PathPtr_t projectedPath(vectorIn_t times, matrixIn_t configs) const; |
| 149 |
|
|
|
| 150 |
|
|
protected: |
| 151 |
|
✗ |
EndEffectorTrajectory(const core::ProblemConstPtr_t& problem) |
| 152 |
|
✗ |
: core::SteeringMethod(problem) {} |
| 153 |
|
|
|
| 154 |
|
✗ |
EndEffectorTrajectory(const EndEffectorTrajectory& other) |
| 155 |
|
✗ |
: core::SteeringMethod(other), |
| 156 |
|
✗ |
eeTraj_(other.eeTraj_), |
| 157 |
|
✗ |
timeRange_(other.timeRange_), |
| 158 |
|
✗ |
constraint_(other.constraint_) {} |
| 159 |
|
|
|
| 160 |
|
|
PathPtr_t impl_compute(ConfigurationIn_t q1, ConfigurationIn_t q2) const; |
| 161 |
|
|
|
| 162 |
|
|
private: |
| 163 |
|
|
core::ConstraintSetPtr_t getUpdatedConstraints() const; |
| 164 |
|
|
|
| 165 |
|
|
DifferentiableFunctionPtr_t eeTraj_; |
| 166 |
|
|
interval_t timeRange_; |
| 167 |
|
|
constraints::ImplicitPtr_t constraint_; |
| 168 |
|
|
}; |
| 169 |
|
|
/// \} |
| 170 |
|
|
} // namespace steeringMethod |
| 171 |
|
|
} // namespace manipulation |
| 172 |
|
|
} // namespace hpp |
| 173 |
|
|
|
| 174 |
|
|
#endif // HPP_MANIPULATION_STEERING_METHOD_END_EFFECTOR_TRAJECTORY_HH |
| 175 |
|
|
|