def controller(self, x, t, x0):
""" Controller.
Parameters
----------
x: state
t: time
x0: initial state
"""
self.logger.debug('Time: ' + str(t))
m = self.model.md
n = self.model.nd
q = np.array(x[:n])
u = np.array(x[n:])
# compute current muscle length and derivative
pose = self.model.model_parameters(q=q, u=u)
lm = self.model.lm.subs(pose)
lm = to_np_vec(lm)
lmd = self.model.lmd.subs(pose)
lmd = to_np_vec(lmd)
# compute target
lm_des = self.target(x, t, x0)
lmd_des = np.zeros(m)
lmdd_des = np.zeros(m)
# update delayed values and get current (must update first)
self.lm_del.add(t, lm)
self.lmd_del.add(t, lmd)
lm_del = np.array(self.lm_del.get_delayed())
lmd_del = np.array(self.lmd_del.get_delayed())
lmdd_des = sp.Matrix(self.pd.compute(
lm_del, lmd_del, lm_des, lmd_des, lmdd_des))
tau, fm = self.musclespace.calculate_force(lmdd_des, pose)
tau = self.__add_in_distrurbance(t, tau, pose)
tau = to_np_vec(tau)
# record
self.reporter.t.append(t)
self.reporter.q.append(q)
self.reporter.u.append(u)
self.reporter.lm.append(lm)
self.reporter.lm_d.append(lm_des)
self.reporter.fm.append(fm)
self.reporter.tau.append(tau)
return tau
def controller(self, x, t, x0):
"""Default simple joint space PD controller.
Parameters
----------
x: state
t: time
x0: initial state
"""
self.logger.debug('Time: ' + str(t))
n = self.model.nd
m = self.model.md
q = np.array(x[:n])
u = np.array(x[n:])
qd = np.array(self.target(x, t, x0))
ud = np.zeros((n))
qddot = np.array(self.pid.compute(t, q, u, qd, ud))
# evalutate desired lmdd_d
pose = self.model.model_parameters(q=q, u=u)
RDotQDot = self.model.RDotQDot.subs(pose)
RQDDot = self.model.R.subs(pose) * sp.Matrix(qddot)
lmdd_d = sp.Matrix(RDotQDot + RQDDot)
lmd = to_np_vec(self.model.lmd.subs(pose))
lmd_d = to_np_vec(self.model.R.subs(pose) * sp.Matrix(u))
tau = self.musclespace.calculate_force(lmdd_d, pose)[0]
tau = to_np_vec(tau)
# store record
self.reporter.t.append(t)
self.reporter.q.append(q)
self.reporter.u.append(u)
self.reporter.qd.append(qd)
self.reporter.lmd.append(lmd)
self.reporter.lmd_d.append(lmd_d)
self.reporter.tau.append(tau)
return tau
def __initialize_delay_components(self):
"""Initializes the delay components with the default muscle length (state(0))
and zero length velocities.
"""
n = self.model.nd
m = self.model.md
state0 = self.model.state0
q = state0[:n]
u = state0[n:]
pose = self.model.model_parameters(q=q, u=u)
self.lm0 = self.model.lm.subs(pose)
self.lm0 = to_np_vec(self.lm0)
delay = np.full(m, self.delay)
self.lm_del = DelayArray(m, delay, self.lm0)
self.lmd_del = DelayArray(m, delay, np.zeros(m))
def controller(self, x, t, x0):
"""Controller function.
Parameters
----------
x: state
t: time
x0: initial state
"""
self.logger.debug('Time: ' + str(t))
n = self.model.nd
q = np.array(x[:n])
u = np.array(x[n:])
pose = self.model.model_parameters(q=q, u=u)
# task variables
xc = to_np_mat(self.task.x(pose))
uc = to_np_mat(self.task.u(pose, u))
xd, ud, ad = self.target(x, t, x0)
ad = sp.Matrix(self.pd.compute(xc, uc, xd, ud, ad))
# forces
tau, ft = self.task.calculate_force(ad, pose)
ft = to_np_vec(ft)
tau = to_np_vec(tau)
# solve static optimization
fm = None
if self.evaluate_muscle_forces:
m = self.model.md
R = to_np_mat(self.model.R.subs(pose))
RT = R.transpose()
def objective(x):
return np.sum(x**2)
def inequality_constraint(x):
return np.array(tau + RT * (x.reshape(-1, 1))).reshape(-1,)
x0 = np.zeros(m)
bounds = tuple([(0, self.model.Fmax[i, i]) for i in range(0, m)])
constraints = ({'type': 'ineq', 'fun': inequality_constraint})
sol = minimize(objective, x0, method='SLSQP',
bounds=bounds,
constraints=constraints)
if sol.success is False:
raise Exception('Static optimization failed at: ' + t)
fm = sol.x.reshape(-1,)
# record
self.reporter.t.append(t)
self.reporter.q.append(q)
self.reporter.u.append(u)
self.reporter.x.append(np.array(xc).reshape(-1,))
self.reporter.xd.append(np.array(xd).reshape(-1,))
self.reporter.tau.append(tau)
self.reporter.ft.append(ft)
self.reporter.fm.append(fm)
# self.fs_reporter.record(t, q, u, tau)
return tau