def main():
N = int(DURATION / DT) + 1
model = models.ThreeInputModel(output_idx=[0, 1])
ts = DT * np.arange(N)
q0 = np.array([0, np.pi / 4.0, -np.pi / 4.0])
p0 = model.forward(q0)
sim = PymunkSimulationTorque(DT, iterations=10)
sim.add_robot(model, q0)
box_body = pymunk.Body()
box_body.position = (p0[0], p0[1] + 0.1)
box_corners = [(-0.2, 0.05), (-0.2, -0.05), (0.2, -0.05), (0.2, 0.05)]
box = pymunk.Poly(box_body, box_corners, radius=0.01)
box.mass = 0.5
box.friction = 0.75
# sim.space.add(box.body, box)
n_balls = 20
ball_r = 0.1
ball_x = np.random.random(n_balls) * 8 - 2
ball_y = np.random.random(n_balls) * 3 + 2
for i in range(n_balls):
body = pymunk.Body()
body.position = (ball_x[i], ball_y[i])
ball = pymunk.Circle(body, ball_r, (0, 0))
ball.mass = 0.1
ball.color = (255, 0, 0, 255)
ball.friction = 0.5
sim.space.add(body, ball)
W = 0.01 * np.eye(model.ni)
K = np.eye(model.no)
controller = control.DiffIKController(model, W, K, DT, model.vel_lim,
model.acc_lim)
# timescaling = trajectories.QuinticTimeScaling(DURATION)
# trajectory = trajectories.Sine(p0, 2, 0.5, 1, timescaling, DURATION)
trajectory = trajectories.Point(p0)
ps = np.zeros((N, model.no))
pds = np.zeros((N, model.no))
robot_renderer = plotter.ThreeInputRenderer(model, q0)
box_renderer = plotter.PolygonRenderer(np.array(box.body.position),
box.body.angle,
np.array(box_corners))
# trajectory_renderer = plotter.TrajectoryRenderer(trajectory, ts)
plot = plotter.RealtimePlotter([robot_renderer, box_renderer])
plot.start(grid=True)
plt.ion()
fig = plt.figure()
ax = plt.gca()
# video = plotter.Video(name='balls.mp4', fps=1./(PLOT_PERIOD * DT))
# video.setup(fig)
ax.set_xlabel('x (m)')
ax.set_ylabel('y (m)')
ax.set_xlim([-1, 6])
ax.set_ylim([-1, 2])
ax.set_aspect('equal')
options = pymunk.matplotlib_util.DrawOptions(ax)
options.flags = pymunk.SpaceDebugDrawOptions.DRAW_SHAPES
sim.space.debug_draw(options)
fig.canvas.draw()
fig.canvas.flush_events()
# video.writer.grab_frame()
q = q0
dq = np.zeros(3)
u = np.zeros(3)
pd, vd, ad = trajectory.sample(0, flatten=True)
ps[0, :] = p0
pds[0, :] = pd[:model.no]
kp = 5
kd = 3
ki = 0.1
ddqd = np.zeros(3)
dqd = np.zeros(3)
qd = q0
e_sum = 0
for i in range(N - 1):
t = ts[i]
# controller
if i % CTRL_PERIOD == 0:
pd, vd, ad = trajectory.sample(t, flatten=True)
u = controller.solve(q, dq, pd, vd)
# sim.command_velocity(u)
# torque control law
e = qd - q
e_sum += CTRL_PERIOD * DT * e
α = ddqd + kp * e + kd * (dqd - dq) + ki * e_sum
tau = model.calc_torque(q, dq, α)
sim.command_torque(tau)
# step the sim
q, dq = sim.step()
p = model.forward(q)
ps[i + 1, :] = p
pds[i + 1, :] = pd[:model.no]
if i % PLOT_PERIOD == 0:
box_renderer.set_state(np.array(box.body.position), box.body.angle)
robot_renderer.set_state(q)
ax.cla()
ax.set_xlim([-1, 6])
ax.set_ylim([-1, 2])
sim.space.debug_draw(options)
fig.canvas.draw()
fig.canvas.flush_events()
# video.writer.grab_frame()
plot.update()
plot.done()
# video.writer.finish()
xe = pds[1:, 0] - ps[1:, 0]
ye = pds[1:, 1] - ps[1:, 1]
print('RMSE(x) = {}'.format(rms(xe)))
print('RMSE(y) = {}'.format(rms(ye)))
def main():
N = int(DURATION / DT) + 1
model = TopDownHolonomicModel(L1, L2, VEL_LIM, acc_lim=ACC_LIM, output_idx=[0, 1])
Q = np.eye(model.no)
R = np.eye(model.ni) * 0.1
controller = control.ObstacleAvoidingMPC(model, MPC_DT, Q, R, VEL_LIM, ACC_LIM)
ts = DT * np.arange(N)
qs = np.zeros((N, model.ni))
dqs = np.zeros((N, model.ni))
us = np.zeros((N, model.ni))
ps = np.zeros((N, model.no))
vs = np.zeros((N, model.no))
pds = np.zeros((N, model.no))
# initial state
q = np.array([0, 0, 0, 0.25*np.pi, -0.5*np.pi])
p = model.forward(q)
dq = np.zeros(model.ni)
# obstacle
pc = np.array([3., 0])
obs = obstacle.Circle(0.5, 10)
pg = np.array([5., 0])
qs[0, :] = q
ps[0, :] = p
pds[0, :] = p
circle_renderer = plotter.CircleRenderer(obs.r, pc)
robot_renderer = plotter.TopDownHolonomicRenderer(model, q)
plot = plotter.RealtimePlotter([robot_renderer, circle_renderer])
plot.start(limits=[-5, 10, -5, 10], grid=True)
for i in range(N - 1):
n = min(NUM_HORIZON, N - 1 - i)
pd = np.tile(pg, n)
u = controller.solve(q, dq, pd, n, pc)
# step the model
q, dq = model.step(q, u, DT, dq_last=dq)
p = model.forward(q)
v = model.jacobian(q).dot(dq)
# obstacle interaction
f, movement = obs.calc_point_force(pc, p)
pc += movement
# record
us[i, :] = u
dqs[i+1, :] = dq
qs[i+1, :] = q
ps[i+1, :] = p
pds[i+1, :] = pd[:model.no]
vs[i+1, :] = v
# render
robot_renderer.set_state(q)
circle_renderer.set_state(pc)
plot.update()
plot.done()
xe = pds[1:, 0] - ps[1:, 0]
ye = pds[1:, 1] - ps[1:, 1]
print('RMSE(x) = {}'.format(rms(xe)))
print('RMSE(y) = {}'.format(rms(ye)))
plt.figure()
plt.plot(ts, pds[:, 0], label='$x_d$', color='b', linestyle='--')
plt.plot(ts, pds[:, 1], label='$y_d$', color='r', linestyle='--')
plt.plot(ts, ps[:, 0], label='$x$', color='b')
plt.plot(ts, ps[:, 1], label='$y$', color='r')
plt.grid()
plt.legend()
plt.xlabel('Time (s)')
plt.ylabel('Position')
plt.title('End effector position')
plt.figure()
plt.plot(ts, dqs[:, 0], label='$\\dot{q}_x$')
plt.plot(ts, dqs[:, 1], label='$\\dot{q}_1$')
plt.plot(ts, dqs[:, 2], label='$\\dot{q}_2$')
plt.grid()
plt.legend()
plt.title('Actual joint velocity')
plt.xlabel('Time (s)')
plt.ylabel('Velocity')
plt.figure()
plt.plot(ts, us[:, 0], label='$u_x$')
plt.plot(ts, us[:, 1], label='$u_1$')
plt.plot(ts, us[:, 2], label='$u_2$')
plt.grid()
plt.legend()
plt.title('Commanded joint velocity')
plt.xlabel('Time (s)')
plt.ylabel('Velocity')
plt.figure()
plt.plot(ts, qs[:, 0], label='$q_x$')
plt.plot(ts, qs[:, 1], label='$q_1$')
plt.plot(ts, qs[:, 2], label='$q_2$')
plt.grid()
plt.legend()
plt.title('Joint positions')
plt.xlabel('Time (s)')
plt.ylabel('Joint positions')
plt.show()
def main():
N = int(DURATION / DT) + 1
model = models.ThreeInputModel(l1=L1,
l2=L2,
vel_lim=VEL_LIM,
acc_lim=ACC_LIM,
output_idx=[0, 1])
W = 0.1 * np.eye(model.ni)
K = np.eye(model.no)
controller = control.DiffIKController(model, W, K, DT, VEL_LIM, ACC_LIM)
ts = DT * np.arange(N)
qs = np.zeros((N, model.ni))
dqs = np.zeros((N, model.ni))
us = np.zeros((N, model.ni))
ps = np.zeros((N, model.no))
vs = np.zeros((N, model.no))
pds = np.zeros((N, model.no))
q0 = np.array([0, np.pi / 4.0, -np.pi / 4.0])
p0 = model.forward(q0)
# reference trajectory
# trajectory = Line(p0, v0=np.zeros(2), a=np.array([0.01, 0]))
# timescaling = trajectories.QuinticTimeScaling(DURATION)
timescaling = trajectories.TrapezoidalTimeScalingV(0.15, DURATION)
# timescaling = trajectories.TrapezoidalTimeScalingA(0.1, DURATION)
# trajectory = trajectories.PointToPoint(p0, p0 + [1, 0], timescaling, DURATION)
trajectory = trajectories.Sine(p0, 2, 0.5, 1, timescaling, DURATION)
# trajectory2 = PointToPoint(p0 + [1, 0], p0 + [2, 0], timescaling, 0.5*DURATION)
# trajectory = Chain([trajectory1, trajectory2])
# points = np.array([p0, p0 + [1, 1], p0 + [2, -1], p0 + [3, 0]])
# trajectory = CubicBezier(points, timescaling, DURATION)
# trajectory = trajectories.Circle(p0, 0.5, timescaling, DURATION)
# points = np.array([p0, p0 + [1, 0], p0 + [1, -1], p0 + [0, -1], p0])
# trajectory = Polygon(points, v=0.4)
# pref, vref, aref = trajectory.sample(ts)
# plt.figure()
# plt.plot(ts, pref[:, 0], label='$p$')
# plt.plot(ts, vref[:, 0], label='$v$')
# plt.plot(ts, aref[:, 0], label='$a$')
# plt.grid()
# plt.legend()
# plt.title('EE reference trajectory')
# plt.xlabel('Time (s)')
# plt.ylabel('Reference signal')
# plt.show()
q = q0
p = p0
dq = np.zeros(model.ni)
qs[0, :] = q0
ps[0, :] = p0
pds[0, :] = p0
robot_renderer = plotter.ThreeInputRenderer(model, q0)
trajectory_renderer = plotter.TrajectoryRenderer(trajectory, ts)
plot = plotter.RealtimePlotter([robot_renderer, trajectory_renderer])
plot.start(grid=True)
for i in range(N - 1):
t = ts[i]
# controller
pd, vd, ad = trajectory.sample(t, flatten=True)
u = controller.solve(q, dq, pd, vd)
# step the model
q, dq = model.step(q, u, DT, dq_last=dq)
p = model.forward(q)
v = model.jacobian(q).dot(dq)
# record
us[i, :] = u
dqs[i + 1, :] = dq
qs[i + 1, :] = q
ps[i + 1, :] = p
pds[i + 1, :] = pd[:model.no]
vs[i + 1, :] = v
# render
robot_renderer.set_state(q)
plot.update()
plot.done()
xe = pds[1:, 0] - ps[1:, 0]
ye = pds[1:, 1] - ps[1:, 1]
print('RMSE(x) = {}'.format(rms(xe)))
print('RMSE(y) = {}'.format(rms(ye)))
plt.figure()
plt.plot(ts, pds[:, 0], label='$x_d$', color='b', linestyle='--')
plt.plot(ts, pds[:, 1], label='$y_d$', color='r', linestyle='--')
plt.plot(ts, ps[:, 0], label='$x$', color='b')
plt.plot(ts, ps[:, 1], label='$y$', color='r')
plt.grid()
plt.legend()
plt.xlabel('Time (s)')
plt.ylabel('Position')
plt.title('End effector position')
plt.figure()
plt.plot(ts, vs[:, 0], label='$v_x$', color='b')
plt.plot(ts, vs[:, 1], label='$v_y$', color='r')
plt.grid()
plt.legend()
plt.xlabel('Time (s)')
plt.ylabel('Velocity (m/s)')
plt.title('End effector velocity')
plt.figure()
plt.plot(ts, dqs[:, 0], label='$\\dot{q}_x$')
plt.plot(ts, dqs[:, 1], label='$\\dot{q}_1$')
plt.plot(ts, dqs[:, 2], label='$\\dot{q}_2$')
plt.grid()
plt.legend()
plt.title('Actual joint velocity')
plt.xlabel('Time (s)')
plt.ylabel('Velocity')
plt.figure()
plt.plot(ts, us[:, 0], label='$u_x$')
plt.plot(ts, us[:, 1], label='$u_1$')
plt.plot(ts, us[:, 2], label='$u_2$')
plt.grid()
plt.legend()
plt.title('Commanded joint velocity')
plt.xlabel('Time (s)')
plt.ylabel('Velocity')
plt.figure()
plt.plot(ts, qs[:, 0], label='$q_x$')
plt.plot(ts, qs[:, 1], label='$q_1$')
plt.plot(ts, qs[:, 2], label='$q_2$')
plt.grid()
plt.legend()
plt.title('Joint positions')
plt.xlabel('Time (s)')
plt.ylabel('Joint positions')
plt.show()
def main():
N = int(DURATION / DT) + 1
model = ThreeInputModel(L1, L2, VEL_LIM, acc_lim=ACC_LIM, output_idx=[0, 1])
Q = np.eye(model.no)
R = np.eye(model.ni) * 0.01
mpc = MPC(model, DT, Q, R, VEL_LIM, ACC_LIM)
ts = DT * np.arange(N)
qs = np.zeros((N, model.ni))
dqs = np.zeros((N, model.ni))
us = np.zeros((N, model.ni))
ps = np.zeros((N, model.no))
vs = np.zeros((N, model.no))
pds = np.zeros((N, model.no))
q0 = np.array([0, np.pi/4.0, -np.pi/4.0])
p0 = model.forward(q0)
# reference trajectory
timescaling = QuinticTimeScaling(DURATION)
points = np.array([p0, p0 + [1, 1], p0 + [2, -1], p0 + [3, 0]])
trajectory = CubicBezier(points, timescaling, DURATION)
# obstacles
# obs = Wall(x=2.5)
# obs = Circle(c=np.array([3.0, 1.5]), r=1)
q = q0
p = p0
dq = np.zeros(model.ni)
qs[0, :] = q0
ps[0, :] = p0
pds[0, :] = p0
robot_renderer = ThreeInputRenderer(model, q0)
trajectory_renderer = TrajectoryRenderer(trajectory, ts)
plotter = RealtimePlotter([robot_renderer, trajectory_renderer])
plotter.start()
for i in range(N - 1):
# MPC
# The +1 ts[i+1] is because we want to generate a u[i] such that
# p[i+1] = FK(q[i+1]) = pd[i+1]
n = min(NUM_HORIZON, N - 1 - i)
pd, _, _ = trajectory.sample(ts[i+1:i+1+n], flatten=True)
u = mpc.solve(q, dq, pd, n)
q, dq = model.step(q, u, DT, dq_last=dq)
p = model.forward(q)
v = model.jacobian(q).dot(dq)
# record
us[i, :] = u
dqs[i+1, :] = dq
qs[i+1, :] = q
ps[i+1, :] = p
pds[i+1, :] = pd[:model.no]
vs[i+1, :] = v
robot_renderer.set_state(q)
plotter.update()
plotter.done()
xe = pds[1:, 0] - ps[1:, 0]
ye = pds[1:, 1] - ps[1:, 1]
print('RMSE(x) = {}'.format(rms(xe)))
print('RMSE(y) = {}'.format(rms(ye)))
plt.figure()
plt.plot(ts, pds[:, 0], label='$x_d$', color='b', linestyle='--')
plt.plot(ts, pds[:, 1], label='$y_d$', color='r', linestyle='--')
plt.plot(ts, ps[:, 0], label='$x$', color='b')
plt.plot(ts, ps[:, 1], label='$y$', color='r')
plt.grid()
plt.legend()
plt.xlabel('Time (s)')
plt.ylabel('Position')
plt.title('End effector position')
plt.figure()
plt.plot(ts, dqs[:, 0], label='$\\dot{q}_x$')
plt.plot(ts, dqs[:, 1], label='$\\dot{q}_1$')
plt.plot(ts, dqs[:, 2], label='$\\dot{q}_2$')
plt.grid()
plt.legend()
plt.title('Actual joint velocity')
plt.xlabel('Time (s)')
plt.ylabel('Velocity')
plt.figure()
plt.plot(ts, us[:, 0], label='$u_x$')
plt.plot(ts, us[:, 1], label='$u_1$')
plt.plot(ts, us[:, 2], label='$u_2$')
plt.grid()
plt.legend()
plt.title('Commanded joint velocity')
plt.xlabel('Time (s)')
plt.ylabel('Velocity')
plt.show()
def main():
N = int(DURATION / DT) + 1
model = ThreeInputModel(L1, L2, VEL_LIM, acc_lim=ACC_LIM, output_idx=[0, 1])
W = 0.1 * np.eye(model.ni)
Kp = np.eye(model.no)
Kv = 0.1 * np.eye(model.no)
controller = AccelerationController(model, W, Kp, Kv, DT, VEL_LIM, ACC_LIM)
ts = np.array([i * DT for i in range(N)])
qs = np.zeros((N, model.ni))
dqs = np.zeros((N, model.ni))
us = np.zeros((N, model.ni))
ps = np.zeros((N, model.no))
vs = np.zeros((N, model.no))
pds = np.zeros((N, model.no))
dq_cmds = np.zeros((N, model.ni))
q0 = np.array([0, np.pi/4.0, -np.pi/4.0])
p0 = model.forward(q0)
# reference trajectory
timescaling = QuinticTimeScaling(DURATION)
trajectory = PointToPoint(p0, p0 + [1, 0], timescaling, DURATION)
# pref, vref, aref = trajectory.sample(ts)
# plt.figure()
# plt.plot(ts, pref[:, 0], label='$p$')
# plt.plot(ts, vref[:, 0], label='$v$')
# plt.plot(ts, aref[:, 0], label='$a$')
# plt.grid()
# plt.legend()
# plt.title('EE reference trajectory')
# plt.xlabel('Time (s)')
# plt.ylabel('Reference signal')
# plt.show()
q = q0
p = p0
dq = np.zeros(model.ni)
qs[0, :] = q0
ps[0, :] = p0
pds[0, :] = p0
robot_renderer = ThreeInputRenderer(model, q0)
trajectory_renderer = TrajectoryRenderer(trajectory, ts)
plotter = RealtimePlotter([robot_renderer, trajectory_renderer])
plotter.start()
for i in range(N - 1):
t = ts[i]
# controller
pd, vd, ad = trajectory.sample(t, flatten=True)
u = controller.solve(q, dq, pd, vd, ad)
dq_cmd = dq + DT * u
# step the model
q, dq = model.step(q, dq_cmd, DT, dq_last=dq)
p = model.forward(q)
v = model.jacobian(q).dot(dq)
# record
us[i, :] = u
dq_cmds[i, :] = dq_cmd
dqs[i+1, :] = dq
qs[i+1, :] = q
ps[i+1, :] = p
pds[i+1, :] = pd[:model.no]
vs[i+1, :] = v
robot_renderer.set_state(q)
plotter.update()
plotter.done()
xe = pds[1:, 0] - ps[1:, 0]
ye = pds[1:, 1] - ps[1:, 1]
print('RMSE(x) = {}'.format(rms(xe)))
print('RMSE(y) = {}'.format(rms(ye)))
plt.figure()
plt.plot(ts, pds[:, 0], label='$x_d$', color='b', linestyle='--')
plt.plot(ts, pds[:, 1], label='$y_d$', color='r', linestyle='--')
plt.plot(ts, ps[:, 0], label='$x$', color='b')
plt.plot(ts, ps[:, 1], label='$y$', color='r')
plt.grid()
plt.legend()
plt.xlabel('Time (s)')
plt.ylabel('Position')
plt.title('End effector position')
plt.figure()
plt.plot(ts, dqs[:, 0], label='$\\dot{q}_x$')
plt.plot(ts, dqs[:, 1], label='$\\dot{q}_1$')
plt.plot(ts, dqs[:, 2], label='$\\dot{q}_2$')
plt.grid()
plt.legend()
plt.title('Joint velocity')
plt.xlabel('Time (s)')
plt.ylabel('Velocity')
plt.figure()
plt.plot(ts, dq_cmds[:, 0], label='$\\dot{q}_x$')
plt.plot(ts, dq_cmds[:, 1], label='$\\dot{q}_1$')
plt.plot(ts, dq_cmds[:, 2], label='$\\dot{q}_2$')
plt.grid()
plt.legend()
plt.title('Commanded Joint Velocity')
plt.xlabel('Time (s)')
plt.ylabel('Velocity')
plt.figure()
plt.plot(ts, us[:, 0], label='$u_x$')
plt.plot(ts, us[:, 1], label='$u_1$')
plt.plot(ts, us[:, 2], label='$u_2$')
plt.grid()
plt.legend()
plt.title('Commanded joint acceleration')
plt.xlabel('Time (s)')
plt.ylabel('Acceleration')
plt.figure()
plt.plot(ts, qs[:, 0], label='$q_x$')
plt.plot(ts, qs[:, 1], label='$q_1$')
plt.plot(ts, qs[:, 2], label='$q_2$')
plt.grid()
plt.legend()
plt.title('Joint positions')
plt.xlabel('Time (s)')
plt.ylabel('Joint positions')
plt.show()
def main():
N = int(DURATION / DT) + 1
model = models.TopDownHolonomicModel(L1,
L2,
VEL_LIM,
acc_lim=ACC_LIM,
output_idx=[0, 1])
W = 0.1 * np.eye(model.ni)
K = np.eye(model.no)
controller = control.ConstrainedDiffIKController(model, W, K, DT, VEL_LIM,
ACC_LIM)
ts = DT * np.arange(N)
qs = np.zeros((N, model.ni))
dqs = np.zeros((N, model.ni))
us = np.zeros((N, model.ni))
ps = np.zeros((N, model.no))
vs = np.zeros((N, model.no))
pds = np.zeros((N, model.no))
# initial state
q = np.array([0, 0, 0, 0.25 * np.pi, -0.5 * np.pi])
p = model.forward(q)
dq = np.zeros(model.ni)
f = np.zeros(2)
# obstacle
pc = np.array([-2., 1.])
obs = obstacle.Circle(0.5, 1000)
# goal position
pg = np.array([5., 0])
qs[0, :] = q
ps[0, :] = p
pds[0, :] = p
circle_renderer = plotter.CircleRenderer(obs.r, pc)
robot_renderer = plotter.TopDownHolonomicRenderer(model, q)
plot = plotter.RealtimePlotter([robot_renderer, circle_renderer])
plot.start(limits=[-5, 10, -5, 10], grid=True)
for i in range(N - 1):
# experimental controller for aligning and pushing object to a goal
# point - generates a desired set point pd; the admittance portion
# doesn't really seem helpful at this point (since we actually *want*
# to hit/interact with the environment)
b = 0.25
cos_alpha = np.cos(np.pi * 0.25)
p1 = pc - 0.25 * unit(pg - pc)
d = np.linalg.norm(p - pc)
J = model.jacobian(q)
# distance from obstacle constraints
A = DT * (p - pc).T.dot(J) / d
A = A.reshape((1, model.ni))
lbA = np.array([obs.r + b - d])
# TODO velocity damper constraints - these are not working correctly
# d = d - obs.r
# xi = 1
# ds = 0.1
# di = 1
# no = unit(pc - p)
# A = no.dot(J)
# A = A.reshape((model.ni, 1))
# lbA = np.array([-xi * (d - ds) / (di - ds)])
#
# if d >= di:
# print(d)
# lbA = np.zeros_like(lbA)
# A = np.zeros_like(A)
# only enforced away from the push location
# TODO other option is to revisit the cardioid stuff, which has the
# advantage of not being discontinous in distance
cos_angle = unit(p - pc).dot(unit(p1 - pc))
if cos_angle >= cos_alpha:
lbA = None
A = None
vd = np.zeros(2)
pd = p1
u = controller.solve(q, dq, pd, vd, A=A, lbA=lbA, ubA=None)
# step the model
q, dq = model.step(q, u, DT, dq_last=dq)
p = model.forward(q)
v = model.jacobian(q).dot(dq)
# obstacle interaction
f = obs.calc_point_force(pc, p)
f, movement = obs.apply_force(f)
pc += movement
# if object is close enough to the goal position, stop
if np.linalg.norm(pg - pc) < 0.1:
print('done')
break
# record
us[i, :] = u
dqs[i + 1, :] = dq
qs[i + 1, :] = q
ps[i + 1, :] = p
pds[i + 1, :] = pd[:model.no]
vs[i + 1, :] = v
# render
robot_renderer.set_state(q)
circle_renderer.set_state(pc)
plot.update()
plot.done()
xe = pds[1:, 0] - ps[1:, 0]
ye = pds[1:, 1] - ps[1:, 1]
print('RMSE(x) = {}'.format(rms(xe)))
print('RMSE(y) = {}'.format(rms(ye)))
plt.figure()
plt.plot(ts, pds[:, 0], label='$x_d$', color='b', linestyle='--')
plt.plot(ts, pds[:, 1], label='$y_d$', color='r', linestyle='--')
plt.plot(ts, ps[:, 0], label='$x$', color='b')
plt.plot(ts, ps[:, 1], label='$y$', color='r')
plt.grid()
plt.legend()
plt.xlabel('Time (s)')
plt.ylabel('Position')
plt.title('End effector position')
plt.figure()
plt.plot(ts, dqs[:, 0], label='$\\dot{q}_x$')
plt.plot(ts, dqs[:, 1], label='$\\dot{q}_1$')
plt.plot(ts, dqs[:, 2], label='$\\dot{q}_2$')
plt.grid()
plt.legend()
plt.title('Actual joint velocity')
plt.xlabel('Time (s)')
plt.ylabel('Velocity')
plt.figure()
plt.plot(ts, us[:, 0], label='$u_x$')
plt.plot(ts, us[:, 1], label='$u_1$')
plt.plot(ts, us[:, 2], label='$u_2$')
plt.grid()
plt.legend()
plt.title('Commanded joint velocity')
plt.xlabel('Time (s)')
plt.ylabel('Velocity')
plt.figure()
plt.plot(ts, qs[:, 0], label='$q_x$')
plt.plot(ts, qs[:, 1], label='$q_1$')
plt.plot(ts, qs[:, 2], label='$q_2$')
plt.grid()
plt.legend()
plt.title('Joint positions')
plt.xlabel('Time (s)')
plt.ylabel('Joint positions')
plt.show()