Add task for actuation bounds

bindings/python/tasks/expose-tasks.hpp

@@ -21,7 +21,7 @@
 #include "tsid/bindings/python/tasks/task-com-equality.hpp"
 #include "tsid/bindings/python/tasks/task-se3-equality.hpp"
 #include "tsid/bindings/python/tasks/task-joint-posture.hpp"
-
+#include "tsid/bindings/python/tasks/task-actuation-bounds.hpp"
 
 
 namespace tsid
@@ -31,12 +31,14 @@ namespace tsid
     void exposeTaskComEquality();
     void exposeTaskSE3Equality();
     void exposeTaskJointPosture();
+    void exposeTaskActuationBounds();
 
     inline void exposeTasks()
     {
       exposeTaskComEquality();
       exposeTaskSE3Equality();
       exposeTaskJointPosture();
+      exposeTaskActuationBounds();
     }
     
   } // namespace python

bindings/python/tasks/task-actuation-bounds.cpp

+//
+// Copyright (c) 2018 CNRS
+//
+// This file is part of tsid
+// tsid 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.
+// tsid 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
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#include "tsid/bindings/python/tasks/task-actuation-bounds.hpp"
+#include "tsid/bindings/python/tasks/expose-tasks.hpp"
+
+namespace tsid
+{
+  namespace python
+  {
+    void exposeTaskActuationBounds()
+    {
+      TaskActuationBoundsPythonVisitor<tsid::tasks::TaskActuationBounds>::expose("TaskActuationBounds");
+    }
+  }
+}

bindings/python/tasks/task-actuation-bounds.hpp

+//
+// Copyright (c) 2018 CNRS
+//
+// This file is part of tsid
+// tsid 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.
+// tsid 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
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __tsid_python_task_actuation_bounds_hpp__
+#define __tsid_python_task_actuation_bounds_hpp__
+
+#include <boost/python.hpp>
+#include <string>
+#include <eigenpy/eigenpy.hpp>
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+
+#include "tsid/tasks/task-actuation-bounds.hpp"
+#include "tsid/robots/robot-wrapper.hpp"
+#include "tsid/math/constraint-inequality.hpp"
+#include "tsid/math/constraint-base.hpp"
+
+namespace tsid
+{
+  namespace python
+  {    
+    namespace bp = boost::python;
+
+    template<typename Task>
+    struct TaskActuationBoundsPythonVisitor
+    : public boost::python::def_visitor< TaskActuationBoundsPythonVisitor<Task> >
+    {
+      
+      template<class PyClass>     
+      
+
+      void visit(PyClass& cl) const
+      {
+        cl
+        .def(bp::init<std::string, robots::RobotWrapper &> ((bp::arg("name"), bp::arg("robot")), "Default Constructor"))
+        .add_property("dim", &Task::dim, "return dimension size")
+        .add_property("mask", bp::make_function(&TaskActuationBoundsPythonVisitor::getmask, bp::return_value_policy<bp::copy_const_reference>()), "Return mask")
+        .def("mask", &TaskActuationBoundsPythonVisitor::setmask, bp::arg("mask"))
+        .def("setBounds", &TaskActuationBoundsPythonVisitor::setBounds, bp::args("lower", "upper"))
+        .def("compute", &TaskActuationBoundsPythonVisitor::compute, bp::args("t", "q", "v", "data"))
+        .def("getConstraint",  &TaskActuationBoundsPythonVisitor::getConstraint)
+        .add_property("getLowerBounds", bp::make_function(&TaskActuationBoundsPythonVisitor::getLowerBounds, bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("getUpperBounds", bp::make_function(&TaskActuationBoundsPythonVisitor::getUpperBounds, bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("name", &TaskActuationBoundsPythonVisitor::name)
+        ;
+      }
+      static std::string name(Task & self){
+        std::string name = self.name();
+        return name;
+      }
+      static math::ConstraintInequality compute(Task & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
+        self.compute(t, q, v, data);
+        math::ConstraintInequality cons(self.getConstraint().name(), self.getConstraint().matrix(),
+                                        self.getConstraint().lowerBound(), self.getConstraint().upperBound());
+        return cons;
+      }
+      static math::ConstraintInequality getConstraint(const Task & self){
+        math::ConstraintInequality cons(self.getConstraint().name(), self.getConstraint().matrix(),
+                                        self.getConstraint().lowerBound(), self.getConstraint().upperBound());
+        return cons;
+      }
+      static const Eigen::VectorXd & getmask(const Task & self){
+        return self.mask();
+      }
+      static void setmask (Task & self, const Eigen::VectorXd mask){
+        return self.mask(mask);
+      }
+      static const Eigen::VectorXd & getLowerBounds (const Task & self){
+        return self.getLowerBounds();
+      }  
+      static const Eigen::VectorXd & getUpperBounds (const Task & self){
+        return self.getUpperBounds();
+      }    
+      static void setBounds (Task & self, const Eigen::VectorXd lower, const Eigen::VectorXd upper){
+        return self.setBounds(lower, upper);
+      }
+      static void expose(const std::string & class_name)
+      {
+        std::string doc = "Task info.";
+        bp::class_<Task>(class_name.c_str(),
+                          doc.c_str(),
+                          bp::no_init)
+        .def(TaskActuationBoundsPythonVisitor<Task>());
+      }
+    };
+  }
+}
+
+
+#endif // ifndef __tsid_python_task_actuation_bounds_hpp__

include/tsid/tasks/task-actuation-bounds.hpp

@@ -15,15 +15,56 @@
 // <http://www.gnu.org/licenses/>.
 //
 
-#ifndef __invdyn_tasks_fwd_hpp__
-#define __invdyn_tasks_fwd_hpp__
+#ifndef __invdyn_task_actuation_bounds_hpp__
+#define __invdyn_task_actuation_bounds_hpp__
+
+#include <tsid/tasks/task-actuation.hpp>
+#include <tsid/trajectories/trajectory-base.hpp>
+#include <tsid/math/constraint-inequality.hpp>
 
 namespace tsid
 {
   namespace tasks
   {
-    
+
+    class TaskActuationBounds : public TaskActuation
+    {
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+      typedef math::Index Index;
+      typedef trajectories::TrajectorySample TrajectorySample;
+      typedef math::Vector Vector;
+      typedef math::VectorXi VectorXi;
+      typedef math::ConstraintInequality ConstraintInequality;
+      typedef pinocchio::Data Data;
+
+      TaskActuationBounds(const std::string & name,
+                          RobotWrapper & robot);
+
+      int dim() const;
+
+      const ConstraintBase & compute(const double t,
+                                     ConstRefVector q,
+                                     ConstRefVector v,
+                                     const Data & data);
+
+      const ConstraintBase & getConstraint() const;
+
+      void setBounds(ConstRefVector lower, ConstRefVector upper);
+      const Vector & getLowerBounds() const;
+      const Vector & getUpperBounds() const;
+
+      const Vector & mask() const;
+      void mask(const Vector & mask);
+
+    protected:
+      Vector m_mask;
+      VectorXi m_activeAxes;
+      ConstraintInequality m_constraint;
+    };
+
   }
 }
 
-#endif // ifndef __invdyn_tasks_fwd_hpp__
+#endif // ifndef __invdyn_task_actuation_bounds_hpp__

src/tasks/task-actuation-bounds.cpp

@@ -15,11 +15,80 @@
 // <http://www.gnu.org/licenses/>.
 //
 
+#include <tsid/tasks/task-actuation-bounds.hpp>
+#include "tsid/robots/robot-wrapper.hpp"
+
 namespace tsid
 {
   namespace tasks
   {
-    
-    
+    using namespace math;
+    using namespace trajectories;
+    using namespace pinocchio;
+
+    TaskActuationBounds::TaskActuationBounds(const std::string & name,
+                                             RobotWrapper & robot):
+      TaskActuation(name, robot),
+      m_constraint(name, robot.na(), robot.na())
+    {
+      Vector m = Vector::Ones(robot.na());
+      mask(m);
+    }
+
+    const Vector & TaskActuationBounds::mask() const
+    {
+      return m_mask;
+    }
+
+    void TaskActuationBounds::mask(const Vector & m)
+    {
+      assert(m.size()==m_robot.na());
+      m_mask = m;
+      const Vector::Index dim = static_cast<Vector::Index>(m.sum());
+      Matrix S = Matrix::Zero(dim, m_robot.na());
+      m_activeAxes.resize(dim);
+      unsigned int j=0;
+      for(unsigned int i=0; i<m.size(); i++)
+        if(m(i)!=0.0)
+        {
+          assert(m(i)==1.0);
+          S(j,i) = 1.0;
+          m_activeAxes(j) = i;
+          j++;
+        }
+      m_constraint.resize((unsigned int)dim, m_robot.na());
+      m_constraint.setMatrix(S);
+    }
+
+    int TaskActuationBounds::dim() const
+    {
+      return (int)m_mask.sum();
+    }
+
+    const Vector & TaskActuationBounds::getLowerBounds() const { return m_constraint.lowerBound(); }
+
+    const Vector & TaskActuationBounds::getUpperBounds() const { return m_constraint.upperBound(); }
+
+    void TaskActuationBounds::setBounds(ConstRefVector lower, ConstRefVector upper)
+    {
+      assert(lower.size()==dim());
+      assert(upper.size()==dim());
+      m_constraint.setLowerBound(lower);
+      m_constraint.setUpperBound(upper);
+    }
+
+    const ConstraintBase & TaskActuationBounds::getConstraint() const
+    {
+      return m_constraint;
+    }
+
+    const ConstraintBase & TaskActuationBounds::compute(const double ,
+                                                        ConstRefVector ,
+                                                        ConstRefVector ,
+                                                        const Data & )
+    {
+      return m_constraint;
+    }
+
   }
 }