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)