diff --git a/python/iLQR_walking_CoM.py b/python/iLQR_walking_CoM.py
new file mode 100644
index 0000000000000000000000000000000000000000..43c59230983ce96205f1685763bb70fe46f1f3a1
--- /dev/null
+++ b/python/iLQR_walking_CoM.py
@@ -0,0 +1,503 @@
+'''
+ iLQR Problem
+ batch iLQR applied on a robot system, that is representing walking movement
+ by considering center of mass (CoM) and the end-effector
+
+ Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
+ Written by Adie Niederberger <aniederberger@idiap.ch>
+
+ This file is part of RCFS.
+
+ RCFS is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License version 3 as
+ published by the Free Software Foundation.
+
+ RCFS is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with RCFS. If not, see <http://www.gnu.org/licenses/>.
+'''
+
+
+import numpy as np
+import numpy.matlib
+import matplotlib as mpl
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+
+
+# Helper functions
+# ===============================
+
+# Logarithmic map for R^2 x S^1 manifold
+def logmap(f, f0, bounds_o=False): # f0 --> target
+ position_error = f[:, :2] - f0[:, :2] # error of (x_current -x_tartget)
+ orientation_error = np.imag(np.log(np.exp(f0[:, -1] * 1j).conj().T *
+ np.exp(f[:, -1] * 1j).T)).conj().reshape((-1, 1))
+ # oritentation error isn't considered if within the tolerance
+ if bounds_o:
+ tolerance = np.pi / 3
+ f_b = f_bounds(f[:, -1], bounds=[f0[0, -1] - tolerance, f0[0, -1] + tolerance])[:, np.newaxis]
+ orientation_error *= np.int64(np.invert(np.isclose(np.abs(f_b), 0, atol=1e-07)))
+ error = np.hstack((position_error, orientation_error))
+ return error # shape(1, 3) columns (0-> x_error, y_error, o_error)
+
+# Forward kinematics for end-effector (in robot coordinate system)
+def fkine0(x, ls):
+ x = x.T
+ A = np.tril(np.ones([len(ls), len(ls)]))
+ f = np.vstack((ls @ np.cos(A @ x),
+ ls @ np.sin(A @ x),
+ np.mod(np.sum(x, 0) + np.pi,
+ 2 * np.pi) - np.pi)) # x1,x2,o
+ return f.T # returns x,y, o (o--> orientation as single euler)
+
+# Bounded task space
+def f_bounds(q_, **kwargs):
+ q = q_
+ if "bounds" not in kwargs:
+ bounds_ = [1e-8, 200]
+ else:
+ bounds_ = kwargs.get("bounds")
+ res = ((q - bounds_[0]) * (q < bounds_[0]) + \
+ (q - bounds_[1]) * (q > bounds_[1]))
+ return res
+
+# reverse the the whole system (joint space) i.e. q_0 -> q_n; q_1 -> q_n-1
+def reverse_system(x):
+ model["linkLengths"] = model["linkLengths"][::-1]
+ orien = np.mod(np.sum(x, 0) + np.pi, 2 * np.pi) - np.pi
+ dim = model["nbVarX"] + 1
+ A = np.zeros((dim, dim + 1), float)
+ diag_v = np.full(shape=dim, fill_value=-1, dtype=np.float64)
+ diag_v[(0, -1),] = 1
+ np.fill_diagonal(np.flipud(A), diag_v)
+ A[(0, -1), -1] = 1
+ x = np.append(x, [orien, np.pi])
+ # last entry of x_bar represent the orientation
+ x_bar = A @ x
+ return x_bar[:-1]
+
+# Forward Kinematics for all joints
+def fkine00(x, reverse_links=False):
+ T = np.tril(np.ones([model["nbVarX"], model["nbVarX"]]))
+ if reverse_links:
+ T2 = np.tril(np.matlib.repmat(model["linkLengths"][::-1], len(x), 1))
+ else:
+ T2 = np.tril(np.matlib.repmat(model["linkLengths"], len(x), 1))
+ f = np.vstack((
+ T2 @ np.cos(T @ x),
+ T2 @ np.sin(T @ x)
+ )).T
+ f = np.vstack((
+ np.zeros(2),
+ f
+ ))
+ return f
+
+# Jacobian with analytical computation (for single time step)
+def jacob0(x, ls):
+ T = np.tril(np.ones((len(ls), len(ls))))
+ J = np.vstack((
+ -np.sin(T @ x).T @ np.diag(ls) @ T,
+ np.cos(T @ x).T @ np.diag(ls) @ T,
+ np.ones(len(ls))
+ ))
+ return J
+
+# Jacobian for the center of mass with analytical computation (for single time step)
+def jacob0_com(x):
+ J_ = np.zeros((model["nbVarF"], model["nbVarX"]))
+ J_com = J_.copy()
+ m = model["mass_i"]
+
+ for i in range(len(model["linkLengths"])):
+ J_i = jacob0(x[:i + 1], model["linkLengths"][:i + 1]) * m[i]
+ J_[:J_i.shape[0], :J_i.shape[1]] = J_i
+ J_com += J_
+ J_com /= np.sum(np.r_[m, m[0]])
+ J_com[-1, :] = 0 # orientation has no impact on the com
+ return J_com
+
+# Residual (task space) and Jacobian
+def f_reach(x, target=0, bounds_o=False):
+ f = logmap(fkine0(x, model["linkLengths"]), model["mu"][target, np.newaxis], bounds_o)
+ J = np.zeros((len(x) * model["nbVarF"], len(x) * model["nbVarX"]))
+ for t in range(x.shape[0]):
+ Jtmp = jacob0(x[t], model["linkLengths"])
+ if bounds_o:
+ Jtmp[-1, :] *= np.int64(np.invert(np.isclose(np.abs(f[t, -1]), 0, atol=1e-07)))
+ J[t * model["nbVarF"]:(t + 1) * model["nbVarF"], t * model["nbVarX"]:(t + 1) * model["nbVarX"]] = Jtmp
+ return f, J
+
+# Residual (joint space) and Jacobian for q0
+def f_reach_q0(x):
+ # Within the tolerance, there's no residual
+ q_target = np.pi / 2
+ q0_error = np.imag(np.log(np.exp(q_target * 1j).conj().T *
+ np.exp(x[:, 0] * 1j).T)).conj().reshape((-1, 1))
+ tolerance = np.pi / 5
+ f_b = f_bounds(x[:, 0], bounds=[q_target - tolerance, q_target + tolerance])[:, np.newaxis]
+ q0_error *= np.int64(np.invert(np.isclose(np.abs(f_b), 0, atol=1e-07)))
+ error = np.zeros((x.shape[0], model["nbVarF"]))
+ error[:, -1] = q0_error[:, 0]
+ J = np.zeros((len(x) * model["nbVarF"], len(x) * model["nbVarX"]))
+ Jt = np.zeros((model["nbVarF"], model["nbVarX"]))
+ Jt[-1, 0] = 1
+ for t in range(x.shape[0]):
+ Jtmp = Jt.copy()
+ Jtmp[-1, :] *= np.int64(np.invert(np.isclose(np.abs(error[t, -1]), 0, atol=1e-07)))
+ J[t * model["nbVarF"]:(t + 1) * model["nbVarF"], t * model["nbVarX"]:(t + 1) * model["nbVarX"]] = Jtmp
+ return error, J
+
+# Residual (of com) and its Jacobian
+def f_reach_com(x, target=0, use_mu_ee=False):
+ # target ensures to use current target in the array defined in param.mu
+ x = x[np.newaxis, :] if x.ndim == 1 else x
+ n = int(x.size / model["nbVarX"])
+ f_t = model["mu"][target, np.newaxis].copy()
+ fcom_val = f_com(x)
+ f_b = f_bounds(fcom_val[:, 1], bounds=[0, 200])
+ if use_mu_ee:
+ f = (1) * (np.c_[fcom_val, np.full((n, 1), 0)] - f_t)
+ else:
+ f = (1) * (np.c_[fcom_val, np.full((n, 1), 0)] - model["mu_com"][target, np.newaxis])
+ f[:, 1] = f_b.copy()
+ f[:, -1] *= 0 # ensure orientation error is zero
+ J = np.zeros((len(x) * model["nbVarF"], len(x) * model["nbVarX"]))
+ for t in range(x.shape[0]):
+ Jtmp = jacob0_com(x[t])
+ Jtmp[1, :] *= np.int64(np.invert(np.isclose(np.abs(f_b[t]), 0, atol=1e-07)))
+ J[t * model["nbVarF"]:(t + 1) * model["nbVarF"], t * model["nbVarX"]:(t + 1) * model["nbVarX"]] = Jtmp
+ return f, J
+
+# Center of Mass computation
+def f_com(x, onedim=False):
+ x = x[np.newaxis, :] if x.ndim == 1 else x
+ com = np.array([fkine00(np.squeeze(x[i]))[1:].T @ model["linkLengths"] /
+ np.sum(np.r_[model["linkLengths"], model["linkLengths"][0]])
+ for i in range(x.shape[0])])
+ # if the com is calculated for just one configuration, return the array in a one dimension format
+ if onedim:
+ com = np.squeeze(com, 0)
+ return com
+
+# Residual (for all articualtions, if one of them is below zero) and its Jacobian
+def f_reach_c2(x, target=0):
+ x = x[np.newaxis, :] if x.ndim == 1 else x
+ J_out = np.empty((0, len(x) * model["nbVarX"]), int)
+ f_out = np.empty((0, model["nbVarF"]), int)
+ f_values = np.array([fkine00(np.squeeze(x[i])) for i in range(x.shape[0])])
+ for k in range(1, model["nbVarX"] + 1):
+ f_y = f_values[:, k, 1]
+ f = np.zeros((model["nbData"], model["nbVarF"]))
+ f_y = f_bounds(f_y, bounds=[-0.04, 200])
+ f[:, 1] = f_y
+ J_c2 = np.zeros((len(x) * model["nbVarF"], len(x) * model["nbVarX"]))
+ J_ = np.zeros((model["nbVarF"], model["nbVarX"]))
+ for t in range(x.shape[0]):
+ J_i = jacob0(x[t, :k], model["linkLengths"][:k])
+ J_[1, :J_i.shape[1]] = J_i[1] * int(np.invert(np.isclose(np.abs(f_y[t]), 0, atol=1e-07)))
+ J_c2[t * model["nbVarF"]:(t + 1) * model["nbVarF"], t * model["nbVarX"]:(t + 1) * model["nbVarX"]] = J_
+ # stack rowwise
+ J_out = np.append(J_out, J_c2, axis=0)
+ f_out = np.append(f_out, f, axis=0)
+ return f_out, J_out
+
+# shift an array by a given input (x,y)
+def shift(array, to_shift):
+ one_dim = False
+ if array.ndim == 1:
+ one_dim = True
+ array_ = array.copy()
+ array_ = array_[np.newaxis, :]
+ else:
+ array_ = array.copy()
+ array_[:, :2] += to_shift
+ if one_dim:
+ array_ = np.squeeze(array_, axis=0)
+
+ return array_
+
+# Calculate the range for plot
+# apply this function on each row
+def expand_range(x):
+ l_tot = np.linalg.norm(x, 1)
+ cnt = x[0] - l_tot / 2
+ expand_fct = 1.4
+ return np.array([cnt - (l_tot * expand_fct / 2), cnt + (l_tot * expand_fct / 2)])
+
+
+ # General model parameters
+# ===============================
+model = {
+"dt":1e-2,
+"nbData":30,
+"nbIter":30,
+"nbPoints":1,
+"nbVarX":9,
+"nbVarU":9,
+"nbVarF":3,
+"nbFootSteps":3,
+"linkLengths":[1.8,2., 2., 2., 2., 2., 2., 2., 1.8],
+"Q_track":1e4,
+"Q_com":1e1,
+"r": 1e0 *2. # before this was 10 and lower it was set to 2.1
+}
+model["mass_i"] = [i*1 for i in model["linkLengths"]]
+
+# Task parameters
+# ===============================
+
+# Targets
+model["mu"] = np.asarray([
+[-2.,0.0,-np.pi/2],
+[-2., 0.0,-np.pi/2],
+ [-2.,0.0, -np.pi/2]])# x , y , orientation
+# further target: [4,-1,0] not direct reachable()
+
+# target of the CoM during first phase of movement
+model["mu_com"] = np.asarray([
+[0.0,0.0,0.0],
+[0.0,0.0,0.0],
+ [0.0,0.0,0.0]])
+
+# Regularization matrix
+R = np.identity( (model["nbData"]-1) * model["nbVarU"] ) * model["r"]
+
+# System parameters
+# ===============================
+
+# Time occurence of viapoints
+tl = np.linspace(0,model["nbData"],model["nbPoints"]+1)
+tl = np.rint(tl[1:]).astype(np.int64)-1
+idx = np.array([ i + np.arange(0,model["nbVarX"],1) for i in (tl* model["nbVarX"])])
+
+u = np.zeros( model["nbVarU"] * (model["nbData"]-1) ) # Initial control command
+x0 = np.array( [np.pi/10 , np.pi/9 , np.pi/9, np.pi/9, np.pi/9,np.pi/9,np.pi/9, np.pi/9, np.pi/9] ) # Initial state (in joint space)
+# Transfer matrices (for linear system as single integrator)
+Su0 = np.vstack([np.zeros((model["nbVarX"], model["nbVarX"]*(model["nbData"]-1))),
+np.tril(np.kron(np.ones((model["nbData"]-1, model["nbData"]-1)), np.eye(model["nbVarX"])*model["dt"]))])
+Sx0 = np.kron( np.ones(model["nbData"]) , np.identity(model["nbVarX"]) ).T
+Su = Su0[idx.flatten()] # We remove the lines that are out of interest
+
+init_setup = {"u": u,
+ "x0": x0,
+ "Su0": Su0,
+ "Sx0": Sx0,
+ "Su" : Su,
+ "R" : R
+ }
+
+# function for one ilqr iteration
+def iLQR(init_setup, step):
+ u = init_setup["u"]
+ x0 = init_setup["x0"]
+ Su0 = init_setup["Su0"]
+ Sx0 = init_setup["Sx0"]
+ Su = init_setup["Su"]
+ R = init_setup["R"]
+ tl_step = 11
+ t_olap = 6 # overlap of part in trajectory, while com at init pos and ee-target reached
+ # (0 => no overlap
+ Sut_a = Su0[:model["nbVarX"] * (tl_step + t_olap), :]
+
+ for i in range(model["nbIter"]):
+ x = Su0 @ u + Sx0 @ x0 # x (150, )
+ x = x.reshape((model["nbData"], model["nbVarX"]))
+ f_pos0, J_pos0 = f_reach(x[tl_step:tl_step + t_olap], target=step, bounds_o=True)
+ f_pos, J_pos = f_reach(x, target=step, bounds_o=True)
+ f_pos[:f_pos0.shape[0], :f_pos0.shape[1]] = f_pos0
+ J_pos[:J_pos0.shape[0], :J_pos0.shape[1]] = J_pos0
+
+ f_com0, J_com0 = f_reach_com(x[:tl_step + t_olap], target=step, use_mu_ee=False)
+ f_com1, J_com1 = f_reach_com(x[-1], target=step, use_mu_ee=True)
+ f_c2, J_c2 = f_reach_c2(x, step)
+ f_q0, J_q0 = f_reach_q0(x)
+ Qq0 = 1e3 * .3
+ Q_com = np.diag(np.array([model["Q_com"] * 10, model["Q_com"] * 0.01, 0]))
+ Q_com_ = np.kron(np.eye(tl_step + t_olap), Q_com)
+ Q_com *= 10
+
+ Q_bounds = np.diag(np.array([0, model["Q_bounds"] * 1, 0]))
+ # same as np.kron()
+ Q_bounds_ = np.einsum('ij,kl->ikjl', np.eye(model["nbVarX"] * model["nbData"]), Q_bounds, \
+ ).reshape(-1, (model["nbVarX"]) * model["nbData"] * model["nbVarF"])
+ Q_q0 = np.kron(np.eye(model["nbData"]), np.diag(np.array([0, 0, 1]))) * Qq0
+
+ Q_pos = np.diag(np.array([model["Q_track"], model["Q_track"] , model["Q_track"] ]))
+ Q_pos_ = np.kron(np.eye(model["nbData"] - tl_step), Q_pos)
+ # When the target is reached, cost on orientation are higher
+ Q_pos_[:Q_pos.shape[0], :Q_pos.shape[1]] = np.eye(model["nbVarF"]) * model["Q_track"] * 1e2
+
+ Q_pos_ = np.kron(np.eye(model["nbData"]), np.eye(model["nbVarF"]) * model["Q_track"])
+ Q_pos_[-3:] *= 1e1
+
+ du = np.linalg.inv(Su0.T @ J_pos.T @ Q_pos_ @ J_pos @ Su0 +
+ Sut_a.T @ J_com0.T @ Q_com_ @ J_com0 @ Sut_a +
+ Su.T @ J_com1.T @ Q_com @ J_com1 @ Su +
+ Su0.T @ (J_c2.T @ Q_bounds_) @ J_c2 @ Su0 +
+ Su0.T @ J_q0.T @ Q_q0 @ J_q0 @ Su0 + R) @ \
+ (- Su0.T @ J_pos.T @ Q_pos_ @ f_pos.flatten() \
+ - Su0.T @ J_c2.T @ Q_bounds_ @ f_c2.flatten() \
+ - Sut_a.T @ J_com0.T @ Q_com_ @ f_com0.flatten() \
+ - Su0.T @ J_q0.T @ Q_q0 @ f_q0.flatten() \
+ - Su.T @ J_com1.T @ Q_com @ f_com1.flatten() - R@u)
+
+ alpha = 1
+ cost_bounds = np.matrix(f_c2.flatten() @ Q_bounds_) @ np.matrix(f_c2.flatten()).T
+ cost_com = (np.matrix(f_com0.flatten() @ Q_com_) @ np.matrix(f_com0.flatten()).T)
+ cost_com += (np.matrix(f_com1.flatten() @ Q_com) @ np.matrix(f_com1.flatten()).T)
+ cost_track = np.matrix(f_pos.flatten() @ Q_pos_).flatten() @ np.matrix(f_pos.flatten()).T
+ cost_q0 = np.matrix(f_q0.flatten() @ Q_q0).flatten() @ np.matrix(f_q0.flatten()).T
+
+ cost0 = cost_track + cost_com + \
+ cost_bounds + cost_q0 + np.linalg.norm(u) * model["r"]
+
+ while True:
+ utmp = u + du * alpha
+ xtmp = Su0 @ utmp + Sx0 @ x0
+ xtmp = xtmp.reshape((model["nbData"], model["nbVarX"]))
+
+ fcomtmp0, _ = f_reach_com(xtmp[:tl_step + t_olap], target=step, use_mu_ee=False)
+ fcomtmp1, _ = f_reach_com(xtmp[-1], target=step, use_mu_ee=True)
+ fpostmp0, _ = f_reach(xtmp[tl_step:tl_step + t_olap], target=step, bounds_o=False)
+ fpostmp, _ = f_reach(xtmp, target=step, bounds_o=True)
+ fpostmp[:fpostmp0.shape[0], :fpostmp0.shape[1]] = fpostmp0
+ fc2tmp, _ = f_reach_c2(xtmp, step)
+ fq0tmp, _ = f_reach_q0(xtmp[tuple(list(range(tl_step, tl_step + t_olap)) + [-1]), :])
+ fq0tmp, _ = f_reach_q0(xtmp)
+
+ cbtmp = (np.matrix(fc2tmp.flatten() @ Q_bounds_) @ np.matrix(fc2tmp.flatten()).T).item()
+ cposttmp = (np.matrix(fpostmp.flatten() @ Q_pos_).flatten() @ np.matrix(fpostmp.flatten()).T).item()
+ ccomtemp = (np.matrix(fcomtmp0.flatten() @ Q_com_) @ np.matrix(
+ fcomtmp0.flatten()).T).item()
+ ccomtemp += (np.matrix(fcomtmp1.flatten() @ Q_com) @ np.matrix(
+ fcomtmp1.flatten()).T).item()
+ cq0tmp = (np.matrix(fq0tmp.flatten() @ Q_q0).flatten() @ np.matrix(fq0tmp.flatten()).T).item()
+ cost = cposttmp + ccomtemp + np.linalg.norm(utmp) * model["r"] + cbtmp + cq0tmp
+
+ if cost < cost0 or alpha < 1e-3 * 0.5:
+ u = utmp
+ print("Iteration {}, cost: {}, alpha: {}".format(i, cost, alpha))
+ break
+ alpha /= 2
+
+ if np.linalg.norm(alpha * du) < 1e-2 or cost > cost0 and alpha < 1e-3 * 0.5: # Early stop condition
+ break
+ return x
+
+# Main program
+# ===============================
+
+coord_shift =[]
+x_track = []
+
+for t in range(model["nbFootSteps"]):
+ print("**Step: {}**".format(t+1))
+ #whole path, because we need to consider the bounds (no articulations below 0)
+ #model['r'] = 1e0*2.1
+ model["Q_com"]=1e1*1.9
+ model["Q_bounds"] = 1e4*2
+ model["Q_track"] = 1e2 *1.1
+ x = iLQR(init_setup = init_setup, step = t)
+ x_track.append(x)
+ coord_shift.append(fkine0(x[-1],model["linkLengths"]))
+ print("------------------ reverse System")
+ x0 = reverse_system(x[-1])
+ init_setup["x0"] = x0
+
+# we have odd number of steps and change the link position back to the initial position for reconstruction
+model["linkLengths"] = model["linkLengths"][::-1]
+model["nbData"] = int(model["nbData"] * .5)
+
+coord_shift0 = np.cumsum(np.array(coord_shift).reshape((-1,3)),0)
+f_steps = np.zeros((model["nbFootSteps"], model["nbData"] * 2, 3))
+f_steps_ = np.zeros((model["nbFootSteps"], model["nbData"] * 2, model["nbVarX"]))
+fT_steps = np.zeros((model["nbFootSteps"] + 1, model["nbVarX"] + 1, 2))
+fT_all = np.zeros((model["nbFootSteps"], model["nbData"] * 2, model["nbVarX"] + 1, 2)) ##(3, 50, 4, 2)
+coms = np.zeros((model["nbFootSteps"] + 1, 2))
+f_coms = np.zeros((model["nbFootSteps"], model["nbData"] * 2, 2))
+
+# i= 2,1,0
+for i in range(len(x_track) - 1, -1, -1):
+ f_t = fkine0(x_track[i], model["linkLengths"])
+ fT_t = fkine00(x_track[i][-1])
+ fT_0to50 = np.array([fkine00(x_track[i][k]) for k in range(x_track[i].shape[0])])
+ shape_ = fT_0to50.shape
+ f_com_t = f_com(x_track[i][-1])[np.newaxis, :]
+ fT_com_T = f_com(x_track[i])
+
+ if i == 0:
+ fT_t2 = fkine00(x_track[i][-1])
+ if i != 0:
+ f_steps[i, ...] = shift(f_t, coord_shift0[i - 1, :2])
+ fT_all[i, ...] = shift(fT_0to50.reshape(-1, 2), coord_shift0[i - 1, :2]).reshape(shape_)
+ fT_steps[i + 1, ...] = shift(fT_t, coord_shift0[i - 1, :2])
+ coms[i + 1, ...] = shift(f_com_t, coord_shift0[i - 1, :2])
+ f_coms[i, ...] = shift(fT_com_T, coord_shift0[i - 1, :2])
+ else:
+ f_steps[i, ...] = f_t
+ fT_steps[i, ...] = fT_t
+ fT_all[i, ...] = fT_0to50
+ fT_steps[i + 1, ...] = fkine00(x_track[i][-1])
+ fT_steps[i, ...] = fkine00(x_track[i][0])
+ coms[i + 1, ...] = f_com_t
+ coms[i, ...] = f_com(x_track[i][0])[np.newaxis, :]
+ f_coms[i, ...] = fT_com_T
+
+f_steps = f_steps[:, :, :2]
+
+f_max = np.amax(f_steps.reshape((model["nbData"] * model["nbFootSteps"]), -1), axis=0)
+f_min = np.amin(f_steps.reshape((model["nbData"] * model["nbFootSteps"]), -1), axis=0)
+f_max = np.amax(fT_all.reshape(-1, 2), axis=0)
+f_min = np.amin(fT_all.reshape(-1, 2), axis=0)
+
+bounds = np.c_[f_max, f_min] # column0 -> max_val
+
+
+
+
+bounds = np.apply_along_axis(expand_range, 1, bounds)
+fT_all = fT_all.reshape(-1, model["nbVarX"]+1,2)
+f_coms = f_coms.reshape(-1,2)
+f_coms_ground = f_coms.copy()
+f_coms_ground[...,1] = 0
+
+
+
+fig = plt.figure(figsize = (8,6))
+ax = fig.add_subplot(1, 1, 1)
+ax.set_xlabel('x [m]')
+ax.set_ylabel('y [m]')
+ax.set_xlim(bounds[0])
+ax.set_ylim(bounds[1])
+
+ax.set_aspect('equal')
+plt.axis('off')
+plt.gca().set_aspect('equal', adjustable='box')
+
+links, = ax.plot( fT_steps[0,:,0] , fT_steps[0,:,1],c='black',linewidth=4,alpha=.6)
+coms = ax.scatter(f_coms[0,0] , f_coms[0,1], s=100,marker="o",c="lightblue")
+coms_ground = ax.scatter(f_coms[0,0] , 0, s=100,marker="o",c='black', alpha = .2)
+ax = plt.gca()
+
+plt.plot( fT_steps[0,:,0] , fT_steps[0,:,1],c='black',linewidth=5,alpha=.2)
+
+# update the animation
+def update(n):
+ # update the position of each link end.
+ links.set_data(fT_all[n,:,0] , fT_all[n,:,1])
+ coms.set_offsets(f_coms[n])
+ coms_ground.set_offsets(f_coms_ground[n])
+ return links, coms_ground, coms,
+
+# Create animation object
+ani = mpl.animation.FuncAnimation(fig, update, frames=model["nbData"]*model["nbFootSteps"]*2,
+ interval=120, blit=True, repeat = True)
+ani.running = True
+ani.direction = +1
+plt.show()
\ No newline at end of file