diff --git a/matlab/iLQR_manipulator_obstacle.m b/matlab/iLQR_manipulator_obstacle.m
index 803316391d56810959e69510bc9859c29b01b50f..717fd267494d75149f689dbd4559b76c7b8056e4 100644
--- a/matlab/iLQR_manipulator_obstacle.m
+++ b/matlab/iLQR_manipulator_obstacle.m
@@ -3,7 +3,7 @@
 %%
 %%    Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
 %%    Written by Teguh Lembono <teguh.lembono@idiap.ch>, 
-%%		Sylvain Calinon <https://calinon.ch>
+%%	  Sylvain Calinon <https://calinon.ch>
 %%
 %%    This file is part of RCFS.
 %%
@@ -79,7 +79,7 @@ for n=1:param.nbIter
 	Su2 = Su0(id2,:);
 	
 	du = (Su' * J' * J * Su * param.q + Su2' * J2' * J2 * Su2 * param.q2 + R) \ ...
-			(-Su' * J' * f(:) * param.q - Su2' * J2' * f2(:) * param.q2 - u * param.r); %Gradient
+	     (-Su' * J' * f(:) * param.q - Su2' * J2' * f2(:) * param.q2 - u * param.r); %Gradient
 		
 	%Estimate step size with backtracking line search method
 	alpha = 1;
@@ -100,7 +100,6 @@ for n=1:param.nbIter
 		alpha = alpha * 0.5;
 	end
 	u = u + du * alpha;
-	disp(cost)
 	if norm(du * alpha) < 1E-2
 		break; %Stop iLQR when solution is reached
 	end
@@ -219,7 +218,7 @@ function f = fkin0batch(x, L)
 	T = tril(ones(size(x,1)));
 	T2 = T * diag(L);
 	f = [(T2 * cos(T * x))(:), ...
-			(T2 * sin(T * x))(:)]; 
+	     (T2 * sin(T * x))(:)]; 
 end
 
 %%%%%%%%%%%%%%%%%%%%%%
@@ -249,9 +248,9 @@ function [f, J, id] = f_avoid2(x, param)
 			id = [id, (t-1) * param.nbVarX + [1:param.nbVarX]'];
 		end
 		
-		f = [f; ftmp(idx_active(:))];		#keep only active value
+		f = [f; ftmp(idx_active(:))]; %keep only active value
 	end
 	
-	id = unique(id); #remove duplicate
-	J = J(:,id); #keep only active value
-end
\ No newline at end of file
+	id = unique(id); %remove duplicate
+	J = J(:,id); %keep only active value
+end
diff --git a/matlab/iLQR_obstacle.m b/matlab/iLQR_obstacle.m
index 2d2b1ce35aff633167c8037682992e783c66ca70..7837266d190e75737de1c61aa9f22120dbc7d2f7 100644
--- a/matlab/iLQR_obstacle.m
+++ b/matlab/iLQR_obstacle.m
@@ -119,22 +119,38 @@ function [f, J] = f_reach(x, param)
 	J = eye(param.nbVarX * size(x,2));
 end
 
-%%%%%%%%%%%%%%%%%%%%%%
-%Residuals f and Jacobians J for obstacle avoidance
+%%%%%%%%%%%%%%%%%%%%%%%
+%Residuals f and Jacobians J for obstacle avoidance (fast version with computation in matrix form)
 function [f, J, id, idt] = f_avoid(x, param)
-	f=[]; J=[]; id=[]; idt=[];
-	for t=1:size(x,2)
-		for i=1:param.nbObstacles			
-			e = param.U2(:,:,i)' * (x(:,t) - param.Mu2(1:2,i));
-			ftmp = 1 - e' * e; %quadratic form
-			%Bounding boxes 
-			if ftmp > 0
-				f = [f; ftmp];
-				Jtmp = -e' * param.U2(:,:,i)'; %quadratic form
-				J = blkdiag(J, Jtmp);
-				id = [id, (t-1) * param.nbVarU + [1:param.nbVarU]];
-				idt = [idt, t];
-			end
-		end
+	tl = 1:param.nbData;
+	f=[]; J=[]; id=[]; idt=[]; 
+	for i=1:param.nbObstacles
+		e = param.U2(:,:,i)' * (x - repmat(param.Mu2(1:2,i), 1, param.nbData));
+		ftmp = 1 - sum(e.^2, 1)'; %Bounding boxes
+		idt = tl(ftmp > 0); %Time indices when inside obstacles 
+		id = [id, (idt-1) * param.nbVarU + [1:param.nbVarU]']; %Corresponding position indices when inside obstacles 
+		f = [f; ftmp(idt)]; %Residuals
+		Jtmp = -e(:,idt)' * repmat(param.U2(:,:,i)', 1, length(idt));
+		J = blkdiag(J, Jtmp); %Jacobians
 	end
 end
+
+%%%%%%%%%%%%%%%%%%%%%%%
+%%Residuals f and Jacobians J for obstacle avoidance (slow version with loop over time)
+%function [f, J, id, idt] = f_avoid(x, param)
+%	f=[]; J=[]; id=[]; idt=[];
+%	for t=1:size(x,2)
+%		for i=1:param.nbObstacles			
+%			e = param.U2(:,:,i)' * (x(:,t) - param.Mu2(1:2,i));
+%			ftmp = 1 - e' * e; %quadratic form
+%			%Bounding boxes 
+%			if ftmp > 0
+%				f = [f; ftmp]; %Residuals
+%				Jtmp = -e' * param.U2(:,:,i)'; %quadratic form
+%				J = blkdiag(J, Jtmp); %Jacobians
+%				idt = [idt, t]; %Time indices when inside obstacles 
+%				id = [id, (t-1) * param.nbVarU + [1:param.nbVarU]]; %Corresponding position indices when inside obstacles
+%			end
+%		end
+%	end
+%end
diff --git a/python/iLQR_walking_CoM.py b/python/iLQR_walking_CoM.py
deleted file mode 100644
index 43c59230983ce96205f1685763bb70fe46f1f3a1..0000000000000000000000000000000000000000
--- a/python/iLQR_walking_CoM.py
+++ /dev/null
@@ -1,503 +0,0 @@
-'''
-    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