Commit 0042584a authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Add python bindings for joint-bounds task

parent abe8dad5
......@@ -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);
}
......
......@@ -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
......
//
// 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");
}
}
}
//
// 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__
......@@ -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()
......@@ -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
......
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment