def main():
np.set_printoptions(precision=3, suppress=True)
model = ThreeInputModel(L1, L2, LB, UB, output_idx=[0, 1])
v = np.array([0.5, 0.5])
q = np.array([0, 0.25 * np.pi, -0.5 * np.pi])
J = model.jacobian(q)
u_free = pseudoinverse(J) @ v
C_free = cost(J, v)
# best_a = 0
# _, best_C = decouple(J, v, rot(best_a))
#
# for _ in range(100):
# a = (np.random.rand() * 2 - 1) * np.pi
# P = rot(a)
# u_con, C_con = decouple(J, v, P)
#
# if C_con < best_C:
# best_a = a
# best_C = C_con
# found by the above, this solution appears to give nearly the same results
# as the unconstrained case
a = 0.395297484101207
P = rot(a)
u_con, C_con = decouple(J, v, P, inspect=True)
IPython.embed()
def main():
model = ThreeInputModel(L1, L2, LB, UB, output_idx=[0, 1])
v = np.array([0.5, 0.5])
q = np.array([0, 0.25 * np.pi, -0.5 * np.pi])
J = model.jacobian(q)
Jps = pseudoinverse(J)
IPython.embed()
def main():
model = ThreeInputModel(L1, L2, LB, UB, output_idx=[0, 1])
vd = np.array([0, 0])
pd = np.array([1, 1])
K = np.eye(2)
dt = 0.1
q0 = np.zeros(3)
q = clik(model, K, dt, vd, pd, q0)
print(q)
print(model.forward(q))
def main():
np.set_printoptions(precision=3, suppress=True)
model = ThreeInputModel(L1, L2, LB, UB, output_idx=[0, 1])
v = np.array([0.5, 0.5])
q = np.array([0, 0.25 * np.pi, -0.5 * np.pi])
J = model.jacobian(q)
# SVD: J = U @ S @ V
U, S, V = svd(J)
v2 = U.T @ v
Jp = right_pseudoinverse(J)
Sp = right_pseudoinverse(S)
u2 = Sp @ v2
u = V @ u2
IPython.embed()
def main():
model = ThreeInputModel(L1, L2, LB, UB, output_idx=[0, 1])
v = np.array([0.5, 0.5])
q = np.array([0, 0.25 * np.pi, -0.5 * np.pi])
u, u123 = block_triangular_jacobian(model, q, v)
# v = np.array([0.5, 0.5])
# q = np.array([0, 0.7*np.pi, np.pi/2])
# C = cost(model, q, v)
IPython.embed()
def main():
model = ThreeInputModel(L1, L2, LB, UB, output_idx=[0, 1])
def manipulability(qa):
# Only for the arm
q = np.array([0, qa[0], qa[1]])
J = model.jacobian(q)
Ja = J[:, 1:]
m2 = np.linalg.det(Ja @ Ja.T)
return -m2
def worst_case_v(J):
W = np.linalg.inv(J.dot(J.T))
max_val = 0
opt_v = np.zeros(2)
for i in range(1000):
v = np.random.rand(2)
v = v / np.linalg.norm(v)
C = v.T @ W @ v
if C > max_val:
max_val = C
opt_v = v
return opt_v, max_val
# qa0 = np.zeros(2)
# res = opt.minimize(manipulability, qa0, method='Nelder-Mead')
# optimal solution is q2 = +-pi/2
q = np.array([0, 0, np.pi/2])
J = model.jacobian(q)
opt_v, max_val = worst_case_v(J)
IPython.embed()
def main():
N = int(DURATION / DT) + 1
model = ThreeInputModel(L1, L2, VEL_LIM, acc_lim=ACC_LIM, output_idx=[0, 1])
pendulum = InvertedPendulum(L_PEND, M_PEND, G)
W = 0.1 * np.eye(model.ni)
# we don't want position feedback on x, only y
K = np.array([[0, 0], [0, 1]])
controller = BaselineController2(model, W, K, DT, VEL_LIM, acc_lim=ACC_LIM)
Q = np.eye(4)
R = 0.01*np.eye(1)
# LQR controller for the pendulum
A = pendulum.A
B = pendulum.B.reshape((4, 1))
Q = np.eye(4)
R = 0.01*np.eye(1)
K, _, _ = control.lqr(A, B, Q, R)
K = K.flatten()
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))
fs = np.zeros((N, 2))
q0 = np.array([0, np.pi/4.0, -np.pi/4.0])
p0 = model.forward(q0)
q = q0
p = p0
dq = np.zeros(model.ni)
v = np.zeros(model.no)
qs[0, :] = q0
ps[0, :] = p0
# pendulum state
X = np.zeros((N, 4))
X[0, :] = np.array([0.3, 0, 0, 0])
# real time plotting
robot_renderer = ThreeInputRenderer(model, q0, render_path=False)
pendulum_renderer = PendulumRenderer(pendulum, X[0, :], p0)
plotter = RealtimePlotter([robot_renderer, pendulum_renderer])
plotter.start()
for i in range(N - 1):
t = ts[i]
u_pendulum = -K @ X[i, :]
# controller
pd = p0
vd = np.zeros(2)
vd[0] = v[0] + DT * u_pendulum
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)
x_acc = (v[0] - vs[i, 0]) / DT
fs[i, :] = pendulum.calc_force(X[i, :], x_acc)
X[i+1, :] = pendulum.step(X[i, :], x_acc, DT)
angle = X[i, 0]
angle_est = np.arctan2(fs[i, 0], -fs[i, 1])
# record
us[i, :] = u
dqs[i+1, :] = dq
qs[i+1, :] = q
ps[i+1, :] = p
vs[i+1, :] = v
robot_renderer.set_state(q)
pendulum_renderer.set_state(X[i+1, :], p)
plotter.update()
plotter.done()
plt.figure()
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('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.figure()
plt.plot(ts, X[:, 0])
plt.grid()
plt.title('Pendulum Angle')
plt.xlabel('Time (s)')
plt.ylabel('Angle (rad)')
plt.figure()
plt.plot(ts[:-1], fs[:-1, 0], label='fx')
plt.plot(ts[:-1], fs[:-1, 1], label='fy')
plt.grid()
plt.title('EE Force')
plt.xlabel('Time (s)')
plt.ylabel('Force (N)')
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()