diff --git a/CMakeLists.txt b/CMakeLists.txt
index b23425c17395281887538fc309aebe885367bd71..e4a67b1bb2ab83919808446a47eab3863bf7fe50 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -93,6 +93,8 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/path-optimization/random-shortcut.hh
include/hpp/manipulation/path-optimization/spline-gradient-based.hh
+ include/hpp/manipulation/path-planner/end-effector-trajectory.hh
+
include/hpp/manipulation/problem-target/state.hh
include/hpp/manipulation/steering-method/cross-state-optimization.hh
diff --git a/include/hpp/manipulation/path-planner/end-effector-trajectory.hh b/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
new file mode 100644
index 0000000000000000000000000000000000000000..e39dd0f5cbf1a851a24d0891a12f28877c8d9646
--- /dev/null
+++ b/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
@@ -0,0 +1,137 @@
+// Copyright (c) 2019 CNRS
+// Authors: Joseph Mirabel
+//
+// This file is part of hpp-manipulation
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation If not, see
+// <http://www.gnu.org/licenses/>.
+
+
+#ifndef HPP_MANIPULATION_PATH_PLANNER_END_EFFECTOR_TRAJECTORY_HH
+# define HPP_MANIPULATION_PATH_PLANNER_END_EFFECTOR_TRAJECTORY_HH
+
+# include <hpp/manipulation/config.hh>
+# include <hpp/manipulation/fwd.hh>
+
+# include <pinocchio/spatial/se3.hpp>
+
+# include <hpp/pinocchio/frame.hh>
+# include <hpp/core/path-planner.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathPlanner {
+ class HPP_MANIPULATION_DLLAPI IkSolverInitialization
+ {
+ public:
+ typedef std::vector<Configuration_t> Configurations_t;
+
+ Configurations_t solve (vectorIn_t target)
+ {
+ return impl_solve (target);
+ }
+
+ protected:
+ virtual Configurations_t impl_solve (vectorIn_t target) = 0;
+ };
+ typedef boost::shared_ptr<IkSolverInitialization> IkSolverInitializationPtr_t;
+
+ HPP_PREDEF_CLASS (EndEffectorTrajectory);
+ typedef boost::shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
+
+ class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory : public core::PathPlanner
+ {
+ public:
+ /// Return shared pointer to new instance
+ /// \param problem the path planning problem
+ static EndEffectorTrajectoryPtr_t create (const core::Problem& problem);
+ /// Return shared pointer to new instance
+ /// \param problem the path planning problem
+ /// \param roadmap previously built roadmap
+ static EndEffectorTrajectoryPtr_t createWithRoadmap
+ (const core::Problem& problem, const core::RoadmapPtr_t& roadmap);
+
+ /// Initialize the problem resolution
+ /// \li call parent implementation
+ /// \li get number nodes in problem parameter map
+ virtual void startSolve ();
+
+ /// One step of the algorithm
+ virtual void oneStep ();
+
+ /// Get the number of random configurations shoot (after using init
+ /// config) in order to generate the initial config of the final path.
+ int nRandomConfig () const
+ { return nRandomConfig_; }
+
+ void nRandomConfig (int n)
+ {
+ assert (n >= 0);
+ nRandomConfig_ = n;
+ }
+
+ /// Number of steps to generate goal config (successive projections).
+ int nDiscreteSteps () const
+ { return nDiscreteSteps_; }
+
+ void nDiscreteSteps (int n)
+ {
+ assert (n > 0);
+ nDiscreteSteps_ = n;
+ }
+
+ /// If enabled, only add one solution to the roadmap.
+ /// Otherwise add all solution.
+ void checkFeasibilityOnly (bool enable);
+
+ bool checkFeasibilityOnly () const
+ {
+ return feasibilityOnly_;
+ }
+
+ void ikSolverInitialization (IkSolverInitializationPtr_t solver)
+ {
+ ikSolverInit_ = solver;
+ }
+
+ void tryConnectInitAndGoals ();
+
+ protected:
+ /// Protected constructor
+ /// \param problem the path planning problem
+ EndEffectorTrajectory (const core::Problem& problem);
+ /// Protected constructor
+ /// \param problem the path planning problem
+ /// \param roadmap previously built roadmap
+ EndEffectorTrajectory (const core::Problem& problem, const core::RoadmapPtr_t& roadmap);
+ /// Store weak pointer to itself
+ void init (const EndEffectorTrajectoryWkPtr_t& weak);
+
+ private:
+ std::vector<core::Configuration_t> configurations(const core::Configuration_t& q_init);
+
+ /// Weak pointer to itself
+ EndEffectorTrajectoryWkPtr_t weak_;
+ /// Number of random config.
+ int nRandomConfig_;
+ /// Number of steps to generate goal config.
+ int nDiscreteSteps_;
+ /// Ik solver initialization. An external Ik solver can be plugged here.
+ IkSolverInitializationPtr_t ikSolverInit_;
+ /// Feasibility
+ bool feasibilityOnly_;
+ }; // class EndEffectorTrajectory
+ } // namespace pathPlanner
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATH_PLANNER_END_EFFECTOR_TRAJECTORY_HH
diff --git a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
index 7c8feee3c521b363160f2eacf33502318201ae47..dd04146f80940f589327abcbdc0ba30117a28d59 100644
--- a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
+++ b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
@@ -46,6 +46,13 @@ namespace hpp {
return ptr;
}
+ /// Build a trajectory in SE(3).
+ /// \param points a Nx7 matrix whose rows corresponds to a pose.
+ /// \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);
@@ -81,6 +88,12 @@ namespace hpp {
return ptr;
}
+ /// Computes an core::InterpolatedPath from the provided interpolation
+ /// points.
+ /// \param times the time of each configuration
+ /// \param configs each column correspond to a configuration
+ PathPtr_t projectedPath (vectorIn_t times, matrixIn_t configs) const;
+
protected:
EndEffectorTrajectory (const core::Problem& problem)
: core::SteeringMethod (problem)
@@ -96,6 +109,8 @@ namespace hpp {
PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
private:
+ core::ConstraintSetPtr_t getUpdatedConstraints () const;
+
DifferentiableFunctionPtr_t eeTraj_;
interval_t timeRange_;
constraints::ImplicitPtr_t constraint_;
diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt
index 7a0c682fe4842063f7cdaa4f4e70db18c4163c12..77bbad0a5bba1c79631719a4122f665f8621cc51 100644
--- a/plugins/CMakeLists.txt
+++ b/plugins/CMakeLists.txt
@@ -20,3 +20,8 @@ HPP_ADD_PLUGIN(manipulation-spline-gradient-based
SOURCES spline-gradient-based.cc
LINK_DEPENDENCIES ${PROJECT_NAME} ${PROJECT_NAME}-gpl hpp-core-gpl
PKG_CONFIG_DEPENDENCIES hpp-core)
+
+HPP_ADD_PLUGIN(end-effector-trajectory
+ SOURCES end-effector-trajectory.cc
+ LINK_DEPENDENCIES ${PROJECT_NAME}
+ PKG_CONFIG_DEPENDENCIES hpp-core)
diff --git a/plugins/end-effector-trajectory.cc b/plugins/end-effector-trajectory.cc
new file mode 100644
index 0000000000000000000000000000000000000000..f2baee78989510cc0245526e5e03a4d7a3c794a4
--- /dev/null
+++ b/plugins/end-effector-trajectory.cc
@@ -0,0 +1,45 @@
+// Copyright (c) 2019, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#include <hpp/core/plugin.hh>
+#include <hpp/core/problem-solver.hh>
+
+#include <hpp/manipulation/steering-method/end-effector-trajectory.hh>
+#include <hpp/manipulation/path-planner/end-effector-trajectory.hh>
+
+namespace hpp {
+ namespace manipulation {
+ class EndEffectorTrajectoryPlugin : public core::ProblemSolverPlugin
+ {
+ public:
+ EndEffectorTrajectoryPlugin ()
+ : ProblemSolverPlugin ("EndEffectorTrajectoryPlugin", "0.0")
+ {}
+
+ protected:
+ virtual bool impl_initialize (core::ProblemSolverPtr_t ps)
+ {
+ ps->pathPlanners.add ("EndEffectorTrajectory",
+ pathPlanner::EndEffectorTrajectory::createWithRoadmap);
+ ps->steeringMethods.add ("EndEffectorTrajectory",
+ steeringMethod::EndEffectorTrajectory::create);
+ return true;
+ }
+ };
+ } // namespace manipulation
+} // namespace hpp
+
+HPP_CORE_DEFINE_PLUGIN(hpp::manipulation::EndEffectorTrajectoryPlugin)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 0ae2c0ecb6b5ff76d5a8902ebd461fa6a955c59d..19c8702c7a5ffe6f9b887ceb0f7d50b8ce02a0ab 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -49,6 +49,8 @@ SET(SOURCES
path-optimization/random-shortcut.cc
path-optimization/enforce-transition-semantic.cc
+ path-planner/end-effector-trajectory.cc
+
problem-target/state.cc
steering-method/end-effector-trajectory.cc
diff --git a/src/path-planner/end-effector-trajectory.cc b/src/path-planner/end-effector-trajectory.cc
new file mode 100644
index 0000000000000000000000000000000000000000..13df4ba0410713bb94b28a84ee4f2bbcedb246c0
--- /dev/null
+++ b/src/path-planner/end-effector-trajectory.cc
@@ -0,0 +1,257 @@
+// Copyright (c) 2019 CNRS
+// Authors: Joseph Mirabel
+//
+// This file is part of hpp-manipulation
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation If not, see
+// <http://www.gnu.org/licenses/>.
+
+# include <hpp/manipulation/path-planner/end-effector-trajectory.hh>
+
+# include <pinocchio/multibody/data.hpp>
+
+# include <hpp/util/exception-factory.hh>
+# include <hpp/pinocchio/util.hh>
+# include <hpp/pinocchio/device-sync.hh>
+# include <hpp/pinocchio/liegroup-element.hh>
+
+# include <hpp/constraints/differentiable-function.hh>
+# include <hpp/constraints/implicit.hh>
+
+# include <hpp/core/config-projector.hh>
+# include <hpp/core/config-validations.hh>
+# include <hpp/core/configuration-shooter.hh>
+# include <hpp/core/path-validation.hh>
+# include <hpp/core/problem.hh>
+# include <hpp/core/roadmap.hh>
+# include <hpp/manipulation/steering-method/end-effector-trajectory.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathPlanner {
+ typedef manipulation::steeringMethod::EndEffectorTrajectory SM_t;
+ typedef manipulation::steeringMethod::EndEffectorTrajectoryPtr_t SMPtr_t;
+
+ EndEffectorTrajectoryPtr_t EndEffectorTrajectory::create (const core::Problem& problem)
+ {
+ EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem));
+ ptr->init(ptr);
+ return ptr;
+ }
+
+ EndEffectorTrajectoryPtr_t EndEffectorTrajectory::createWithRoadmap (
+ const core::Problem& problem, const core::RoadmapPtr_t& roadmap)
+ {
+ EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem, roadmap));
+ ptr->init(ptr);
+ return ptr;
+ }
+
+ void EndEffectorTrajectory::tryConnectInitAndGoals ()
+ {}
+
+ void EndEffectorTrajectory::startSolve ()
+ {
+ //core::PathPlanner::startSolve();
+ //problem().checkProblem ();
+ if (!problem().robot ()) {
+ std::string msg ("No device in problem.");
+ hppDout (error, msg);
+ throw std::runtime_error (msg);
+ }
+
+ if (!problem().initConfig ()) {
+ std::string msg ("No init config in problem.");
+ hppDout (error, msg);
+ throw std::runtime_error (msg);
+ }
+
+ // Tag init and goal configurations in the roadmap
+ roadmap()->resetGoalNodes ();
+
+ SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem().steeringMethod()));
+ if (!sm)
+ throw std::invalid_argument ("Steering method must be of type hpp::manipulation::steeringMethod::EndEffectorTrajectory");
+
+ if (!sm->constraints() || !sm->constraints()->configProjector())
+ throw std::invalid_argument ("Steering method constraint has no ConfigProjector.");
+ core::ConfigProjectorPtr_t constraints (sm->constraints()->configProjector());
+
+ const constraints::ImplicitPtr_t& trajConstraint = sm->trajectoryConstraint ();
+ if (!trajConstraint)
+ throw std::invalid_argument ("EndEffectorTrajectory has no trajectory constraint.");
+ if (!sm->trajectory())
+ throw std::invalid_argument ("EndEffectorTrajectory has no trajectory.");
+
+ const core::NumericalConstraints_t& ncs = constraints->numericalConstraints();
+ bool ok = false;
+ for (std::size_t i = 0; i < ncs.size(); ++i) {
+ if (ncs[i] == trajConstraint) {
+ ok = true;
+ break; // Same pointer
+ }
+ // Here, we do not check the right hand side on purpose.
+ // if (*ncs[i] == *trajConstraint) {
+ if (ncs[i]->functionPtr() == trajConstraint->functionPtr()
+ && ncs[i]->comparisonType() == trajConstraint->comparisonType()) {
+ ok = true;
+ // TODO We should only modify the path constraint.
+ // However, only the pointers to implicit constraints are copied
+ // while we would like the implicit constraints to be copied as well.
+ ncs[i]->rightHandSideFunction (sm->trajectory());
+ break; // logically identical
+ }
+ }
+ if (!ok) {
+ HPP_THROW (std::logic_error, "EndEffectorTrajectory could not find "
+ "constraint " << trajConstraint->function());
+ }
+ }
+
+ void EndEffectorTrajectory::oneStep ()
+ {
+ SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem().steeringMethod()));
+ if (!sm)
+ throw std::invalid_argument ("Steering method must be of type hpp::manipulation::steeringMethod::EndEffectorTrajectory");
+ if (!sm->trajectoryConstraint ())
+ throw std::invalid_argument ("EndEffectorTrajectory has no trajectory constraint.");
+ if (!sm->trajectory())
+ throw std::invalid_argument ("EndEffectorTrajectory has no trajectory.");
+
+ if (!sm->constraints() || !sm->constraints()->configProjector())
+ throw std::invalid_argument ("Steering method constraint has no ConfigProjector.");
+ core::ConfigProjectorPtr_t constraints (sm->constraints()->configProjector());
+
+ core::ConfigValidationPtr_t cfgValidation (problem().configValidations());
+ core:: PathValidationPtr_t pathValidation (problem().pathValidation());
+ core:: ValidationReportPtr_t cfgReport;
+ core::PathValidationReportPtr_t pathReport;
+
+ core::interval_t timeRange (sm->timeRange());
+
+ std::vector<core::Configuration_t> qs (configurations(*problem().initConfig()));
+ if (qs.empty()) {
+ hppDout (info, "Failed to generate initial configs.");
+ return;
+ }
+
+ // Generate a valid initial configuration.
+ bool success = false;
+ bool resetRightHandSide = true;
+ std::size_t i;
+
+ vector_t times (nDiscreteSteps_+1);
+ matrix_t steps (problem().robot()->configSize(), nDiscreteSteps_+1);
+
+ times[0] = timeRange.first;
+ for (int j = 1; j < nDiscreteSteps_; ++j)
+ times[j] = timeRange.first + j * (timeRange.second - timeRange.first) / nDiscreteSteps_;
+ times[nDiscreteSteps_] = timeRange.second;
+
+ for (i = 0; i < qs.size(); ++i)
+ {
+ if (resetRightHandSide) {
+ constraints->rightHandSideAt (times[0]);
+ resetRightHandSide = false;
+ }
+ Configuration_t& q (qs[i]);
+ if (!constraints->apply (q)) continue;
+ if (!cfgValidation->validate (q, cfgReport)) continue;
+ resetRightHandSide = true;
+
+ steps.col(0) = q;
+ success = true;
+ for (int j = 1; j <= nDiscreteSteps_; ++j) {
+ constraints->rightHandSideAt (times[j]);
+ hppDout (info, "RHS: " << setpyformat << constraints->rightHandSide().transpose());
+ steps.col(j) = steps.col(j-1);
+ if (!constraints->apply (steps.col(j))) {
+ hppDout (info, "Failed to generate destination config.\n" << setpyformat
+ << *constraints
+ << "\nq=" << one_line (q));
+ success = false;
+ break;
+ }
+ }
+ if (!success) continue;
+ success = false;
+
+ if (!cfgValidation->validate (steps.col(nDiscreteSteps_), cfgReport)) {
+ hppDout (info, "Destination config is in collision.");
+ continue;
+ }
+
+ core::PathPtr_t path = sm->projectedPath(times, steps);
+ if (!path) {
+ hppDout (info, "Steering method failed.\n" << setpyformat
+ << "times: " << one_line(times) << '\n'
+ << "configs:\n" << condensed(steps.transpose()) << '\n'
+ );
+ continue;
+ }
+
+ core::PathPtr_t validPart;
+ if (!pathValidation->validate (path, false, validPart, pathReport)) {
+ hppDout (info, "Path is in collision.");
+ continue;
+ }
+
+ roadmap()->initNode (boost::make_shared<Configuration_t>(steps.col(0)));
+ core::NodePtr_t init = roadmap()-> initNode ();
+ core::NodePtr_t goal = roadmap()->addGoalNode (
+ boost::make_shared<Configuration_t>(steps.col(nDiscreteSteps_)));
+ roadmap()->addEdge (init, goal, path);
+ success = true;
+ if (feasibilityOnly_) break;
+ }
+ }
+
+ std::vector<core::Configuration_t> EndEffectorTrajectory::configurations(const core::Configuration_t& q_init)
+ {
+ if (!ikSolverInit_) {
+ std::vector<core::Configuration_t> configs(nRandomConfig_ + 1);
+ configs[0] = q_init;
+ for (int i = 1; i < nRandomConfig_ + 1; ++i)
+ problem().configurationShooter()->shoot(configs[i]);
+ return configs;
+ }
+
+ // TODO Compute the target and call ikSolverInit_
+ // See https://gepgitlab.laas.fr/airbus-xtct/hpp_airbus_xtct for an
+ // example using IKFast.
+ throw std::runtime_error ("Using an IkSolverInitialization is not implemented yet");
+ }
+
+ EndEffectorTrajectory::EndEffectorTrajectory (const core::Problem& problem)
+ : core::PathPlanner (problem)
+ {}
+
+ EndEffectorTrajectory::EndEffectorTrajectory (const core::Problem& problem, const core::RoadmapPtr_t& roadmap)
+ : core::PathPlanner (problem, roadmap)
+ {}
+
+ void EndEffectorTrajectory::checkFeasibilityOnly (bool enable)
+ {
+ feasibilityOnly_ = enable;
+ }
+
+ void EndEffectorTrajectory::init (const EndEffectorTrajectoryWkPtr_t& weak)
+ {
+ core::PathPlanner::init (weak);
+ weak_ = weak;
+ nRandomConfig_ = 10;
+ nDiscreteSteps_ = 1;
+ feasibilityOnly_ = true;
+ }
+ } // namespace pathPlanner
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index a75cd757a8eb622b65eb626ab7fcdf6c5ef6e948..92eede11847f4842373c0a733e8c1eac6fa38b7f 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -57,6 +57,7 @@
#include "hpp/manipulation/graph-node-optimizer.hh"
#include "hpp/manipulation/path-optimization/random-shortcut.hh"
#include "hpp/manipulation/path-optimization/enforce-transition-semantic.hh"
+#include "hpp/manipulation/path-planner/end-effector-trajectory.hh"
#include "hpp/manipulation/problem-target/state.hh"
#include "hpp/manipulation/steering-method/cross-state-optimization.hh"
#include "hpp/manipulation/steering-method/graph.hh"
@@ -121,6 +122,7 @@ namespace hpp {
robotType ("hpp::manipulation::Device");
pathPlanners.add ("M-RRT", ManipulationPlanner::create);
+ pathPlanners.add ("EndEffectorTrajectory", pathPlanner::EndEffectorTrajectory::createWithRoadmap);
pathValidations.add ("Graph-Discretized" , createDiscretizedCollisionGraphPathValidation);
pathValidations.add ("Graph-DiscretizedCollision" , createDiscretizedCollisionGraphPathValidation);
diff --git a/src/steering-method/end-effector-trajectory.cc b/src/steering-method/end-effector-trajectory.cc
index 776f0d68d1b5c1ea1bafd5308c1d8531c88a8e97..3825b02534d06ee3f8fcd50f88a91cece5a17e9d 100644
--- a/src/steering-method/end-effector-trajectory.cc
+++ b/src/steering-method/end-effector-trajectory.cc
@@ -26,7 +26,9 @@
#include <hpp/constraints/implicit.hh>
#include <hpp/core/config-projector.hh>
+#include <hpp/core/interpolated-path.hh>
#include <hpp/core/path.hh>
+#include <hpp/core/path-vector.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/straight-path.hh>
@@ -80,6 +82,34 @@ namespace hpp {
};
}
+ 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();
+ core::PathVectorPtr_t path = core::PathVector::create(7, 6);
+ if (points.rows() == 1)
+ path->appendPath (core::StraightPath::create (se3,
+ 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;
+ }
+
void EndEffectorTrajectory::trajectoryConstraint (const constraints::ImplicitPtr_t& ic)
{
constraint_ = ic;
@@ -112,7 +142,48 @@ namespace hpp {
ConfigurationIn_t q2) const
{
try {
+ core::ConstraintSetPtr_t c (getUpdatedConstraints());
+
+ return core::StraightPath::create (problem().robot(), q1, q2, timeRange_, c);
+ } catch (const std::exception& e) {
+ std::cout << timeRange_.first << ", " << timeRange_.second << '\n';
+ if (eeTraj_)
+ std::cout << (*eeTraj_) (vector_t::Constant(1,timeRange_.first )) << '\n'
+ << (*eeTraj_) (vector_t::Constant(1,timeRange_.second)) << std::endl;
+ std::cout << *constraints() << std::endl;
+ std::cout << e.what() << std::endl;
+ throw;
+ }
+ }
+
+ PathPtr_t EndEffectorTrajectory::projectedPath (vectorIn_t times,
+ matrixIn_t configs) const
+ {
+ core::ConstraintSetPtr_t c (getUpdatedConstraints());
+
+ size_type N = configs.cols();
+ if (timeRange_.first != times[0] || timeRange_.second != times[N-1]) {
+ HPP_THROW (std::logic_error, "Time range (" << timeRange_.first <<
+ ", " << timeRange_.second << ") does not match configuration "
+ "times (" << times[0] << ", " << times[N-1]);
+ }
+
+ using core::InterpolatedPath;
+ using core::InterpolatedPathPtr_t;
+
+ InterpolatedPathPtr_t path = InterpolatedPath::create(
+ problem().robot(), configs.col(0), configs.col(N-1), timeRange_, c);
+
+ for (size_type i = 1; i < configs.cols()-1; ++i)
+ path->insert(times[i], configs.col(i));
+
+ return path;
+ }
+
+ core::ConstraintSetPtr_t EndEffectorTrajectory::getUpdatedConstraints () const
+ {
if (!eeTraj_) throw std::logic_error ("EndEffectorTrajectory not initialized.");
+
// Update (or check) the constraints
core::ConstraintSetPtr_t c (constraints());
if (!c || !c->configProjector()) {
@@ -144,18 +215,8 @@ namespace hpp {
HPP_THROW (std::logic_error, "EndEffectorTrajectory could not find "
"constraint " << constraint_->function());
}
-
- return core::StraightPath::create (problem().robot(), q1, q2, timeRange_, c);
- } catch (const std::exception& e) {
- std::cout << timeRange_.first << ", " << timeRange_.second << '\n';
- if (eeTraj_)
- std::cout << (*eeTraj_) (vector_t::Constant(1,timeRange_.first )) << '\n'
- << (*eeTraj_) (vector_t::Constant(1,timeRange_.second)) << std::endl;
- std::cout << *constraints() << std::endl;
- std::cout << e.what() << std::endl;
- throw;
+ return c;
}
- }
} // namespace steeringMethod
} // namespace manipulation
} // namespace hpp