Add MotionProjector Class

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
 

include/hpp/manipulation/motion-projector.hh

+//
+// 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

src/motion-projector.cc

+//
+// 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