def calculate_simple_muscles(self, t):
"""Construct Z f_m0 <= B for the case of a linear muscle model for a particular
time instance.
Parameters
----------
t: time
"""
# find nearesrt index corresponding to t
idx = np.abs(np.array(self.simulation_reporter.t) - t).argmin()
t = self.simulation_reporter.t[idx]
q = self.simulation_reporter.q[idx]
u = self.simulation_reporter.u[idx]
tau = self.simulation_reporter.tau[idx]
pose = self.model.model_parameters(q=q, u=u)
n = self.model.nd
# calculate required variables
R = to_np_mat(self.model.R.subs(pose))
RBarT = np.asmatrix(np.linalg.pinv(R.T))
# reduce to independent columns to avoid singularities (proposition 3)
NR = nullspace(R.transpose())
fm_par = np.asmatrix(-RBarT * tau.reshape((n, 1)))
Fmax = to_np_mat(self.model.Fmax)
Z, B = construct_muscle_space_inequality(NR, fm_par, Fmax)
return q, Z, B, NR, fm_par
def calculate_force(self, xddot, pose):
"""Calculate task forces.
For a given end effector acceleration compute the joint space
torques:
ft = Lt (xddot - JDotQDot) + JtBarT f
tau = JtT ft + NtT f
Parameters
----------
xddot: [2 x 1] a sympy Matrix containing the desired accelerations in 2D
pose: system constants, coordinates and speeds as dictionary
Returns
-------
(tau, ft): required torque and task forces to track the desired
acceleration
"""
M = to_np_mat(self.model.M.subs(pose))
f = to_np_mat(self.model.f.subs(pose))
Jt = to_np_mat(self.Jt.subs(pose))
JtDotQDot = to_np_mat(self.JtDotQDot.subs(pose))
JtT = to_np_mat(self.JtT.subs(pose))
MInv = np.linalg.inv(M)
LtInv = Jt * MInv * JtT
Lt = np.linalg.pinv(LtInv)
JtBarT = Lt * Jt * MInv
NtT = np.asmatrix(np.eye(len(JtT))) - JtT * JtBarT
ft = Lt * (xddot - JtDotQDot) + JtBarT * f
return JtT * ft + 0 * NtT * f, ft
def __init__(self, model, use_optimization=False):
""" Constructor
Parameters
----------
model: model
"""
self.logger = Logger('MuscleSpace')
self.model = model
self.use_optimization = use_optimization
self.Fmax = to_np_mat(self.model.Fmax)
self.x_max = np.max(self.Fmax)
def visualize_simple_muscle(self, t, ax=None):
"""Visualize the feasible force set at a particular time instance for a linear
muscle.
Parameters
----------
t: time
ax: 1 x 3 axis
"""
m = self.model.md
q, Z, B, NR, fm_par = self.calculate_simple_muscles(t)
x_max = np.max(to_np_mat(self.model.Fmax))
fm_set = self.generate_solutions(Z, B, NR, fm_par)
dataframe = pd.DataFrame(
fm_set, columns=['$m_' + str(i) + '$' for i in range(1, m + 1)])
# box plot
if ax is None or ax.shape[0] < 3:
fig, ax = plt.subplots(1, 3, figsize=(15, 5))
# box plot
dataframe.plot.box(ax=ax[0])
ax[0].set_xlabel('muscle id')
ax[0].set_ylabel('force $(N)$')
ax[0].set_title('Muscle-Force Box Plot')
ax[0].set_ylim([0, 1.1 * x_max])
# correlation matrix
cmap = mpl.cm.jet
norm = mpl.colors.Normalize(vmin=-1, vmax=1)
corr = dataframe.corr()
m = plot_corr_ellipses(corr, ax=ax[1], norm=norm, cmap=cmap)
cb = plt.colorbar(m,
ax=ax[1],
orientation='vertical',
norm=norm,
cmap=cmap)
cb.set_label('Correlation Coefficient')
ax[1].margins(0.1)
ax[1].set_xlabel('muscle id')
ax[1].set_ylabel('muscle id')
ax[1].set_title('Correlation Matrix')
ax[1].axis('equal')
# draw model
self.model.draw_model(q, False, ax[2], scale=0.7, text=False)
def calculate_force(self, lmdd, pose):
"""Calculate muscle forces.
For a given muscle length acceleration compuyte the muscle space EoM of
motion and required muscle force to track the goal acceleration.
fm_par = -Lm (lmdd - RDotQDot) - RBarT f
tau = -R^T fm_par
Parameters
----------
lmdd: [m x 1] a sympy Matrix containing the desired muscle length
accelerations
pose: dictionary
Returns
-------
(tau, fm_par + fm_perp) required torque to track the desired
acceleration
"""
M = to_np_mat(self.model.M.subs(pose))
f = to_np_mat(self.model.f.subs(pose))
R = to_np_mat(self.model.R.subs(pose))
RT = R.transpose()
RDotQDot = to_np_mat(self.model.RDotQDot.subs(pose))
MInv = np.linalg.inv(M)
LmInv = R * MInv * RT
Lm = np.linalg.pinv(LmInv)
RBarT = np.linalg.pinv(RT)
NR = np.asmatrix(np.eye(len(RBarT)) - RBarT * RT)
fm_par = -Lm * (lmdd - RDotQDot) - RBarT * f
# Ensure fm_par > 0 not required for simulation, but for muscle analysis
# otherwise muscle forces will be negative. Since RT * NR = 0 the null
# space term does not affect the resultant torques.
m = fm_par.shape[0]
fm_0 = np.zeros((m, 1))
if self.use_optimization:
Z, B = construct_muscle_space_inequality(NR, fm_par, self.Fmax)
def objective(x):
return np.sum(x**2)
def inequality_constraint(x):
return np.array(B - Z * (x.reshape(-1, 1))).reshape(-1, )
x0 = np.zeros(m)
bounds = tuple([(-self.x_max, self.x_max) for i in range(0, m)])
constraints = ({'type': 'ineq', 'fun': inequality_constraint})
sol = minimize(objective,
x0,
method='SLSQP',
bounds=bounds,
constraints=constraints)
fm_0 = sol.x.reshape(-1, 1)
if sol.success == False:
raise RuntimeError('Some muscles are too week for this action')
fm_perp = NR * fm_0
return -RT * fm_par, fm_par + fm_perp
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
def calculate_stiffness_properties(self, t, calc_feasible_stiffness=False):
"""Calculates the stiffness properties of the model at a particular time
instance.
Parameters
----------
t: float
time of interest
calc_feasible_stiffness: bool
whether to calculate the feasible stiffness
Returns
-------
t: float
actual time (closest to recorded values, not interpolated)
q: mat n x 1 (mat = numpy.matrix)
generalized coordinates
xc: mat d x 1
position of the task
Km: mat m x m
muscle space stiffness
Kj: mat n x n
joint space stiffness
Kt: mat d x d
task space stiffness
"""
# find nearesrt index corresponding to t
idx = np.abs(np.array(self.simulation_reporter.t) - t).argmin()
t = self.simulation_reporter.t[idx]
q = self.simulation_reporter.q[idx]
u = self.simulation_reporter.u[idx]
fm = self.simulation_reporter.fm[idx]
ft = self.simulation_reporter.ft[idx]
pose = self.model.model_parameters(q=q, u=u)
# calculate required variables
R = to_np_mat(self.model.R.subs(pose))
RT = R.transpose()
RTDq = to_np_array(self.model.RTDq.subs(pose))
Jt = to_np_mat(self.task.Jt.subs(pose))
JtPInv = np.linalg.pinv(Jt)
JtTPInv = JtPInv.transpose()
JtTDq = to_np_array(self.task.JtTDq.subs(pose))
xc = to_np_mat(self.task.x(pose))
# calculate feasible muscle forces
if calc_feasible_stiffness:
q, Z, B, NR, fm_par = self.feasible_muscle_set_analysis\
.calculate_simple_muscles(t)
fm_set = self.feasible_muscle_set_analysis.generate_solutions(
Z, B, NR, fm_par)
# calculate stiffness properties
Km = []
Kj = []
Kt = []
for i in range(0, fm_set.shape[0]): # , fm_set.shape[0] / 500):
if calc_feasible_stiffness:
fm = fm_set[i, :]
# calculate muscle stiffness from sort range stiffness (ignores
# tendon stiffness)
gamma = 23.5
Km.append(
np.asmatrix(
np.diag([
gamma * fm[i] / self.model.lm0[i]
for i in range(0, self.model.lm0.shape[0])
]), np.float))
# switches for taking into account (=1) the tensor products
dynamic = 1.0
static = 1.0
# transpose is required in the tensor product because n(dq) x n(q) x
# d(t) and we need n(q) x n(dq)
# calculate joint stiffness
RTDqfm = np.matmul(RTDq, fm)
Kj.append(-dynamic * RTDqfm.T - static * RT * Km[-1] * R)
# calculate task stiffness
JtTDqft = np.matmul(JtTDq, ft)
Kt.append(JtTPInv * (Kj[-1] - dynamic * JtTDqft.T) * JtPInv)
return t, q, xc, Km, Kj, Kt
