From 815964c695fd9b213509e38405b1f69c3e08d283 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 14 Feb 2020 16:02:51 +0100
Subject: [PATCH] Update API of
steeringMethod::EndEffectorTrajectory::makePiecewiseLinearTrajectory
---
.../end-effector-trajectory.hh | 5 ++-
.../end-effector-trajectory.cc | 33 +++++++++++--------
2 files changed, 23 insertions(+), 15 deletions(-)
diff --git a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
index f9d565b..6593562 100644
--- a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
+++ b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
@@ -48,7 +48,10 @@ namespace hpp {
/// Build a trajectory in SE(3).
/// \param points a Nx7 matrix whose rows corresponds to a pose.
- static PathPtr_t makePiecewiseLinearTrajectory (matrixIn_t points);
+ /// \param weights a 6D vector, weights to be applied when computing
+ /// the distance between two SE3 points.
+ static PathPtr_t makePiecewiseLinearTrajectory (matrixIn_t points,
+ vectorIn_t weights);
/// Set the constraint whose right hand side will vary.
void trajectoryConstraint (const constraints::ImplicitPtr_t& ic);
diff --git a/src/steering-method/end-effector-trajectory.cc b/src/steering-method/end-effector-trajectory.cc
index 6213854..634075b 100644
--- a/src/steering-method/end-effector-trajectory.cc
+++ b/src/steering-method/end-effector-trajectory.cc
@@ -82,26 +82,31 @@ namespace hpp {
};
}
- PathPtr_t EndEffectorTrajectory::makePiecewiseLinearTrajectory (matrixIn_t points)
+ PathPtr_t EndEffectorTrajectory::makePiecewiseLinearTrajectory (
+ matrixIn_t points,
+ vectorIn_t weights)
{
if (points.cols() != 7)
throw std::invalid_argument("The input matrix should have 7 columns");
+ if (weights.size() != 6)
+ throw std::invalid_argument("The weights vector should have 6 elements");
LiegroupSpacePtr_t se3 = LiegroupSpace::SE3();
- if (points.rows() == 1)
- return core::StraightPath::create (se3,
- points.row(0),
- points.row(0), interval_t(0,0));
-
core::PathVectorPtr_t path = core::PathVector::create(7, 6);
- for (size_type i = 1; i < points.rows(); ++i) {
- value_type d = (
- se3->elementConstRef(points.row(i)) - se3->elementConstRef(points.row(i-1))
- ).norm();
+ if (points.rows() == 1)
path->appendPath (core::StraightPath::create (se3,
- points.row(i-1),
- points.row(i),
- interval_t(0, d)));
- }
+ points.row(0),
+ points.row(0), interval_t(0,0)));
+ else
+ for (size_type i = 1; i < points.rows(); ++i) {
+ value_type d =
+ ( se3->elementConstRef(points.row(i))
+ - se3->elementConstRef(points.row(i-1))
+ ).cwiseProduct(weights).norm();
+ path->appendPath (core::StraightPath::create (se3,
+ points.row(i-1),
+ points.row(i),
+ interval_t(0, d)));
+ }
return path;
}
--
GitLab