Commit 9ee02ea9 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Add python bindings for method addActuationTask of...

Add python bindings for method addActuationTask of inverse-dynamics-formulation-acc-force. Change name of method addTorqueTask to addActuationTask. Extend python example with UR5 robot with actuation bounds.
parent c5d08cee
......@@ -30,6 +30,8 @@
#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"
namespace tsid
{
namespace python
......@@ -54,7 +56,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("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 +88,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 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);
}
......
......@@ -5,20 +5,27 @@ from numpy.linalg import norm as norm
import matplotlib.pyplot as plt
import plot_utils as plut
import time
import ur5_conf as conf
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
PLOT_EE_VEL = 0
PLOT_EE_ACC = 0
PLOT_TORQUES = 1
tsid = TsidManipulator(conf)
N = conf.N_SIMULATION
tau = matlib.empty((tsid.robot.na, N))*nan
ee_pos = matlib.empty((3, N))*nan
ee_vel = matlib.empty((3, N))*nan
ee_acc = matlib.empty((3, N))*nan
ee_pos_ref = matlib.empty((3, N))*nan
ee_vel_ref = matlib.empty((3, N))*nan
ee_acc_ref = matlib.empty((3, N))*nan
......@@ -27,13 +34,10 @@ ee_acc_des = matlib.empty((3, N))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_
sampleEE = tsid.trajEE.computeNext()
samplePosture = tsid.trajPosture.computeNext()
amp = np.matrix([0.02, 0.1, 0.10]).T # amplitude
phi = np.matrix([0.0, 0.5*np.pi, 0.0]).T # phase
two_pi_f = 2*np.pi*np.matrix([1.0, 0.5, 0.5]).T # frequency (time 2 PI)
offset = sampleEE.pos()
two_pi_f_amp = np.multiply(two_pi_f,amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
offset[:3,0] += conf.offset
two_pi_f_amp = np.multiply(conf.two_pi_f, conf.amp)
two_pi_f_squared_amp = np.multiply(conf.two_pi_f, two_pi_f_amp)
pEE = offset.copy()
vEE = matlib.zeros((6,1))
......@@ -48,9 +52,9 @@ q, v = tsid.q, tsid.v
for i in range(0, N):
time_start = time.time()
pEE[:3,0] = offset[:3,0] + np.multiply(amp, matlib.sin(two_pi_f*t + phi))
vEE[:3,0] = np.multiply(two_pi_f_amp, matlib.cos(two_pi_f*t + phi))
aEE[:3,0] = np.multiply(two_pi_f_squared_amp, -matlib.sin(two_pi_f*t + phi))
pEE[:3,0] = offset[:3,0] + np.multiply(conf.amp, matlib.sin(conf.two_pi_f*t + conf.phi))
vEE[:3,0] = np.multiply(two_pi_f_amp, matlib.cos(conf.two_pi_f*t + conf.phi))
aEE[:3,0] = np.multiply(two_pi_f_squared_amp, -matlib.sin(conf.two_pi_f*t + conf.phi))
sampleEE.pos(pEE)
sampleEE.vel(vEE)
sampleEE.acc(aEE)
......@@ -61,10 +65,10 @@ for i in range(0, N):
sol = tsid.solver.solve(HQPData)
if(sol.status!=0):
print "QP problem could not be solved! Error code:", sol.status
print "Time %.3f QP problem could not be solved! Error code:"%t, sol.status
break
tau = tsid.formulation.getActuatorForces(sol)
tau[:,i] = tsid.formulation.getActuatorForces(sol)
dv = tsid.formulation.getAccelerations(sol)
ee_pos[:,i] = tsid.robot.framePosition(tsid.formulation.data(), tsid.EE).translation
......@@ -93,8 +97,9 @@ for i in range(0, N):
# PLOT STUFF
time = np.arange(0.0, N*conf.dt, conf.dt)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
if(PLOT_EE_POS):
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, ee_pos[i,:].A1, label='EE '+str(i))
ax[i].plot(time, ee_pos_ref[i,:].A1, 'r:', label='EE Ref '+str(i))
ax[i].set_xlabel('Time [s]')
......@@ -102,8 +107,9 @@ for i in range(3):
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
if(PLOT_EE_VEL):
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, ee_vel[i,:].A1, label='EE Vel '+str(i))
ax[i].plot(time, ee_vel_ref[i,:].A1, 'r:', label='EE Vel Ref '+str(i))
ax[i].set_xlabel('Time [s]')
......@@ -111,8 +117,9 @@ for i in range(3):
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
if(PLOT_EE_ACC):
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, ee_acc[i,:].A1, label='EE Acc '+str(i))
ax[i].plot(time, ee_acc_ref[i,:].A1, 'r:', label='EE Acc Ref '+str(i))
ax[i].plot(time, ee_acc_des[i,:].A1, 'g--', label='EE Acc Des '+str(i))
......@@ -121,4 +128,16 @@ for i in range(3):
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
if(PLOT_TORQUES):
(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, tau[i,:].A1, label='Torque '+str(i))
ax[i].plot([time[0], time[-1]], 2*[tsid.tau_min[i,0]], ':')
ax[i].plot([time[0], time[-1]], 2*[tsid.tau_max[i,0]], ':')
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()
......@@ -22,16 +22,16 @@ class TsidManipulator:
vector.extend(item for item in conf.path)
self.robot = tsid.RobotWrapper(conf.urdf, vector, False)
robot = self.robot
self.model = robot.model()
self.model = model = robot.model()
try:
q = se3.getNeutralConfiguration(robot.model(), conf.srdf, False)
# q = robot.model().referenceConfigurations["half_sitting"]
q = se3.getNeutralConfiguration(model, conf.srdf, False)
# q = model.referenceConfigurations["half_sitting"]
except:
q = conf.q0
# q = np.matrix(np.zeros(robot.nv)).T
v = np.matrix(np.zeros(robot.nv)).T
assert self.model.existFrame(conf.ee_frame_name)
assert model.existFrame(conf.ee_frame_name)
formulation = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
......@@ -46,11 +46,18 @@ class TsidManipulator:
self.eeTask.setKd(2.0 * np.sqrt(self.conf.kp_ee) * np.matrix(np.ones(6)).T)
self.eeTask.setMask(conf.ee_task_mask)
self.eeTask.useLocalFrame(False)
self.EE = robot.model().getFrameId(conf.ee_frame_name)
self.EE = model.getFrameId(conf.ee_frame_name)
H_ee_ref = self.robot.framePosition(formulation.data(), self.EE)
self.trajEE = tsid.TrajectorySE3Constant("traj-ee", H_ee_ref)
formulation.addMotionTask(self.eeTask, conf.w_ee, 1, 0.0)
self.tau_max = conf.tau_max_scaling*model.effortLimit
self.tau_min = -self.tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
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)
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q)
postureTask.setReference(trajPosture.computeNext())
......@@ -59,6 +66,7 @@ class TsidManipulator:
self.trajPosture = trajPosture
self.postureTask = postureTask
self.actuationBoundsTask = actuationBoundsTask
self.formulation = formulation
self.solver = solver
self.q = q
......
......@@ -11,19 +11,28 @@ import os
np.set_printoptions(precision=3, linewidth=200, suppress=True)
LINE_WIDTH = 60
N_SIMULATION = 4000 # number of time steps simulated
N_SIMULATION = 10000 # 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
ee_frame_name = "ee_fixed_joint" # end-effector frame name
ee_task_mask = np.matrix([1., 1, 1, 0, 0, 0]).T
# REFERENCE SINUSOIDAL TRAJECTORY
amp = np.matrix([0*0.02, 0.1, 0.10]).T # amplitude
phi = np.matrix([0.0, 0.5*np.pi, 0.0]).T # phase
two_pi_f = 1.4*2*np.pi*np.matrix([1.0, 0.5, 0.5]).T # frequency (time 2 PI)
offset = np.matrix([0.0, 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 = 1.0 # weight of the torque bounds
kp_ee = 2.0 # proportional gain of end-effector constraint
kp_ee = 5.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
ee_frame_name = "ee_fixed_joint" # end-effector frame name
ee_task_mask = np.matrix([1., 1, 1, 0, 0, 0]).T
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps
CAMERA_TRANSFORM = [2.582354784011841, 1.620774507522583, 1.0674564838409424, 0.2770655155181885, 0.5401807427406311, 0.6969326734542847, 0.3817386031150818]
......
# -*- coding: utf-8 -*-
"""
Created on Thu Apr 18 09:47:07 2019
@author: student
"""
import numpy as np
import os
np.set_printoptions(precision=3, linewidth=200, suppress=True)
LINE_WIDTH = 60
N_SIMULATION = 3000 # 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
# REFERENCE SINUSOIDAL TRAJECTORY
amp = np.matrix([0.0, 0.0, 0.0]).T # amplitude
phi = np.matrix([0.0, 0.5*np.pi, 0.0]).T # phase
two_pi_f = 2*np.pi*np.matrix([1.0, 0.5, 0.5]).T # frequency (time 2 PI)
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
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
ee_frame_name = "ee_fixed_joint" # end-effector frame name
ee_task_mask = np.matrix([1., 1, 1, 0, 0, 0]).T
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps
CAMERA_TRANSFORM = [2.582354784011841, 1.620774507522583, 1.0674564838409424, 0.2770655155181885, 0.5401807427406311, 0.6969326734542847, 0.3817386031150818]
SPHERE_RADIUS = 0.03
REF_SPHERE_RADIUS = 0.03
EE_SPHERE_COLOR = (1, 0.5, 0, 0.5)
EE_REF_SPHERE_COLOR = (1, 0, 0, 0.5)
ERROR_MSG = 'You should set the environment variable UR5_MODEL_DIR to something like "$DEVEL_DIR/install/share"\n';
path = os.environ.get('UR5_MODEL_DIR', ERROR_MSG)
urdf = path + "/ur_description/urdf/ur5_robot.urdf";
srdf = path + '/ur_description/srdf/ur5_robot.srdf'
\ No newline at end of file
......@@ -103,7 +103,7 @@ namespace tsid
unsigned int priorityLevel,
double transition_duration=0.0);
bool addTorqueTask(TaskActuation & task,
bool addActuationTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double transition_duration=0.0);
......
......@@ -74,7 +74,7 @@ namespace tsid
unsigned int priorityLevel,
double transition_duration=0.0) = 0;
virtual bool addTorqueTask(TaskActuation & task,
virtual bool addActuationTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double transition_duration=0.0) = 0;
......
......@@ -166,7 +166,7 @@ bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task,
}
bool InverseDynamicsFormulationAccForce::addTorqueTask(TaskActuation & task,
bool InverseDynamicsFormulationAccForce::addActuationTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double transition_duration)
......
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