Commit 6d705702 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Add python script showing how to use TSID with a robot manipulator.

parent b7ece956
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 ur5_conf as conf
from tsid_manipulator import TsidManipulator
print "".center(conf.LINE_WIDTH,'#')
print " Task Space Inverse Dynamics - Manipulator ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH,'#'), '\n'
tsid = TsidManipulator(conf)
N = conf.N_SIMULATION
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
ee_acc_des = matlib.empty((3, N))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
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)
pEE = offset.copy()
vEE = matlib.zeros((6,1))
aEE = matlib.zeros((6,1))
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
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))
sampleEE.pos(pEE)
sampleEE.vel(vEE)
sampleEE.acc(aEE)
tsid.eeTask.setReference(sampleEE)
HQPData = tsid.formulation.computeProblemData(t, q, v)
# if i == 0: HQPData.print_all()
sol = tsid.solver.solve(HQPData)
if(sol.status!=0):
print "QP problem could not be solved! Error code:", sol.status
break
tau = tsid.formulation.getActuatorForces(sol)
dv = tsid.formulation.getAccelerations(sol)
ee_pos[:,i] = tsid.robot.framePosition(tsid.formulation.data(), tsid.EE).translation
ee_vel[:,i] = tsid.robot.frameVelocityWorldOriented(tsid.formulation.data(), tsid.EE).linear
ee_acc[:,i] = tsid.eeTask.getAcceleration(dv)[:3,0]
ee_pos_ref[:,i] = sampleEE.pos()[:3,0]
ee_vel_ref[:,i] = sampleEE.vel()[:3,0]
ee_acc_ref[:,i] = sampleEE.acc()[:3,0]
ee_acc_des[:,i] = tsid.eeTask.getDesiredAcceleration[:3,0]
if i%conf.PRINT_N == 0:
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)
t += conf.dt
if i%conf.DISPLAY_N == 0:
tsid.robot_display.display(q)
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.])
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)
(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]')
ax[i].set_ylabel('EE [m]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(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]')
ax[i].set_ylabel('EE Vel [m/s]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(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))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('EE Acc [m/s^2]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
plt.show()
import pinocchio as se3
import tsid
import numpy as np
import numpy.matlib as matlib
import os
import gepetto.corbaserver
import time
import commands
class TsidManipulator:
''' Standard TSID formulation for a robot manipulator
- end-effector task
- Postural task
- torque limits
- pos/vel limits
'''
def __init__(self, conf, viewer=True):
self.conf = conf
vector = se3.StdVec_StdString()
vector.extend(item for item in conf.path)
self.robot = tsid.RobotWrapper(conf.urdf, vector, False)
robot = self.robot
self.model = robot.model()
try:
q = se3.getNeutralConfiguration(robot.model(), conf.srdf, False)
# q = robot.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)
formulation = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
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)
self.eeTask = tsid.TaskSE3Equality("task-ee", self.robot, self.conf.ee_frame_name)
self.eeTask.setKp(self.conf.kp_ee * np.matrix(np.ones(6)).T)
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)
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)
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q)
postureTask.setReference(trajPosture.computeNext())
solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
self.trajPosture = trajPosture
self.postureTask = postureTask
self.formulation = formulation
self.solver = solver
self.q = q
self.v = v
# for gepetto viewer
if(viewer):
self.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()
self.robot_display.initDisplay(loadModel=True)
self.robot_display.displayCollisions(False)
self.robot_display.displayVisuals(True)
self.robot_display.display(q)
self.gui = self.robot_display.viewer.gui
self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM)
def integrate_dv(self, q, v, dv, dt):
v_mean = v + 0.5*dt*dv
v += dt*dv
q = se3.integrate(self.model, q, dt*v_mean)
return q,v
# -*- 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 = 4000 # 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
w_ee = 1.0 # weight of end-effector task
w_posture = 1e-3 # weight of joint posture task
kp_ee = 2.0 # proportional gain of end-effector constraint
kp_posture = 1.0 # proportional gain of joint posture task
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
Supports Markdown
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