Add python bindings for joint-bounds task

bindings/python/formulations/formulation.hpp

@@ -31,6 +31,7 @@
 #include "tsid/tasks/task-se3-equality.hpp"
 #include "tsid/tasks/task-com-equality.hpp"
 #include "tsid/tasks/task-actuation-bounds.hpp"
+#include "tsid/tasks/task-joint-bounds.hpp"
 
 namespace tsid
 {
@@ -56,6 +57,7 @@ namespace tsid
         .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_SE3, bp::args("task", "weight", "priorityLevel", "transition duration"))
         .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_COM, bp::args("task", "weight", "priorityLevel", "transition duration"))
         .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_Joint, bp::args("task", "weight", "priorityLevel", "transition duration"))
+        .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_JointBounds, bp::args("task", "weight", "priorityLevel", "transition duration"))
         .def("addActuationTask", &InvDynPythonVisitor::addActuationTask_Bounds, bp::args("task", "weight", "priorityLevel", "transition duration"))
         
         .def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight, bp::args("task_name", "weight"))
@@ -88,6 +90,9 @@ namespace tsid
       static bool addMotionTask_Joint(T & self, tasks::TaskJointPosture & task, double weight, unsigned int priorityLevel, double transition_duration){
         return self.addMotionTask(task, weight, priorityLevel, transition_duration);
       }
+      static bool addMotionTask_JointBounds(T & self, tasks::TaskJointBounds & task, double weight, unsigned int priorityLevel, double transition_duration){
+        return self.addMotionTask(task, weight, priorityLevel, transition_duration);
+      }
       static bool addActuationTask_Bounds(T & self, tasks::TaskActuationBounds & task, double weight, unsigned int priorityLevel, double transition_duration){
         return self.addActuationTask(task, weight, priorityLevel, transition_duration);
       }

bindings/python/tasks/expose-tasks.hpp

@@ -22,6 +22,7 @@
 #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"
+#include "tsid/bindings/python/tasks/task-joint-bounds.hpp"
 
 
 namespace tsid
@@ -32,6 +33,7 @@ namespace tsid
     void exposeTaskSE3Equality();
     void exposeTaskJointPosture();
     void exposeTaskActuationBounds();
+    void exposeTaskJointBounds();
 
     inline void exposeTasks()
     {
@@ -39,6 +41,7 @@ namespace tsid
       exposeTaskSE3Equality();
       exposeTaskJointPosture();
       exposeTaskActuationBounds();
+      exposeTaskJointBounds();
     }
     
   } // namespace python

bindings/python/tasks/task-joint-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-joint-bounds.hpp"
+#include "tsid/bindings/python/tasks/expose-tasks.hpp"
+
+namespace tsid
+{
+  namespace python
+  {
+    void exposeTaskJointBounds()
+    {
+      TaskJointBoundsPythonVisitor<tsid::tasks::TaskJointBounds>::expose("TaskJointBounds");
+    }
+  }
+}

bindings/python/tasks/task-joint-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_joint_bounds_hpp__
+#define __tsid_python_task_joint_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-joint-bounds.hpp"
+#include "tsid/robots/robot-wrapper.hpp"
+#include "tsid/math/constraint-bound.hpp"
+#include "tsid/math/constraint-base.hpp"
+
+namespace tsid
+{
+  namespace python
+  {    
+    namespace bp = boost::python;
+
+    template<typename Task>
+    struct TaskJointBoundsPythonVisitor
+    : public boost::python::def_visitor< TaskJointBoundsPythonVisitor<Task> >
+    {
+      
+      template<class PyClass>     
+      
+
+      void visit(PyClass& cl) const
+      {
+        cl
+        .def(bp::init<std::string, robots::RobotWrapper &, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("Time step")), "Default Constructor"))
+        .add_property("dim", &Task::dim, "return dimension size")
+        .def("setTimeStep", &TaskJointBoundsPythonVisitor::setTimeStep, bp::args("dt"))
+        .def("setVelocityBounds", &TaskJointBoundsPythonVisitor::setVelocityBounds, bp::args("lower", "upper"))
+        .def("setAccelerationBounds", &TaskJointBoundsPythonVisitor::setAccelerationBounds, bp::args("lower", "upper"))
+        .def("compute", &TaskJointBoundsPythonVisitor::compute, bp::args("t", "q", "v", "data"))
+        .def("getConstraint",  &TaskJointBoundsPythonVisitor::getConstraint)
+        .add_property("getAccelerationLowerBounds", bp::make_function(&TaskJointBoundsPythonVisitor::getAccelerationLowerBounds, bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("getAccelerationUpperBounds", bp::make_function(&TaskJointBoundsPythonVisitor::getAccelerationUpperBounds, bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("getVelocityLowerBounds", bp::make_function(&TaskJointBoundsPythonVisitor::getVelocityLowerBounds, bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("getVelocityUpperBounds", bp::make_function(&TaskJointBoundsPythonVisitor::getVelocityUpperBounds, bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("name", &TaskJointBoundsPythonVisitor::name)
+        ;
+      }
+      static std::string name(Task & self){
+        std::string name = self.name();
+        return name;
+      }
+      static math::ConstraintBound 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::ConstraintBound cons(self.getConstraint().name(),
+                                   self.getConstraint().lowerBound(),
+                                   self.getConstraint().upperBound());
+        return cons;
+      }
+      static math::ConstraintBound getConstraint(const Task & self){
+        math::ConstraintBound cons(self.getConstraint().name(),
+                                   self.getConstraint().lowerBound(),
+                                   self.getConstraint().upperBound());
+        return cons;
+      }
+      static const Eigen::VectorXd & getAccelerationLowerBounds (const Task & self){
+        return self.getAccelerationLowerBounds();
+      }  
+      static const Eigen::VectorXd & getAccelerationUpperBounds (const Task & self){
+        return self.getAccelerationUpperBounds();
+      }
+      static const Eigen::VectorXd & getVelocityLowerBounds (const Task & self){
+        return self.getVelocityLowerBounds();
+      }
+      static const Eigen::VectorXd & getVelocityUpperBounds (const Task & self){
+        return self.getVelocityUpperBounds();
+      }
+      static void setTimeStep (Task & self, const double dt){
+        return self.setTimeStep(dt);
+      }
+      static void setVelocityBounds (Task & self, const Eigen::VectorXd lower, const Eigen::VectorXd upper){
+        return self.setVelocityBounds(lower, upper);
+      }
+      static void setAccelerationBounds (Task & self, const Eigen::VectorXd lower, const Eigen::VectorXd upper){
+        return self.setAccelerationBounds(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(TaskJointBoundsPythonVisitor<Task>());
+      }
+    };
+  }
+}
+
+
+#endif // ifndef __tsid_python_task_actuation_bounds_hpp__

exercizes/ex_1_ur5.py

@@ -7,22 +7,25 @@ import plot_utils as plut
 import time
 from tsid_manipulator import TsidManipulator
 
-import ur5_conf as conf
-#import ur5_reaching_conf as conf
+#import ur5_conf as conf
+import ur5_reaching_conf as conf
 
 print "".center(conf.LINE_WIDTH,'#')
 print " Task Space Inverse Dynamics - Manipulator ".center(conf.LINE_WIDTH, '#')
 print "".center(conf.LINE_WIDTH,'#'), '\n'
 
 PLOT_EE_POS = 1
-PLOT_EE_VEL = 0
+PLOT_EE_VEL = 1
 PLOT_EE_ACC = 0
+PLOT_JOINT_VEL = 1
 PLOT_TORQUES = 1
 
 tsid = TsidManipulator(conf)
 
 N = conf.N_SIMULATION
 tau    = matlib.empty((tsid.robot.na, N))*nan
+q      = matlib.empty((tsid.robot.nq, N+1))*nan
+v      = matlib.empty((tsid.robot.nv, N+1))*nan
 ee_pos = matlib.empty((3, N))*nan
 ee_vel = matlib.empty((3, N))*nan
 ee_acc = matlib.empty((3, N))*nan
@@ -47,7 +50,7 @@ tsid.gui.addSphere('world/ee', conf.SPHERE_RADIUS, conf.EE_SPHERE_COLOR)
 tsid.gui.addSphere('world/ee_ref', conf.REF_SPHERE_RADIUS, conf.EE_REF_SPHERE_COLOR)
 
 t = 0.0
-q, v = tsid.q, tsid.v
+q[:,0], v[:,0] = tsid.q, tsid.v
 
 for i in range(0, N):
     time_start = time.time()
@@ -60,7 +63,7 @@ for i in range(0, N):
     sampleEE.acc(aEE)
     tsid.eeTask.setReference(sampleEE)
 
-    HQPData = tsid.formulation.computeProblemData(t, q, v)
+    HQPData = tsid.formulation.computeProblemData(t, q[:,i], v[:,i])
     # if i == 0: HQPData.print_all()
 
     sol = tsid.solver.solve(HQPData)
@@ -83,11 +86,11 @@ for i in range(0, N):
         print "Time %.3f"%(t)
         print "\ttracking err %s: %.3f"%(tsid.eeTask.name.ljust(20,'.'), norm(tsid.eeTask.position_error, 2))
 
-    q, v = tsid.integrate_dv(q, v, dv, conf.dt)
+    q[:,i+1], v[:,i+1] = tsid.integrate_dv(q[:,i], v[:,i], dv, conf.dt)
     t += conf.dt
     
     if i%conf.DISPLAY_N == 0: 
-        tsid.robot_display.display(q)
+        tsid.robot_display.display(q[:,i])
         tsid.gui.applyConfiguration('world/ee',     ee_pos[:,i].A1.tolist()+[0,0,0,1.])
         tsid.gui.applyConfiguration('world/ee_ref', ee_pos_ref[:,i].A1.tolist()+[0,0,0,1.])
 
@@ -140,4 +143,16 @@ if(PLOT_TORQUES):
         leg = ax[i].legend()
         leg.get_frame().set_alpha(0.5)
 
+if(PLOT_JOINT_VEL):    
+    (f, ax) = plut.create_empty_figure(tsid.robot.nv/2,2)
+    ax = ax.reshape(tsid.robot.nv)
+    for i in range(tsid.robot.nv):
+        ax[i].plot(time, v[i,:-1].A1, label='Joint vel '+str(i))
+        ax[i].plot([time[0], time[-1]], 2*[tsid.v_min[i,0]], ':')
+        ax[i].plot([time[0], time[-1]], 2*[tsid.v_max[i,0]], ':')
+        ax[i].set_xlabel('Time [s]')
+        ax[i].set_ylabel('Joint velocity [rad/s]')
+        leg = ax[i].legend()
+        leg.get_frame().set_alpha(0.5)
+        
 plt.show()

exercizes/tsid_manipulator.py

@@ -57,7 +57,14 @@ class TsidManipulator:
         actuationBoundsTask.setBounds(self.tau_min, self.tau_max)
         if(conf.w_torque_bounds>0.0):
             formulation.addActuationTask(actuationBoundsTask, conf.w_torque_bounds, 0, 0.0)
-                        
+        
+        jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, conf.dt)
+        self.v_max = conf.v_max_scaling * model.velocityLimit
+        self.v_min = -self.v_max
+        jointBoundsTask.setVelocityBounds(self.v_min, self.v_max)
+        if(conf.w_joint_bounds>0.0):
+            formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0, 0.0)
+        
         trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q)
         postureTask.setReference(trajPosture.computeNext())
         
@@ -67,6 +74,7 @@ class TsidManipulator:
         self.trajPosture = trajPosture
         self.postureTask  = postureTask
         self.actuationBoundsTask = actuationBoundsTask
+        self.jointBoundsTask = jointBoundsTask
         self.formulation = formulation
         self.solver = solver
         self.q = q

exercizes/ur5_reaching_conf.py

@@ -11,7 +11,7 @@ import os
 np.set_printoptions(precision=3, linewidth=200, suppress=True)
 LINE_WIDTH = 60
 
-N_SIMULATION = 3000             # number of time steps simulated
+N_SIMULATION = 500             # number of time steps simulated
 dt = 0.002                      # controller time step
 q0 = np.matrix([[ 0. , -1.0,  0.7,  0. ,  0. ,  0. ]]).T  # initial configuration
 
@@ -23,12 +23,14 @@ offset               = np.matrix([0.2, 0.0, 0.0]).T
 
 w_ee = 1.0                      # weight of end-effector task
 w_posture = 1e-3                # weight of joint posture task
-w_torque_bounds = 0.0           # weight of the torque bounds
+w_torque_bounds = 1.0           # weight of the torque bounds
+w_joint_bounds = 1.0            # weight of the joint bounds
 
 kp_ee = 100.0                   # proportional gain of end-effector constraint
 kp_posture = 1.0               # proportional gain of joint posture task
 
 tau_max_scaling = 0.4           # scaling factor of torque bounds
+v_max_scaling = 0.4             # scaling factor of velocity bounds
 
 ee_frame_name = "ee_fixed_joint"        # end-effector frame name
 ee_task_mask = np.matrix([1., 1, 1, 0, 0, 0]).T