From 1186e9e6c376ce82bf57c44ffee96970d7a02875 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 16 Jul 2014 10:24:55 +0200
Subject: [PATCH] Add MotionProjector Class
---
include/hpp/manipulation/fwd.hh | 3 +
include/hpp/manipulation/motion-projector.hh | 75 ++++++++++++++++++
src/motion-projector.cc | 81 ++++++++++++++++++++
3 files changed, 159 insertions(+)
create mode 100644 include/hpp/manipulation/motion-projector.hh
create mode 100644 src/motion-projector.cc
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index dbafc0ae..a47a56e0 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -75,6 +75,9 @@ namespace hpp {
typedef std::pair< GripperPtr_t, HandlePtr_t> Grasp_t;
typedef boost::shared_ptr <Grasp_t> GraspPtr_t;
typedef std::map <DifferentiableFunctionPtr_t, GraspPtr_t> GraspsMap_t;
+
+ HPP_PREDEF_CLASS(MotionProjector);
+ typedef boost::shared_ptr < MotionProjector > MotionProjectorPtr_t;
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/motion-projector.hh b/include/hpp/manipulation/motion-projector.hh
new file mode 100644
index 00000000..677baf01
--- /dev/null
+++ b/include/hpp/manipulation/motion-projector.hh
@@ -0,0 +1,75 @@
+//
+// Copyright (c) 2014 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_MOTION_PROJECTOR_HH
+# define HPP_MANIPULATION_MOTION_PROJECTOR_HH
+
+# include <hpp/manipulation/config-projector.hh>
+
+namespace hpp {
+ namespace manipulation {
+ /// Implicit non-linear constraint with offset
+ ///
+ /// Defined by a list of vector-valued functions and solved numerically
+ /// by Newton Raphson like method.
+ /// Store locked degrees of freedom for performance optimisation.
+ class HPP_MANIPULATION_DLLAPI MotionProjector : public ConfigProjector
+ {
+ public:
+ /// Return shared pointer to new object
+ /// \param robot robot the constraint applies to.
+ /// \param errorThreshold norm of the value of the constraint under which
+ /// the constraint is considered satified,
+ /// \param maxIterations maximal number of iteration in the resolution of
+ /// the constraint.
+ static MotionProjectorPtr_t create (const DevicePtr_t& robot,
+ const std::string& name,
+ value_type errorThreshold,
+ size_type maxIterations);
+
+ /// Add constraint
+ void addConstraint (const DifferentiableFunctionPtr_t& constraint);
+
+ protected:
+ /// Constructor
+ /// \param robot robot the constraint applies to.
+ /// \param errorThreshold norm of the value of the constraint under which
+ /// the constraint is considered satified,
+ /// \param maxIterations maximal number of iteration in the resolution of
+ /// the constraint.
+ MotionProjector (const DevicePtr_t& robot, const std::string& name,
+ value_type errorThreshold, size_type maxIterations);
+ /// Store weak pointer to itself
+ void init (const MotionProjectorPtr_t& self)
+ {
+ ConfigProjector::init (self);
+ weak_ = self;
+ }
+ /// Numerically solve constraint
+ virtual bool impl_compute (ConfigurationOut_t configuration);
+
+ private:
+ virtual std::ostream& print (std::ostream& os) const;
+
+ void resize ();
+ mutable vector_t offset_;
+ MotionProjectorWkPtr_t weak_;
+ }; // class MotionProjector
+ } // namespace manipulation
+} // namespace hpp
+#endif // HPP_MANIPULATION_MOTION_PROJECTOR_HH
diff --git a/src/motion-projector.cc b/src/motion-projector.cc
new file mode 100644
index 00000000..5a3b98c8
--- /dev/null
+++ b/src/motion-projector.cc
@@ -0,0 +1,81 @@
+//
+// Copyright (c) 2014 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 <limits>
+#include <hpp/util/debug.hh>
+#include <hpp/model/configuration.hh>
+#include <hpp/model/device.hh>
+#include <hpp/core/config-projector.hh>
+#include <hpp/core/constraint-set.hh>
+#include <hpp/core/differentiable-function.hh>
+#include <hpp/core/locked-dof.hh>
+
+#include "hpp/manipulation/motion-projector.hh"
+
+namespace hpp {
+ namespace manipulation {
+ MMotionProjectorPtr_t MotionProjector::create (const DevicePtr_t& robot,
+ const std::string& name,
+ value_type errorThreshold,
+ size_type maxIterations)
+ {
+ MotionProjector* ptr = new MotionProjector (robot, name, errorThreshold,
+ maxIterations);
+ MotionProjectorPtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ MotionProjector::MotionProjector (const DevicePtr_t& robot,
+ const std::string& name,
+ value_type errorThreshold,
+ size_type maxIterations) :
+ ConfigProjector (robot, name, errorThreshold, maxIterations), weak_ ()
+ {
+ }
+
+ void MotionProjector::addConstraint
+ (const DifferentiableFunctionPtr_t& constraint)
+ {
+ ConfigProjector::addConstraint (constraint);
+ resize ();
+ }
+
+ void MotionProjector::resize ()
+ {
+ offset_.resize(value_.size());
+ }
+
+ bool MotionProjector::impl_compute (ConfigurationOut_t configuration)
+ {
+ return false;
+ }
+
+ std::ostream& MotionProjector::print (std::ostream& os) const
+ {
+ os << "Motion projector: " << name () << ", contains" << std::endl;
+ for (NumericalConstraints_t::const_iterator it = constraints_.begin ();
+ it != constraints_.end (); it++) {
+ const DifferentiableFunction& f (*(it->function));
+ os << f << std::endl;
+ }
+ return os;
+ }
+
+ } // namespace manipulation
+} // namespace hpp
--
GitLab