def jointCtlComp(ctls=['P'], isSetPoint=False, pauseTime=False): dt = 0.002 robot = DoubleLink() robot.friction = np.array([2.5, 2.5]) t_end = 3.0 time = np.arange(0, t_end, dt) nSteps = len(time) numContrlComp = len(ctls) target = {'cartCtl': False} if isSetPoint: target['q'] = np.tile([-pi / 2, 0], (nSteps, 1)) target['qd'] = np.tile([0, 0], (nSteps, 1)) target['qdd'] = np.tile([0, 0], (nSteps, 1)) else: f1 = 2 f2 = 0.5 target['q'] = np.array( [np.sin(2 * pi * f1 * time) - pi, np.sin(2 * pi * f2 * time)]).T target['qd'] = np.array([ 2 * pi * f1 * np.cos(2 * pi * f1 * time), 2 * pi * f2 * np.cos(2 * pi * f2 * time) ]).T target['qdd'] = np.array([ -(2 * pi * f1)**2 * np.sin(2 * pi * f1 * time), -(2 * pi * f2)**2 * np.sin(2 * pi * f2 * time) ]).T states = simSys(robot, dt, nSteps, ctls, target, pauseTime) traj_plot(states, numContrlComp, ctls, target['q'], target['qd'], time, 0) traj_plot(states, numContrlComp, ctls, target['q'], target['qd'], time, 1) plt.pause(0.001)
def taskSpace_plot(states, robot: DoubleLink, ctls): fig = plt.figure() ax1 = fig.add_subplot(111, autoscale_on=False, xlim=(-2.5, 2.5), ylim=(-2.5, 2.5)) ax1.plot(2 * np.array([-1.1, 1.1]), np.array([0, 0]), 'b--') line1, = ax1.plot([], [], 'o-', lw=2, color='k', markerfacecolor='w', markersize=12) ax1.set_xlabel('x-axis in m', fontsize=15) ax1.set_ylabel('y-axis in m', fontsize=15) ax1.set_title('Initial Position') robot.visualize(states[0, :], line1) ax1.scatter([-.35], [1.5], marker='x', c='red', zorder=10, label='Setpoint') ax1.legend() plt.title('Initial Position for Task Space Control') plt.gca().set_aspect('equal', adjustable='box') plt.tight_layout() plt.show() # Uncomment to save # fig.savefig(fname="SavedPlots/" + "TaskCtlInitial_" + ".pdf", format='pdf') fig = plt.figure() ax2 = fig.add_subplot(111, autoscale_on=False, xlim=(-2.5, 2.5), ylim=(-2.5, 2.5)) ax2.plot(2 * np.array([-1.1, 1.1]), np.array([0, 0]), 'b--') line2, = ax2.plot([], [], 'o-', lw=2, color='k', markerfacecolor='w', markersize=12) ax2.set_xlabel('x-axis in m', fontsize=15) ax2.set_ylabel('y-axis in m', fontsize=15) ax2.set_title('Final Position after 3s') robot.visualize(states[-1, :], line2) ax2.scatter([-.35], [1.5], marker='x', c='red', zorder=10, label='Setpoint') ax2.legend() plt.title(f'Final Position for {str(ctls[0])} Control') plt.gca().set_aspect('equal', adjustable='box') plt.tight_layout() plt.show()
def taskCtlComp(ctls=['JacDPseudo'], pauseTime=False, resting_pos=None): dt = 0.002 robot = DoubleLink() robot.friction = np.array([2.5, 2.5]) t_end = 3.0 time = np.arange(0, t_end, dt) nSteps = len(time) numContrlComp = len(ctls) target = {} target['x'] = np.tile([-0.35, 1.5], (nSteps, 1)) target['cartCtl'] = True states = simSys(robot, dt, nSteps, ctls, target, pauseTime, resting_pos)
def jointCtlComp(ctls, isSetPoint, pauseTime, filename): if 'ctls' not in locals() or not ctls: ctls = ['P'] if 'isSetPoint' not in locals() or not isSetPoint: isSetPoint = False if 'pauseTime' not in locals() or not pauseTime: pauseTime = False dt = 0.002 robot = DoubleLink() robot.friction = np.array([2.5, 2.5]) t_end = 3.0 time = np.arange(0, t_end, dt) nSteps = len(time) numContrlComp = len(ctls) target = {'cartCtl': False} if isSetPoint: target['q'] = np.tile([-pi / 2, 0], (nSteps, 1)) target['qd'] = np.tile([0, 0], (nSteps, 1)) target['qdd'] = np.tile([0, 0], (nSteps, 1)) else: f1 = 2 f2 = 0.5 target['q'] = np.array( [np.sin(2 * pi * f1 * time) - pi, np.sin(2 * pi * f2 * time)]).transpose() target['qd'] = np.array([ 2 * pi * f1 * np.cos(2 * pi * f1 * time), 2 * pi * f2 * np.cos(2 * pi * f2 * time) ]).transpose() target['qdd'] = np.array([ -(2 * pi * f1)**2 * np.sin(2 * pi * f1 * time), -(2 * pi * f2)**2 * np.sin(2 * pi * f2 * time) ]).transpose() states = simSys(robot, dt, nSteps, ctls, target, pauseTime) colors = [1, 2, 3] traj_plot(states, colors, numContrlComp, ctls, target['q'], target['qd'], time, 0) traj_plot(states, colors, numContrlComp, ctls, target['q'], target['qd'], time, 1) plt.pause(5)
def taskCtlComp(ctls, pauseTime, filename): if 'ctls' not in locals() or not ctls: ctls = ['JacDPseudo'] if 'pauseTime' not in locals() or not pauseTime: pauseTime = False dt = 0.002 robot = DoubleLink() robot.friction = np.array([2.5, 2.5]) t_end = 3.0 time = np.arange(0, t_end, dt) nSteps = len(time) numContrlComp = len(ctls) target = {} target['x'] = np.tile([-0.35, 1.5], (nSteps, 1)) target['cartCtl'] = True states = simSys(robot, dt, nSteps, ctls, target, pauseTime)
def dmpComparison(goals, taus, filename): dt = 0.002 robot = DoubleLink() robot.friction = np.array([2.5, 2.5]) t_end = 3.0 sim_time = np.arange(dt, t_end - dt, dt) nSteps = len(sim_time) data = getImitationData(dt, sim_time) q = data[0] qd = data[1] qdd = data[2] dmpParams = dmpTrain(q, qd, qdd, dt, len(sim_time)) states = np.zeros((nSteps, 4)) states[0, ::2] = [-pi, 0] states = simSys(states, dmpParams, dt, nSteps) f1 = plt.figure() plt.plot(sim_time, q[0, :], linewidth=2.0, label='Desired $q_1$') plt.plot(sim_time, states[:, 0], ':', color='r', linewidth=4.0, label='DMP $q_1$') f2 = plt.figure() plt.plot(sim_time, q[1, :], linewidth=2.0, label='Desired $q_2$') plt.plot(sim_time, states[:, 2], ':', color='r', linewidth=4.0, label='DMP $q_2$') dmpParamsOld = dmpParams p1_h = [0, 0] p2_h = [0, 0] if goals != []: for i in range(len(goals)): states = np.zeros((nSteps, 4)) states[0, ::2] = [-pi, 0] dmpParams.goal = goals[i] states = simSys(states, dmpParams, dt, nSteps) plt.figure(f1.number) plt.plot(sim_time, states[:, 0], linewidth=2.0, label='DMP $q_1$ with goal = [' + str(goals[i][0]) + ']') plt.plot(sim_time[-1], goals[i][0], 'kx', markersize=15.0) plt.figure(f2.number) plt.plot(sim_time, states[:, 2], linewidth=2.0, label='DMP $q_2$ with goal = [' + str(goals[i][1]) + ']') plt.plot(sim_time[-1], goals[i][1], 'kx', markersize=15.0) dmpParams = dmpParamsOld if taus != []: for i in range(len(taus)): states = np.zeros((nSteps, 4)) states[0, ::2] = [-pi, 0] dmpParams.tau = taus[i] states = simSys(states, dmpParams, dt, nSteps) plt.figure(f1.number) plt.plot(sim_time, states[:, 0], linewidth=2.0, label=r'DMP $q_1$ with $\tau$ = [' + str(taus[i]) + ']') plt.figure(f2.number) plt.plot(sim_time, states[:, 2], linewidth=2.0, label=r'DMP $q_2$ with $\tau$ = [' + str(taus[i]) + ']') plt.figure(f1.number) plt.legend(loc=0) plt.grid() plt.xlim((0, 3)) plt.savefig(filename + "_1.pdf") plt.figure(f2.number) plt.legend(loc=0) plt.grid() plt.xlim((0, 3)) plt.savefig(filename + "_2.pdf") plt.draw_all() plt.pause(0.001)
def dmpComparison(goals, taus, filename): dt = 0.002 robot = DoubleLink() robot.friction = np.array([2.5, 2.5]) t_end = 3.0 sim_time = np.arange(dt, t_end - dt, dt) nSteps = len(sim_time) # Imitation Data data = getImitationData(dt, sim_time) q = data[0] qd = data[1] qdd = data[2] # To train the robot with imitation data and get the parameters # fw(z) = phi(z)T * w; dmpParams = w dmpParams = dmpTrain(q, qd, qdd, dt, len(sim_time)) states = np.zeros((nSteps, 4)) states[0, ::2] = [-pi, 0] # plot 1, joint 0 h1 = plt.figure() plt.hold('on') p1_1, = plt.plot(sim_time, q[0, :], linewidth=2.0, label='Desired $q_1$') h2 = plt.figure() plt.hold('on') # joint 1 p2_1, = plt.plot(sim_time, q[1, :], linewidth=2.0, label='Desired $q_2$') states = simSys(states, dmpParams, dt, nSteps) plt.figure(h1.number) p1_2, = plt.plot(sim_time, states[:, 0], ':', color='r', linewidth=4.0, label='DMP $q_1$') plt.legend(loc=0) plt.figure(h2.number) p2_2, = plt.plot(sim_time, states[:, 2], ':', color='r', linewidth=4.0, label='DMP $q_2$') plt.legend(loc=0) dmpParamsOld = dmpParams p1_h = [0, 0] p2_h = [0, 0] if goals != []: for i in range(len(goals)): states = np.zeros((nSteps, 4)) states[0, ::2] = [-pi, 0] dmpParams.goal = goals[i] states = simSys(states, dmpParams, dt, nSteps) plt.figure(h1.number) p1_h[i], = plt.plot(sim_time, states[:, 0], linewidth=2.0, label='DMP $q_1$ with goal = [' + str(goals[i][0]) + ']') plt.plot(sim_time[-1], goals[i][0], 'kx', markersize=15.0) plt.figure(h2.number) p2_h[i], = plt.plot(sim_time, states[:, 2], linewidth=2.0, label='DMP $q_2$ with goal = [' + str(goals[i][1]) + ']') plt.plot(sim_time[-1], goals[i][1], 'kx', markersize=15.0) plt.figure(h1.number) # plt.legend([p1_1, p1_2, p1_h[0], p1_h[1]], loc= 0) plt.legend(loc=0) plt.figure(h2.number) # plt.legend([p2_1, p2_2, p2_h[0], p2_h[1]], loc= 0) plt.legend(loc=0) dmpParams = dmpParamsOld if taus != []: for i in range(len(taus)): states = np.zeros((nSteps, 4)) states[0, ::2] = [-pi, 0] dmpParams.tau = taus[i] states = simSys(states, dmpParams, dt, nSteps) plt.figure(h1.number) p1_h[i], = plt.plot(sim_time, states[:, 0], linewidth=2.0, label='DMP $q_1$ with $\tau$ = [' + str(taus[i]) + ']') plt.figure(h2.number) p2_h[i], = plt.plot(sim_time, states[:, 2], linewidth=2.0, label='DMP $q_2$ with $\tau$ = [' + str(taus[i]) + ']') plt.figure(h1.number) # plt.legend(handles=[p1_1, p1_2, p1_h[0], p1_h[1]], loc= 0) plt.legend(loc=0) plt.figure(h2.number) # plt.legend(handles=[p2_1, p2_2, p2_h[0], p2_h[1]], loc= 0) plt.legend(loc=0) plt.pause(40)