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