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

exercizes/ex_1_ur5.py

+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()

exercizes/tsid_manipulator.py

+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

exercizes/ur5_conf.py

+# -*- 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