Commit 1aa9de93 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'devel' of github.com:stack-of-tasks/tsid into devel

parents 243e0728 dd400cb3
......@@ -88,7 +88,7 @@ SET(BOOST_OPTIONAL_COMPONENTS "")
IF(BUILD_PYTHON_INTERFACE)
SET(BOOST_OPTIONAL_COMPONENTS ${BOOST_OPTIONAL_COMPONENTS} python)
FINDPYTHON()
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
ADD_REQUIRED_DEPENDENCY("eigenpy >= 1.4.0")
ENDIF(BUILD_PYTHON_INTERFACE)
......
......@@ -30,6 +30,9 @@
#include "tsid/tasks/task-joint-posture.hpp"
#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
{
namespace python
......@@ -54,7 +57,8 @@ 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"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContact6dDeprecated, bp::args("contact"))
......@@ -86,6 +90,12 @@ 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);
}
static bool updateTaskWeight(T& self, const std::string & task_name, double weight){
return self.updateTaskWeight(task_name, weight);
}
......
......@@ -48,6 +48,7 @@ namespace tsid
.def(bp::init<std::string, std_vec, pinocchio::JointModelVariant, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("roottype"), bp::arg("verbose")), "Default constructor without RootJoint."))
.add_property("nq", &Robot::nq)
.add_property("nv", &Robot::nv)
.add_property("na", &Robot::na)
.def("model", &RobotPythonVisitor::model)
.def("data", &RobotPythonVisitor::data)
......@@ -72,6 +73,9 @@ namespace tsid
.def("frameVelocity", &RobotPythonVisitor::frameVelocity, bp::args("data", "index"))
.def("frameAcceleration", &RobotPythonVisitor::frameAcceleration, bp::args("data", "index"))
.def("frameClassicAcceleration", &RobotPythonVisitor::frameClassicAcceleration, bp::args("data", "index"))
.def("frameVelocityWorldOriented", &RobotPythonVisitor::frameVelocityWorldOriented, bp::args("data", "index"))
.def("frameAccelerationWorldOriented", &RobotPythonVisitor::frameAccelerationWorldOriented, bp::args("data", "index"))
.def("frameClassicAccelerationWorldOriented", &RobotPythonVisitor::frameClassicAccelerationWorldOriented, bp::args("data", "index"))
;
}
......@@ -137,7 +141,15 @@ namespace tsid
static pinocchio::Motion frameClassicAcceleration(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
return self.frameClassicAcceleration(data, index);
}
static pinocchio::Motion frameVelocityWorldOriented(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
return self.frameVelocityWorldOriented(data, index);
}
static pinocchio::Motion frameAccelerationWorldOriented(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
return self.frameAccelerationWorldOriented(data, index);
}
static pinocchio::Motion frameClassicAccelerationWorldOriented(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
return self.frameClassicAccelerationWorldOriented(data, index);
}
static void expose(const std::string & class_name)
{
std::string doc = "Robot Wrapper info.";
......
......@@ -21,7 +21,8 @@
#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"
#include "tsid/bindings/python/tasks/task-joint-bounds.hpp"
namespace tsid
......@@ -31,12 +32,16 @@ namespace tsid
void exposeTaskComEquality();
void exposeTaskSE3Equality();
void exposeTaskJointPosture();
void exposeTaskActuationBounds();
void exposeTaskJointBounds();
inline void exposeTasks()
{
exposeTaskComEquality();
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-actuation-bounds.hpp"
#include "tsid/bindings/python/tasks/expose-tasks.hpp"
namespace tsid
{
namespace python
{
void exposeTaskActuationBounds()
{
TaskActuationBoundsPythonVisitor<tsid::tasks::TaskActuationBounds>::expose("TaskActuationBounds");
}
}
}
//
// 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__
//
// 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__
import numpy as np
import numpy.matlib as matlib
from numpy import nan
from numpy.linalg import norm as norm
import matplotlib.pyplot as plt
import plot_utils as plut
import time
import pinocchio as se3
import tsid
import gepetto.corbaserver
import commands
import os
import ur5_conf as conf
print "".center(conf.LINE_WIDTH,'#')
print " Joint Space Inverse Dynamics - Manipulator ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH,'#'), '\n'
PLOT_JOINT_POS = 1
PLOT_JOINT_VEL = 1
PLOT_JOINT_ACC = 1
PLOT_TORQUES = 0
USE_VIEWER = 1
vector = se3.StdVec_StdString()
vector.extend(item for item in conf.path)
robot = tsid.RobotWrapper(conf.urdf, vector, False)
model = robot.model()
formulation = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
q0 = conf.q0
v0 = np.matrix(np.zeros(robot.nv)).T
formulation.computeProblemData(0.0, q0, v0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * matlib.ones(robot.nv).T)
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) * matlib.ones(robot.nv).T)
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q0)
postureTask.setReference(trajPosture.computeNext())
v_max = conf.v_max_scaling * model.velocityLimit
v_min = -v_max
#jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, conf.dt)
#jointBoundsTask.setVelocityBounds(v_min, v_max)
#formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0, 0.0)
solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
if(USE_VIEWER):
robot_display = se3.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path, ])
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
gepetto.corbaserver.Client()
robot_display.initDisplay(loadModel=True)
robot_display.displayCollisions(False)
robot_display.displayVisuals(True)
robot_display.display(q0)
robot_display.viewer.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM)
N = conf.N_SIMULATION
tau = matlib.empty((robot.na, N))*nan
q = matlib.empty((robot.nq, N+1))*nan
v = matlib.empty((robot.nv, N+1))*nan
dv = matlib.empty((robot.nv, N+1))*nan
q_ref = matlib.empty((robot.nq, N))*nan
v_ref = matlib.empty((robot.nv, N))*nan
dv_ref = matlib.empty((robot.nv, N))*nan
dv_des = matlib.empty((robot.nv, N))*nan
samplePosture = trajPosture.computeNext()
amp = np.matrix([0.2, 0.3, 0.4, 0.0, 0.0, 0.0]).T # amplitude
phi = np.matrix([0.0, 0.5*np.pi, 0.0, 0.0, 0.0, 0.0]).T # phase
two_pi_f = 2*np.pi*np.matrix([1.0, 0.5, 0.3, 0.0, 0.0, 0.0]).T # frequency (time 2 PI)
two_pi_f_amp = np.multiply(two_pi_f, amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
t = 0.0
dt = conf.dt
q[:,0], v[:,0] = q0, v0
for i in range(0, N):
time_start = time.time()
# set reference trajectory
q_ref[:,i] = q0 + np.multiply(amp, matlib.sin(two_pi_f*t + phi))
v_ref[:,i] = np.multiply(two_pi_f_amp, matlib.cos(two_pi_f*t + phi))
dv_ref[:,i] = np.multiply(two_pi_f_squared_amp, -matlib.sin(two_pi_f*t + phi))
samplePosture.pos(q_ref[:,i])
samplePosture.vel(v_ref[:,i])
samplePosture.acc(dv_ref[:,i])
postureTask.setReference(samplePosture)
HQPData = formulation.computeProblemData(t, q[:,i], v[:,i])
sol = solver.solve(HQPData)
if(sol.status!=0):
print "Time %.3f QP problem could not be solved! Error code:"%t, sol.status
break
tau[:,i] = formulation.getActuatorForces(sol)
dv[:,i] = formulation.getAccelerations(sol)
dv_des[:,i] = postureTask.getDesiredAcceleration
if i%conf.PRINT_N == 0:
print "Time %.3f"%(t)
print "\ttracking err %s: %.3f"%(postureTask.name.ljust(20,'.'), norm(postureTask.position_error, 2))
# numerical integration
v_mean = v[:,i] + 0.5*dt*dv[:,i]
v[:,i+1] = v[:,i] + dt*dv[:,i]
q[:,i+1] = se3.integrate(model, q[:,i], dt*v_mean)
t += conf.dt
if i%conf.DISPLAY_N == 0:
robot_display.display(q[:,i])
time_spent = time.time() - time_start
if(time_spent < conf.dt): time.sleep(conf.dt-time_spent)
# PLOT STUFF
time = np.arange(0.0, N*conf.dt, conf.dt)
if(PLOT_JOINT_POS):
(f, ax) = plut.create_empty_figure(robot.nv/2,2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, q[i,:-1].A1, label='Joint pos '+str(i))
ax[i].plot(time, q_ref[i,:].A1, '--', label='Joint ref pos '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Joint angles [rad]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
if(PLOT_JOINT_VEL):
(f, ax) = plut.create_empty_figure(robot.nv/2,2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, v[i,:-1].A1, label='Joint vel '+str(i))
ax[i].plot(time, v_ref[i,:].A1, '--', label='Joint ref vel '+str(i))
ax[i].plot([time[0], time[-1]], 2*[v_min[i,0]], ':')
ax[i].plot([time[0], time[-1]], 2*[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)
if(PLOT_JOINT_ACC):
(f, ax) = plut.create_empty_figure(robot.nv/2,2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, dv[i,:-1].A1, label='Joint acc '+str(i))
ax[i].plot(time, dv_ref[i,:].A1, '--', label='Joint ref acc '+str(i))
ax[i].plot(time, dv_des[i,:].A1, ':', label='Joint des acc '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Joint acceleration [rad/s^2]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
if(PLOT_TORQUES):
(f, ax) = plut.create_empty_figure(robot.nv/2,2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, tau[i,:].A1, label='Torque '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Torque [Nm]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
plt.show()
import numpy as np
import numpy.matlib as matlib
from numpy import nan
from numpy.linalg import norm as norm
import matplotlib.pyplot as plt
import plot_utils as plut
import time
from tsid_manipulator import TsidManipulator
#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