From e06ad3ad38c9e5199d4b504ab4b30c8568756298 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 31 Jan 2019 15:41:19 +0100
Subject: [PATCH] Add EndEffectorTrajectory steering method.
---
CMakeLists.txt | 1 +
.../end-effector-trajectory.hh | 98 +++++++++++
src/CMakeLists.txt | 1 +
src/problem-solver.cc | 2 +
.../end-effector-trajectory.cc | 161 ++++++++++++++++++
5 files changed, 263 insertions(+)
create mode 100644 include/hpp/manipulation/steering-method/end-effector-trajectory.hh
create mode 100644 src/steering-method/end-effector-trajectory.cc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b3c09fd0..2fcec30f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -102,6 +102,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/steering-method/cross-state-optimization.hh
include/hpp/manipulation/steering-method/fwd.hh
include/hpp/manipulation/steering-method/graph.hh
+ include/hpp/manipulation/steering-method/end-effector-trajectory.hh
)
ADD_SUBDIRECTORY(src)
diff --git a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
new file mode 100644
index 00000000..65bcaba6
--- /dev/null
+++ b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
@@ -0,0 +1,98 @@
+// Copyright (c) 2019, LAAS-CNRS
+// 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/>.
+
+#ifndef HPP_MANIPULATION_STEERING_METHOD_END_EFFECTOR_TRAJECTORY_HH
+#define HPP_MANIPULATION_STEERING_METHOD_END_EFFECTOR_TRAJECTORY_HH
+
+#include <hpp/core/steering-method.hh>
+
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/config.hh>
+
+namespace hpp {
+ namespace manipulation {
+ /// \addtogroup steering_method
+ /// \{
+ namespace steeringMethod {
+ HPP_PREDEF_CLASS (EndEffectorTrajectory);
+ typedef boost::shared_ptr < EndEffectorTrajectory > EndEffectorTrajectoryPtr_t;
+
+ using core::PathPtr_t;
+
+ /// Build StraightPath constrained by a varying right hand side constraint.
+ class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory
+ : public core::SteeringMethod
+ {
+ public:
+ typedef core::interval_t interval_t;
+
+ static EndEffectorTrajectoryPtr_t create (const core::Problem& problem)
+ {
+ EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory (problem));
+ ptr->init(ptr);
+ return ptr;
+ }
+
+ /// Set the constraint whose right hand side will vary.
+ void trajectoryConstraint (const constraints::ImplicitPtr_t& ic);
+
+ /// Set the right hand side of the function from a path
+ /// \param se3Output set to True if the output of path must be
+ /// understood as SE3.
+ void trajectory (const PathPtr_t& eeTraj, bool se3Output);
+
+ /// Set the right hand side of the function from another function.
+ /// \param eeTraj a function whose input space is of dimension 1.
+ /// \param timeRange the input range of eeTraj.
+ void trajectory (const DifferentiableFunctionPtr_t& eeTraj, const interval_t& timeRange);
+
+ const DifferentiableFunctionPtr_t& trajectory () const
+ {
+ return eeTraj_;
+ }
+
+ core::SteeringMethodPtr_t copy () const
+ {
+ EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory (*this));
+ ptr->init (ptr);
+ return ptr;
+ }
+
+ protected:
+ EndEffectorTrajectory (const core::Problem& problem)
+ : core::SteeringMethod (problem)
+ {}
+
+ EndEffectorTrajectory (const EndEffectorTrajectory& other)
+ : core::SteeringMethod (other),
+ eeTraj_ (other.eeTraj_),
+ timeRange_ (other.timeRange_),
+ constraint_ (other.constraint_)
+ {}
+
+ PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
+ private:
+ DifferentiableFunctionPtr_t eeTraj_;
+ interval_t timeRange_;
+ constraints::ImplicitPtr_t constraint_;
+ };
+ } // namespace steeringMethod
+ /// \}
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_STEERING_METHOD_END_EFFECTOR_TRAJECTORY_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e4b11af2..0c942364 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -51,6 +51,7 @@ SET(SOURCES
problem-target/state.cc
+ steering-method/end-effector-trajectory.cc
steering-method/cross-state-optimization.cc
steering-method/graph.cc
)
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index e2ed4d0b..e04aad15 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -57,6 +57,7 @@
#include "hpp/manipulation/problem-target/state.hh"
#include "hpp/manipulation/steering-method/cross-state-optimization.hh"
#include "hpp/manipulation/steering-method/graph.hh"
+#include "hpp/manipulation/steering-method/end-effector-trajectory.hh"
#if HPP_MANIPULATION_HAS_WHOLEBODY_STEP
#include <hpp/wholebody-step/small-steps.hh>
@@ -149,6 +150,7 @@ namespace hpp {
createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::Dubins>);
steeringMethods.add ("CrossStateOptimization-Snibud",
createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::Snibud>);
+ steeringMethods.add ("EndEffectorTrajectory", steeringMethod::EndEffectorTrajectory::create);
#if HPP_MANIPULATION_HAS_WHOLEBODY_STEP
pathOptimizers.add ("Walkgen", wholebodyStep::SmallSteps::create);
diff --git a/src/steering-method/end-effector-trajectory.cc b/src/steering-method/end-effector-trajectory.cc
new file mode 100644
index 00000000..776f0d68
--- /dev/null
+++ b/src/steering-method/end-effector-trajectory.cc
@@ -0,0 +1,161 @@
+// Copyright (c) 2019, LAAS-CNRS
+// 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/manipulation/steering-method/end-effector-trajectory.hh>
+
+#include <hpp/util/indent.hh>
+
+#include <hpp/pinocchio/util.hh>
+#include <hpp/pinocchio/device.hh>
+#include <hpp/pinocchio/urdf/util.hh>
+
+#include <hpp/constraints/differentiable-function.hh>
+#include <hpp/constraints/implicit.hh>
+
+#include <hpp/core/config-projector.hh>
+#include <hpp/core/path.hh>
+#include <hpp/core/problem.hh>
+#include <hpp/core/straight-path.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace steeringMethod {
+ namespace {
+ template <bool SE3>
+ class FunctionFromPath : public constraints::DifferentiableFunction
+ {
+ public:
+ FunctionFromPath (const PathPtr_t& p)
+ : DifferentiableFunction (1, 1, SE3 ? LiegroupSpace::SE3() : LiegroupSpace::Rn(p->outputSize())),
+ path_ (p)
+ {
+ assert (!SE3 ||
+ (p->outputSize() == 7 && p->outputDerivativeSize() == 6));
+ }
+
+ const PathPtr_t& path () const
+ {
+ return path_;
+ }
+
+ std::ostream& print (std::ostream& os) const
+ {
+ return os
+ << (SE3 ? "FunctionFromSE3Path: " : "FunctionFromPath: ")
+ << path_->timeRange().first << ", "
+ << path_->timeRange().second
+ ;
+ }
+
+ protected:
+ void impl_compute (core::LiegroupElementRef result, vectorIn_t arg) const
+ {
+ bool success = (*path_) (result.vector(), arg[0]);
+ if (!success) {
+ hppDout (warning, "Failed to evaluate path at param " << arg[0]
+ << incindent << iendl << *path_ << decindent);
+ }
+ }
+
+ void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const
+ {
+ path_->derivative (jacobian.col(0), arg[0], 1);
+ }
+
+ private:
+ PathPtr_t path_;
+ };
+ }
+
+ void EndEffectorTrajectory::trajectoryConstraint (const constraints::ImplicitPtr_t& ic)
+ {
+ constraint_ = ic;
+ if (eeTraj_) trajectory (eeTraj_, timeRange_);
+ }
+
+ void EndEffectorTrajectory::trajectory (const PathPtr_t& eeTraj, bool se3Output)
+ {
+ if (se3Output)
+ trajectory (DifferentiableFunctionPtr_t
+ (new FunctionFromPath <true > (eeTraj)), eeTraj->timeRange());
+ else
+ trajectory (DifferentiableFunctionPtr_t
+ (new FunctionFromPath <false> (eeTraj)), eeTraj->timeRange());
+ }
+
+ void EndEffectorTrajectory::trajectory (const DifferentiableFunctionPtr_t& eeTraj, const interval_t& tr)
+ {
+ assert (eeTraj->inputSize() == 1);
+ assert (eeTraj->inputDerivativeSize() == 1);
+ eeTraj_ = eeTraj;
+ timeRange_ = tr;
+
+ if (!constraint_) return;
+
+ constraint_->rightHandSideFunction (eeTraj_);
+ }
+
+ PathPtr_t EndEffectorTrajectory::impl_compute (ConfigurationIn_t q1,
+ ConfigurationIn_t q2) const
+ {
+ try {
+ if (!eeTraj_) throw std::logic_error ("EndEffectorTrajectory not initialized.");
+ // Update (or check) the constraints
+ core::ConstraintSetPtr_t c (constraints());
+ if (!c || !c->configProjector()) {
+ throw std::logic_error ("EndEffectorTrajectory steering method must "
+ "have a ConfigProjector");
+ }
+ ConfigProjectorPtr_t cp (c->configProjector());
+
+ const core::NumericalConstraints_t& ncs = cp->numericalConstraints();
+ bool ok = false;
+ for (std::size_t i = 0; i < ncs.size(); ++i) {
+ if (ncs[i] == constraint_) {
+ ok = true;
+ break; // Same pointer
+ }
+ // Here, we do not check the right hand side on purpose.
+ // if (*ncs[i] == *constraint_) {
+ if (ncs[i]->functionPtr() == constraint_->functionPtr()
+ && ncs[i]->comparisonType() == constraint_->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 (eeTraj_);
+ break; // logically identical
+ }
+ }
+ if (!ok) {
+ 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;
+ }
+ }
+ } // namespace steeringMethod
+ } // namespace manipulation
+} // namespace hpp
--
GitLab