├── .gitignore ├── README.pdf ├── fig ├── SM-Figure-1.jpeg ├── SM-Figure-2.jpeg ├── SM-Figure-3.jpeg ├── SM-Figure-4.jpeg ├── SM-Figure-5.jpeg └── SM-Figure-6.jpeg ├── rho.m ├── up_star.m ├── rot2q.m ├── q2rot.m ├── up_plus.m ├── up_oplus.m ├── up_vee.m ├── get_end.m ├── q2arc.m ├── get_err.m ├── arc2q.m ├── soln2xi.m ├── up_hat.m ├── veelog.m ├── exphat.m ├── allocate_time.m ├── xi2arc.m ├── arc2xi.m ├── solve_r2.m ├── main_demo.m ├── LICENSE ├── dp.m ├── xi2len.m ├── solve_r1.m ├── collision_marker.m ├── jacobian3cc.m ├── revise_plot.m ├── collision_indicator.m ├── revise_newton.m ├── revise_grad.m ├── revise_dls.m ├── frame.m ├── circles3c.m ├── circles3.m ├── micsolver.m ├── main_demo2.m ├── micsolverd.m └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | # MATLAB 2 | .mat 3 | .asv 4 | -------------------------------------------------------------------------------- /README.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mr-November/micsolver/HEAD/README.pdf -------------------------------------------------------------------------------- /fig/SM-Figure-1.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mr-November/micsolver/HEAD/fig/SM-Figure-1.jpeg -------------------------------------------------------------------------------- /fig/SM-Figure-2.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mr-November/micsolver/HEAD/fig/SM-Figure-2.jpeg -------------------------------------------------------------------------------- /fig/SM-Figure-3.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mr-November/micsolver/HEAD/fig/SM-Figure-3.jpeg -------------------------------------------------------------------------------- /fig/SM-Figure-4.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mr-November/micsolver/HEAD/fig/SM-Figure-4.jpeg -------------------------------------------------------------------------------- /fig/SM-Figure-5.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mr-November/micsolver/HEAD/fig/SM-Figure-5.jpeg -------------------------------------------------------------------------------- /fig/SM-Figure-6.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mr-November/micsolver/HEAD/fig/SM-Figure-6.jpeg -------------------------------------------------------------------------------- /rho.m: -------------------------------------------------------------------------------- 1 | function d = rho(a, L) 2 | %RHO Computes the linear distance between two ends of a circular arc. 3 | % This is a private function of our solver. 4 | 5 | if a == 1 6 | d = L; 7 | else 8 | d = L*sqrt(1-a^2)/acos(a); 9 | end 10 | end -------------------------------------------------------------------------------- /up_star.m: -------------------------------------------------------------------------------- 1 | function q_up_star = up_star(q) 2 | %UP_STAR Computes the quaternion conjugation. 3 | % Q_UP_STAR = UP_STAR(Q) returns the conjugation of Q. 4 | 5 | % q^* = (\delta, -\epsilon) 6 | delta = q(1); 7 | epsilon = q(2:4); 8 | q_up_star = [delta; -epsilon]; 9 | end -------------------------------------------------------------------------------- /rot2q.m: -------------------------------------------------------------------------------- 1 | function q = rot2q(R) 2 | %ROT2Q Converts a rotation matrix to a quaternion. 3 | % Q = ROT2Q(R) returns the quaternion that is equivalent to the rotation 4 | % matrix. 5 | 6 | u = up_vee(logm(R)); 7 | theta = norm(u); 8 | u = u / theta; 9 | q = [cos(theta/2); sin(theta/2).*u]; 10 | if q(1) < 0 11 | q = -q; 12 | end 13 | end -------------------------------------------------------------------------------- /q2rot.m: -------------------------------------------------------------------------------- 1 | function R = q2rot(q) 2 | %Q2ROT Converts a quaternion to a rotation matrix. 3 | % R = Q2ROT(Q) returns the rotation matrix that is equivalent to the 4 | % quaternion. 5 | 6 | a = q(1); 7 | b = q(2); 8 | c = q(3); 9 | d = q(4); 10 | R = [1-2*c^2-2*d^2, 2*b*c-2*a*d, 2*a*c+2*b*d; 11 | 2*b*c+2*a*d, 1-2*b^2-2*d^2, 2*c*d-2*a*b; 12 | 2*b*d-2*a*c, 2*a*b+2*c*d, 1-2*b^2-2*c^2]; 13 | end -------------------------------------------------------------------------------- /up_plus.m: -------------------------------------------------------------------------------- 1 | function q_up_plus = up_plus(q) 2 | %UP_PLUS Computes the matrix for left multiplications of quaternions. 3 | % Q_UP_PLUS = UP_PLUS(Q) returns the left multiplication matrix of Q. 4 | 5 | % q^+ = (\delta, -\epsilon^t 6 | % \epsilon^t, \delta I + \epsilon^\times) 7 | delta = q(1); 8 | epsilon = q(2:4); 9 | q_up_plus = [delta, -epsilon.'; epsilon, delta * eye(3) + up_hat(epsilon)]; 10 | end -------------------------------------------------------------------------------- /up_oplus.m: -------------------------------------------------------------------------------- 1 | function q_up_oplus = up_oplus(q) 2 | %UP_OPLUS Computes the matrix for right multiplications of quaternions. 3 | % Q_UP_OPLUS = UP_OPLUS(Q) returns the right multiplication matrix of Q. 4 | 5 | % q^\oplus = (\delta, -\epsilon^t 6 | % \epsilon^t, \delta I - \epsilon^\times) 7 | delta = q(1); 8 | epsilon = q(2:4); 9 | q_up_oplus = [delta, -epsilon.'; epsilon, delta * eye(3) - up_hat(epsilon)]; 10 | end -------------------------------------------------------------------------------- /up_vee.m: -------------------------------------------------------------------------------- 1 | function V = up_vee(M) 2 | %UP_VEE Computes the Lie algebra of a vector. 3 | % V = UP_VEE(M) is an element of \mathbb{R}^3 or \mathbb{R}^6, where M is 4 | % an element of \mathsf{so}_3 or \mathsf{se}_3, respectively. 5 | 6 | if length(M) == 3 7 | V = [-M(2, 3); M(1, 3); -M(1, 2)]; 8 | elseif length(M) == 4 9 | V = [-M(2, 3); M(1, 3); -M(1, 2); M(1:3, 4)]; 10 | else 11 | error('Input not in so/se3'); 12 | end 13 | end -------------------------------------------------------------------------------- /get_end.m: -------------------------------------------------------------------------------- 1 | function T = get_end(L1, L2, L3, xi) 2 | %GET_END Computes the end pose of a 3-section constant-curvature robot. 3 | % T = GET_END(L1, L2, L3, XI) returns the 4-by-4 matrix of end pose. 4 | % 5 | % Example 6 | % xi = [-1.6; 0.8; 1.2; -0.2; 0.6; 0.2]; 7 | % T = get_end(1, 1, 1, xi); 8 | 9 | T1 = exphat( [xi(1); xi(2); 0; 0; 0; L1] ); 10 | T2 = exphat( [xi(3); xi(4); 0; 0; 0; L2] ); 11 | T3 = exphat( [xi(5); xi(6); 0; 0; 0; L3] ); 12 | T = T1 * T2 * T3; 13 | end -------------------------------------------------------------------------------- /q2arc.m: -------------------------------------------------------------------------------- 1 | function arc = q2arc(q, L) 2 | %Q2ARC Converts a quaternion to arc parameters. 3 | % ARC = Q2ARC(Q, L) computes the arc parameters of a 1-section 4 | % constant-curvature robot, including the curvature KAPPA and bending 5 | % angle PHI. The section length is L. The quaternion Q represents the end 6 | % rotation. 7 | % 8 | % Example 9 | % q = [1/2; -3/4; sqrt(3)/4; 0]; 10 | % arc = q2arc(q, 1); 11 | 12 | kappa = 2/L*acos(q(1)); 13 | phi = atan2(-q(2), q(3)); 14 | arc = [kappa, phi]; 15 | end -------------------------------------------------------------------------------- /get_err.m: -------------------------------------------------------------------------------- 1 | function err = get_err(r1, r2, r3, L1, L2, L3, q, r) 2 | %GET_ERR Computes the error between desired and current end pose. 3 | % This is a private function of our solver. 4 | 5 | Td = [q2rot(q), r; 0, 0, 0, 1]; 6 | T1 = [q2rot([r1(3), -r1(2), r1(1), 0]), rho(r1(3), L1).*r1; 0, 0, 0, 1]; 7 | T2 = [q2rot([r2(3), -r2(2), r2(1), 0]), rho(r2(3), L2).*r2; 0, 0, 0, 1]; 8 | T3 = [q2rot([r3(3), -r3(2), r3(1), 0]), rho(r3(3), L3).*r3; 0, 0, 0, 1]; 9 | Tt = T1 * T2 * T3; 10 | V = veelog(Tt \ Td); 11 | err = norm(V); 12 | end -------------------------------------------------------------------------------- /arc2q.m: -------------------------------------------------------------------------------- 1 | function q = arc2q(kappa, phi, L) 2 | %ARC2Q Converts arc parameters to a quaternion. 3 | % Q = ARC2Q(KAPPA, PHI, L) computes the quaternion Q representing the end 4 | % rotation of a 1-section constant-curvature robot with curvature KAPPA, 5 | % bending angle PHI, and section length L. 6 | % 7 | % Example 8 | % kappa = 2*pi/3; 9 | % phi = pi/3; 10 | % q = arc2q(kappa, phi, 1); 11 | 12 | q(1) = cos(kappa*L/2); 13 | q(2) = -sin(kappa*L/2)*sin(phi); 14 | q(3) = sin(kappa*L/2)*cos(phi); 15 | q(4) = 0; 16 | end 17 | -------------------------------------------------------------------------------- /soln2xi.m: -------------------------------------------------------------------------------- 1 | function xi = soln2xi(L1, L2, L3, soln) 2 | %SOLN2XI Converts the output of our solver to an exponential coordinate. 3 | % This is a private function of our solver. 4 | 5 | % kappa = 2/L*acos(q(1)); 6 | % phi = atan2(-q(2), q(3)); 7 | % arc = [kappa, phi]; 8 | k1 = 2/L1*acos(soln(3)); 9 | p1 = atan2(soln(2), soln(1)); 10 | k2 = 2/L2*acos(soln(6)); 11 | p2 = atan2(soln(5), soln(4)); 12 | k3 = 2/L3*acos(soln(9)); 13 | p3 = atan2(soln(8), soln(7)); 14 | xi = [-L1*k1*sin(p1); 15 | L1*k1*cos(p1); 16 | -L2*k2*sin(p2); 17 | L2*k2*cos(p2); 18 | -L3*k3*sin(p3); 19 | L3*k3*cos(p3)]; 20 | end -------------------------------------------------------------------------------- /up_hat.m: -------------------------------------------------------------------------------- 1 | function M = up_hat(V) 2 | %UP_HAT Computes the Lie algebra of a vector. 3 | % M = UP_HAT(V) is an element of \mathsf{so}_3 or \mathsf{se}_3, where V 4 | % is an element of \mathbb{R}^3 or \mathbb{R}^6, respectively. 5 | 6 | if length(V) == 3 7 | M = [ 0, -V(3), V(2); 8 | V(3), 0, -V(1); 9 | -V(2), V(1), 0]; 10 | elseif length(V) == 6 11 | M = [ 0, -V(3), V(2), V(4); 12 | V(3), 0, -V(1), V(5); 13 | -V(2), V(1), 0, V(6); 14 | 0, 0, 0, 0]; 15 | else 16 | error('Input not in R3/R6'); 17 | end 18 | end -------------------------------------------------------------------------------- /veelog.m: -------------------------------------------------------------------------------- 1 | function V = veelog(M) 2 | %VEELOG Composition of the matrix logarithm and the vee map. 3 | % V = VEELOG(M) is a vector in \mathbb{R}^3 or \mathbb{R}^4 and is 4 | % computed using Rodrigues' formula. The matrix M is in \mathsf{SO}_3 or 5 | % \mathsf{SE}_3. The vee map sends an element of \mathsf{so}_3 or 6 | % \mathsf{se}_3 to a vector. 7 | 8 | R = M(1: 3, 1: 3); 9 | if norm(R - eye(3)) < 2e-8 10 | V = [0; 0; 0; M(1:3, 4)]; 11 | else 12 | theta = acos((trace(R)-1)/2); 13 | omega_hat = 1/(2*sin(theta))*(R - R.'); 14 | V = [up_vee(omega_hat) * theta; (eye(3) - theta/2*omega_hat + (1 - theta/2*cot(theta/2)) * omega_hat^2) * M(1:3, 4)]; 15 | end 16 | end -------------------------------------------------------------------------------- /exphat.m: -------------------------------------------------------------------------------- 1 | function M = exphat(V) 2 | %EXPHAT Composition of the hat map and the matrix exponential. 3 | % M = EXPHAT(V) is a matrix in \mathsf{SO}_3 or \mathsf{SE}_3 and is 4 | % computed using Rodrigues' formula. The vector V is in \mathbb{R}^3 or 5 | % \mathbb{R}^4. The hat map sends V to an element of \mathsf{so}_3 or 6 | % \mathsf{se}_3. 7 | 8 | theta = norm(V(1:3)); 9 | if theta < 2e-8 10 | M = [eye(3), V(4:6); 0, 0, 0, 1]; 11 | else 12 | omega = V(1:3) ./ theta; 13 | v = V(4:6) ./ theta; 14 | M = [eye(3) + sin(theta) * up_hat(omega) + (1 - cos(theta)) * up_hat(omega)^2, (eye(3) * theta + (1 - cos(theta)) * up_hat(omega) + (theta - sin(theta)) * up_hat(omega)^2) * v; 0, 0, 0, 1]; 15 | end 16 | end -------------------------------------------------------------------------------- /allocate_time.m: -------------------------------------------------------------------------------- 1 | function ts = allocate_time(xis) 2 | %ALLOCATE_TIME allocates optimal time to a given sequence of concatenated 3 | % parameters, considering the actuator velocity constraints. 4 | % 5 | % TS = ALLOCATE_TIME(XIS) returns the time array TS. 6 | 7 | RESOLUTION = -200000/(21*pi); 8 | FACTOR = sqrt(2); 9 | VEL_MAX = [4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000].'; 10 | L_DOT_MAX = VEL_MAX ./ RESOLUTION ./ FACTOR; 11 | 12 | p = size(xis, 2); 13 | titv = zeros(1, p); 14 | for i = 2:p 15 | xi_diff = xis(:, i) - xis(:, i-1); 16 | diff = xi2len(xi_diff); 17 | titv(i) = max(abs(diff) ./ abs(L_DOT_MAX)); 18 | end 19 | ts = zeros(1, p); 20 | for i = 2:p 21 | ts(i) = titv(i) + ts(i-1); 22 | end 23 | 24 | end 25 | 26 | -------------------------------------------------------------------------------- /xi2arc.m: -------------------------------------------------------------------------------- 1 | function arc = xi2arc(L1, L2, L3, xi) 2 | %XI2ARC Converts the overall exponential coordinate to the arc parameters 3 | % of 3 sections. 4 | % ARC = XI2ARC(L1, L2, L3, XI) computes the arc parameter ARC of a 5 | % 3-section constant-curvature robot. The section lengths are L1, L2 and 6 | % L3, respectively. The parameter XI is the overall exponential 7 | % coordinate. 8 | % 9 | % Example 10 | % xi = [-1.6; 0.8; 1.2; -0.2; 0.6; 0.2]; 11 | % arc = xi2arc(1, 1, 1, xi); 12 | 13 | k1 = mod(sqrt(xi(1)^2 + xi(2)^2), 2*pi) / L1; 14 | p1 = atan2(-xi(1), xi(2)); 15 | k2 = mod(sqrt(xi(3)^2 + xi(4)^2), 2*pi) / L2; 16 | p2 = atan2(-xi(3), xi(4)); 17 | k3 = mod(sqrt(xi(5)^2 + xi(6)^2), 2*pi) / L3; 18 | p3 = atan2(-xi(5), xi(6)); 19 | arc = [k1, p1, k2, p2, k3, p3]; 20 | end -------------------------------------------------------------------------------- /arc2xi.m: -------------------------------------------------------------------------------- 1 | function xi = arc2xi(L1, L2, L3, arc) 2 | %ARC2XI Converts the arc parameters of 3 sections to the exponential coordinate. 3 | % XI = ARC2XI(L1, L2, L3, ARC) computes the exponential coordinate XI of 4 | % a 3-section constant-curvature robot. The section lengths are L1, L2 5 | % and L3, respectively. The parameter ARC is an array containing 6 | % curvatures and bending angles of each section. 7 | % 8 | % Example 9 | % k1 = 4*sqrt(5)/5; p1 = atan(2); 10 | % k2 = sqrt(37)/5; p2 = -pi+atan(6); 11 | % k3 = sqrt(10)/5; p3 = -atan(3); 12 | % xi = arc2xi(1, 1, 1, [k1, p1, k2, p2, k3, p3]); 13 | 14 | xi = [-L1*arc(1)*sin(arc(2)); 15 | L1*arc(1)*cos(arc(2)); 16 | -L2*arc(3)*sin(arc(4)); 17 | L2*arc(3)*cos(arc(4)); 18 | -L3*arc(5)*sin(arc(6)); 19 | L3*arc(5)*cos(arc(6))]; 20 | end -------------------------------------------------------------------------------- /solve_r2.m: -------------------------------------------------------------------------------- 1 | function [r2r, r2t] = solve_r2(L1, L3, q, r, r3, r1) 2 | %SOLVE_R2 Computes the model parameter of the 2nd section using rotational and translational constraints. 3 | % This is a private function of our solver. 4 | 5 | a = q(1); b = q(2); c = q(3); d = q(4); 6 | B = [d, a, b; -a, d, c; -b, -c, d]; 7 | m = [c; -b; a]; 8 | qe = [m.' * r3; B * r3]; 9 | re = [0; r] - rho(r3(3), L3) .* up_plus(qe) * up_oplus(up_star(qe)) * [0; r3]; re = re(2:4); 10 | 11 | % Use rotational constraint. 12 | % q_e = q \otimes q_3^{-1} 13 | % A_e = ( -a_e -d_e c_e 14 | % d_e -a_e -b_e 15 | % c_e, -b_e, a_e ) 16 | % r2 = A_e r1 17 | ae = qe(1); be = qe(2); ce = qe(3); de = qe(4); 18 | Ae = [-ae, -de, ce; de, -ae, -be; ce, -be, ae]; 19 | r2r = Ae * r1; 20 | 21 | % Use translational constraint. 22 | rv = re - rho(r1(3), L1) .* r1; 23 | negtive_w2 = (2 * (r1 * r1.') - eye(3)) * rv / norm(rv); 24 | r2t = [-negtive_w2(1); -negtive_w2(2); negtive_w2(3)]; 25 | end -------------------------------------------------------------------------------- /main_demo.m: -------------------------------------------------------------------------------- 1 | clear; 2 | clc; 3 | close all; 4 | 5 | % Robot section lengths. 6 | L1 = 1; L2 = 1; L3 = 1; 7 | 8 | % Self-defined end pose. 9 | % This example provides an example with four multiple solutions. 10 | alpha = 15*pi/16; omega = [0.48; sqrt(3)/10; -0.86]; 11 | q = [cos(alpha/2); sin(alpha/2)*omega]; 12 | r = [-0.4; 1.1; 0.8]; 13 | 14 | % Solve the problem. 15 | tol = 1e-2; exitfcn = @(e, x) e < tol; 16 | [sol, ~, ~] = micsolverd(L1, L2, L3, q, r, 0.01, [5, 5], tol, 4, exitfcn, 'plot', 'plot'); 17 | 18 | % Plot the results. 19 | for eta = 1: size(sol, 2) 20 | fh = figure(10+eta); 21 | circles3(fh, L1, L2, L3, sol(:, 2), 'k--'); 22 | circles3(fh, L1, L2, L3, sol(:, eta), 'k-'); 23 | 24 | % Set the azimuth and elevation angles of the sight. 25 | view(119, 20); 26 | 27 | % Set axis limits. 28 | xlim([-1.6, 0.8]); 29 | xticks([-1, 0, 1]); 30 | ylim([-0.8, 1.6]); 31 | yticks([0, 1]); 32 | zlim([-0.7, 1.7]); 33 | zticks([0, 1]); 34 | end 35 | 36 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Mr-November 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /dp.m: -------------------------------------------------------------------------------- 1 | function [optpath, totcost] = dp(xisc, lossfcn) 2 | %DP Finds the shortest path in a graph. 3 | % [PATH, COST] = DP(XISC, LOSSFCN) returns the paths and corresponding 4 | % costs using the Dijkstra's algorithm. The cell array XISC defines the 5 | % vertices. The function handle LOSSFCN defines the weight of two 6 | % adjacent edges. The output PATH and COST are cell arrays. 7 | 8 | n1 = size(xisc{1}, 2); 9 | pre_cost = zeros(n1, 1); 10 | cur_cost = NaN(n1, 1); 11 | pre_path = num2cell((1: n1).'); 12 | 13 | for k = 1: size(xisc, 2)-1 14 | m = size(xisc{k+1}, 2); 15 | cost = NaN(m, 1); 16 | path = num2cell(NaN(m, 1)); 17 | for j = 1: m 18 | for i = 1: size(xisc{k}, 2) 19 | cur_cost(i) = pre_cost(i) + lossfcn(xisc{k}(:, i), xisc{k+1}(:, j)); 20 | end 21 | cost(j) = min(cur_cost); 22 | idcs = find(cur_cost == cost(j)); 23 | path{j} = [vertcat(pre_path{idcs}), j .* ones(size(vertcat(pre_path{idcs}), 1), 1)]; 24 | end 25 | pre_cost = cost; 26 | cur_cost = NaN(size(pre_cost)); 27 | pre_path = path; 28 | end 29 | 30 | totcost = pre_cost; 31 | optpath = pre_path; 32 | 33 | end 34 | 35 | 36 | -------------------------------------------------------------------------------- /xi2len.m: -------------------------------------------------------------------------------- 1 | function len = xi2len(xi) 2 | %XI2LEN Converts the concatenated parameter to the actuator lengths. 3 | % 4 | % LEN = XI2LEN(XI) computes the actuator lengths LEN of a 3-section 5 | % constant-curvature robot. The concatenated parameter XI is defined in 6 | % our article. 7 | % 8 | % Example 9 | % xi = [-1.6; 0.8; 1.2; -0.2; 0.6; 0.2]; 10 | % len = xi2len(xi); 11 | 12 | r = 84/2; 13 | psi_lambda = [0, 4*pi/9, 2*pi/9]; 14 | psi_mu = [0, 2*pi/3, 4*pi/3]; 15 | delta_1_mu = r * [cos(psi_lambda(1) + psi_mu); 16 | sin(psi_lambda(1) + psi_mu)]; 17 | delta_2_mu = r * [cos(psi_lambda(2) + psi_mu); 18 | sin(psi_lambda(2) + psi_mu)]; 19 | delta_3_mu = r * [cos(psi_lambda(3) + psi_mu); 20 | sin(psi_lambda(3) + psi_mu)]; 21 | U = [delta_1_mu, delta_2_mu, delta_3_mu; 22 | zeros(size(delta_1_mu)), delta_2_mu, delta_3_mu; 23 | zeros(size(delta_1_mu)), zeros(size(delta_2_mu)), delta_3_mu]; 24 | C = [0, -1, 0, 0, 0, 0; 25 | 1, 0, 0, 0, 0, 0; 26 | 0, 0, 0, -1, 0, 0; 27 | 0, 0, 1, 0, 0, 0; 28 | 0, 0, 0, 0, 0, -1; 29 | 0, 0, 0, 0, 1, 0;]; 30 | len = U.' * C * xi; 31 | end 32 | 33 | -------------------------------------------------------------------------------- /solve_r1.m: -------------------------------------------------------------------------------- 1 | function r1 = solve_r1(L1, q, r, r3, noc) 2 | %SOLVE_R1 Computes the model parameter of the 1st section. 3 | % This is a private function of our solver. 4 | 5 | a = q(1); b = q(2); c = q(3); d = q(4); 6 | B = [d, a, b; -a, d, c; -b, -c, d]; 7 | n0 = B.' * r; 8 | r0 = (L1 * d)/norm(n0)^2 .* n0; 9 | ne = B * r3; 10 | 11 | r1 = spp(n0, (1/2 + 1/pi) * L1 * d, ne, r3); 12 | 13 | if d ~= 0 14 | for cor_idx = 1: noc % One-step correction. 15 | tmp = r1 - [r0, ne] * (([n0.' + [0, 0, L1*d*(1/acos(r1(3)) - 1/sqrt(1-r1(3)^2))]; ne.'] * [r0, ne]) \ [n0.' * r1 - rho(r1(3), L1) * d; ne.' * r1]); 16 | r1 = tmp / norm(tmp); 17 | end 18 | end 19 | end 20 | 21 | 22 | function soln = spp(n1, d, n2, rn) 23 | % This function solves $r$ that satisfies 24 | % n_1 \cdot r - d = 0, 25 | % n_2 \cdot r = 0, 26 | % | r | = 1. 27 | % Geometrically, $r$ is the intersections of a sphere and two planes. 28 | % Here, we need n_1 cross n_2 \neq 0. 29 | 30 | n11 = n1(1); 31 | n12 = n1(2); 32 | n13 = n1(3); 33 | 34 | n21 = n2(1); 35 | n22 = n2(2); 36 | n23 = n2(3); 37 | 38 | det0 = n11 * n22 - n12 * n21; 39 | det1 = n12 * n23 - n13 * n22; 40 | det2 = n11 * n23 - n13 * n21; 41 | 42 | a = det1^2 + det2^2 + det0^2; 43 | if a < eps 44 | soln = [-rn(1)*rn(3); -rn(2)*rn(3); rn(1)^2+rn(2)^2]/sqrt(rn(1)^2+rn(2)^2); 45 | else 46 | b = 2 * d * (n22 * det1 + n21 * det2); 47 | c = d^2 * (n22^2 + n21^2) - det0^2; 48 | delta = b^2 - 4 * a * c; 49 | if delta < 0 50 | r3 = -b / (2 * a); 51 | r1 = (det1 * r3 + n22 * d) / det0; 52 | r2 = -(det2 * r3 + n21 * d) / det0; 53 | soln = [r1; r2; r3] ./ norm([r1, r2, r3]); 54 | else 55 | r3 = (-b + sqrt(delta)) / (2 * a); 56 | r1 = (det1 * r3 + n22 * d) / det0; 57 | r2 = -(det2 * r3 + n21 * d) / det0; 58 | soln = [r1; r2; r3]; 59 | end 60 | end 61 | end -------------------------------------------------------------------------------- /collision_marker.m: -------------------------------------------------------------------------------- 1 | function collision_marker(L1, L2, L3, xi, ro, ror) 2 | %COLLISION_MARKER Visualises the collision part in yellow. 3 | % COLLISION_MARKER(L1, L2, L3, XI, RO, ROR) draws the part of the robot 4 | % that is collided with obstacles. The robot is described by the section 5 | % lengths L1, L2, L3 and the overall exponential coordinate XI. The 6 | % obstacles are spheres centring at RO with radius ROR. 7 | % 8 | % Example 9 | % L1 = 1; L2 = 1; L3 = 1; 10 | % xi = [-1.6; 0.8; 1.2; -0.2; 0.6; 0.2]; 11 | % ro = [0.8; 0.7; 0.6]; 12 | % ror = 0.4; 13 | % circles3(1, L1, L2, L3, xi, 'k-'); 14 | % collision_marker(L1, L2, L3, xi, ro, ror); 15 | % view(75, 9); 16 | 17 | ms = 16; 18 | clr = [[255, 221, 0]/255, 1]; 19 | smp = 600; 20 | k = length(ror); 21 | T1 = exphat( [xi(1); xi(2); 0; 0; 0; L1] ); 22 | T12 = T1 * exphat( [xi(3); xi(4); 0; 0; 0; L2] ); 23 | m = []; 24 | for i = 1: smp 25 | Tt1 = exphat( [xi(1); xi(2); 0; 0; 0; L1]/smp*i ); 26 | r = Tt1(1:3, 4); 27 | for j = 1: k 28 | dr = r - ro(:, j); 29 | dist = norm(dr); 30 | if dist < ror(k) 31 | m = [m, [r(1); r(2); r(3)]]; 32 | end 33 | end 34 | 35 | Tt2 = T1 * exphat( [xi(3); xi(4); 0; 0; 0; L2]/smp*i ); 36 | r = Tt2(1:3, 4); 37 | for j = 1: k 38 | dr = r - ro(:, j); 39 | dist = norm(dr); 40 | if dist < ror(k) 41 | m = [m, [r(1); r(2); r(3)]]; 42 | end 43 | end 44 | 45 | Tt3 = T12 * exphat( [xi(5); xi(6); 0; 0; 0; L3]/smp*i ); 46 | r = Tt3(1:3, 4); 47 | for j = 1: k 48 | dr = r - ro(:, j); 49 | dist = norm(dr); 50 | if dist < ror(k) 51 | m = [m, [r(1); r(2); r(3)]]; 52 | end 53 | end 54 | end 55 | if ~isempty(m) 56 | plot3(m(1, :), m(2, :), m(3, :), '.', 'MarkerSize', ms, 'Color', clr); 57 | end 58 | end -------------------------------------------------------------------------------- /jacobian3cc.m: -------------------------------------------------------------------------------- 1 | function J = jacobian3cc(L1, L2, L3, xi) 2 | %JACOBIAN3CC Computes the Jacobian matrix when the forward kinematics is 3 | % expressed by the product of exponentials formula. 4 | % J = JACOBIAN3CC(L1, L2, L3, XI) returns the 6-by-6 Jacobian matrix. 5 | 6 | j3 = jaco_c12(xi(5), xi(6), L3); 7 | 8 | T3 = expm(up_hat( [xi(5); xi(6); 0; 0; 0; L3] )); 9 | invT3 = [T3(1:3, 1:3).', -T3(1:3, 1:3).' * T3(1:3, 4); 0, 0, 0, 1]; 10 | j2 = jaco_c12(xi(3), xi(4), L2); 11 | j2_c1 = up_vee(invT3 * up_hat(j2(:, 1)) * T3); 12 | j2_c2 = up_vee(invT3 * up_hat(j2(:, 2)) * T3); 13 | j2 = [j2_c1, j2_c2]; 14 | 15 | T2 = expm(up_hat( [xi(3); xi(4); 0; 0; 0; L2] )); 16 | invT2 = [T2(1:3, 1:3).', -T2(1:3, 1:3).' * T2(1:3, 4); 0, 0, 0, 1]; 17 | j1 = jaco_c12(xi(1), xi(2), L1); 18 | j1_c1 = up_vee(invT3 * invT2 * up_hat(j1(:, 1)) * T2 * T3); 19 | j1_c2 = up_vee(invT3 * invT2 * up_hat(j1(:, 2)) * T2 * T3); 20 | j1 = [j1_c1, j1_c2]; 21 | 22 | J = [j1, j2, j3]; 23 | end 24 | 25 | function Jc = jaco_c12(w1, w2, L) 26 | % Jleft = @(w) eye(3) + (1 - cos(norm(w)))/(norm(w))^2 .* up_hat(w) + (norm(w) - sin(norm(w)))/(norm(w))^3 .* (up_hat(w))^2; 27 | % Jright = @(w) eye(3) - (1 - cos(norm(w)))/(norm(w))^2 .* up_hat(w) + (norm(w) - sin(norm(w)))/(norm(w))^3 .* (up_hat(w))^2; 28 | % g = @(w, v) [expm(up_hat(w)), Jleft(w)*v; 0, 0, 0, 1]; 29 | w = [w1, w2, 0]; 30 | n = norm(w); 31 | if n == 0 32 | ML = L/2; 33 | Jc = [eye(3); [0, ML, 0; -ML, 0, 0; 0, 0, 0]]; 34 | else 35 | M = (1 - cos(n)) / n^2; 36 | N = (n - sin(n)) / n^3; 37 | p1M = w1/n^2 - w1*N - 2*w1*M/n^2; 38 | p2M = w2/n^2 - w2*N - 2*w2*M/n^2; 39 | p1N = w1*M/n^2 - 3*w1*N/n^2; 40 | p2N = w2*M/n^2 - 3*w2*N/n^2; 41 | pwJleftwv = L * [ p1M*w2, p2M*w2 + M, 0; 42 | -p1M*w1 - M, -p2M*w1, 0; 43 | -p1N*n^2 - 2*N*w1, -p2N*n^2 - 2*N*w2, 0]; 44 | Jc = [eye(3) - M .* up_hat(w) + N .* (up_hat(w))^2; 45 | expm(up_hat(w)).' * pwJleftwv]; 46 | end 47 | Jc = Jc(:, 1:2); 48 | end -------------------------------------------------------------------------------- /revise_plot.m: -------------------------------------------------------------------------------- 1 | function revise_plot(k, omg_e, v_e, err, tol, method) 2 | %REVISE_PLOT Visualises the numerical correction. 3 | % This is a private function of numerical methods. 4 | figure(); 5 | pos = [50, 50, 680, 600]; 6 | lwt = 3; 7 | ms = 5.5; 8 | colour2 = [255,78,166]/255; 9 | colour1 = [127,17,70]/255; 10 | rs = semilogy(k, omg_e, 's--', 'LineWidth', lwt, 'MarkerSize', ms, 'Color', colour1, 'MarkerFaceColor', colour1); 11 | hold on; 12 | bs = semilogy(k, v_e, 's--', 'LineWidth', lwt, 'MarkerSize', ms, 'Color', colour2, 'MarkerFaceColor', colour2); 13 | ks = semilogy(k, err, 's-', 'LineWidth', lwt, 'MarkerSize', ms, 'Color', [0, 0, 0, 1]); 14 | 15 | xlabel('Step of iterations', 'Units', 'normalized', 'Position', [0.5,-0.09,0]); 16 | ylabel('Error', 'Units', 'normalized', 'Position', [-0.13,0.5,0]); 17 | grid off; 18 | 19 | xl = xlim; 20 | yl = ylim; 21 | plot([0, k(end)], [tol, tol], '-', 'Color', [0.7, 0.7, 0.7, 0.2], 'LineWidth', 2*lwt); 22 | tolax = fill([-1,-1,k(end),k(end)], [1e-20,tol,tol,1e-20], [1,1,1]*0.8, 'FaceAlpha', 0.2, 'EdgeColor', 'none'); 23 | xlim(xl); 24 | ylim(yl); 25 | 26 | if method == 1 27 | title('Gradient Method'); 28 | legend([rs, bs, ks, tolax], ' Angular error', ' Linear error', ' Total error', ' Tolerance', ... 29 | 'Location', 'northeast'); 30 | elseif method == 2 31 | title('Damped least-squares Method'); 32 | legend([rs, bs, ks, tolax], ' Angular error', ' Linear error', ' Total error', ' Tolerance', ... 33 | 'Location', 'northeast'); 34 | elseif method == 3 35 | title('Newton-Raphson Method'); 36 | legend([rs, bs, ks, tolax], ' Angular error', ' Linear error', ' Total error', ' Tolerance', ... 37 | 'Location', 'southwest'); 38 | end 39 | set(gcf, 'Position', pos); 40 | set(gca, 'InnerPosition', pos, 'Units', 'centimeters', 'Position', [2.7,2.2,14.3,12.7], ... 41 | 'FontName', 'Times New Roman', 'FontSize', 20, ... 42 | 'XColor', 'k', 'YColor', 'k', 'ZColor', 'k', ... 43 | 'YGrid', 'on', 'YMinorGrid', 'on', ... 44 | 'LineWidth', 0.7, 'Box', 'off'); 45 | 46 | end 47 | -------------------------------------------------------------------------------- /collision_indicator.m: -------------------------------------------------------------------------------- 1 | function collide = collision_indicator(L1, L2, L3, xi, ro, ror, smp) 2 | %COLLISION_INDICATOR Computes the minimal distance between the sample 3 | % points and the spherical obstacles. 4 | % COLLIDE = COLLISION_INDICATOR(L1, L2, L3, XI, RO, ROR, SMP) returns the 5 | % minimal distance COLLIDE. If COLLIDE > 0, then no collision occurs. If 6 | % COLLIDE < 0, then the robot collides with obstacles. The robot is 7 | % described by the section lengths L1, L2, L3 and the overall exponential 8 | % coordinate XI. The obstacles are spheres centring at RO with radius 9 | % ROR. The sample points are distributed uniformly along the robot curve 10 | % with the number SMP. 11 | % 12 | % Example 13 | % L1 = 1; L2 = 1; L3 = 1; 14 | % xi = [-1.6; 0.8; 1.2; -0.2; 0.6; 0.2]; 15 | % ro = [0.8; 0.7; 0.6]; 16 | % ror = 0.4; 17 | % collide = collision_indicator(L1, L2, L3, xi, ro, ror, 10); 18 | 19 | k = length(ror); 20 | if k == 0 21 | collide = inf; 22 | else 23 | T1 = exphat( [xi(1); xi(2); 0; 0; 0; L1] ); 24 | T12 = T1 * exphat( [xi(3); xi(4); 0; 0; 0; L2] ); 25 | ind = nan(1, smp*3); 26 | for i = 1: smp 27 | Tt1 = exphat( [xi(1); xi(2); 0; 0; 0; L1]/smp*i ); 28 | inds = nan(1, k); 29 | for j = 1: k 30 | dr = Tt1(1:3, 4) - ro(:, j); 31 | dist = norm(dr); 32 | inds(j) = dist - ror(k); 33 | end 34 | ind(i) = min(inds); 35 | 36 | Tt2 = T1 * exphat( [xi(3); xi(4); 0; 0; 0; L2]/smp*i ); 37 | inds = nan(1, k); 38 | for j = 1: k 39 | dr = Tt2(1:3, 4) - ro(:, j); 40 | dist = norm(dr); 41 | inds(j) = dist - ror(k); 42 | end 43 | ind(smp+i) = min(inds); 44 | 45 | Tt3 = T12 * exphat( [xi(5); xi(6); 0; 0; 0; L3]/smp*i ); 46 | inds = nan(1, k); 47 | for j = 1: k 48 | dr = Tt3(1:3, 4) - ro(:, j); 49 | dist = norm(dr); 50 | inds(j) = dist - ror(k); 51 | end 52 | ind(2*smp+i) = min(inds); 53 | end 54 | collide = min(ind); 55 | end 56 | end -------------------------------------------------------------------------------- /revise_newton.m: -------------------------------------------------------------------------------- 1 | function [xi_star, err, k] = revise_newton(L1, L2, L3, q, r, xi, mstep, tol, type) 2 | %REVISE_NEWTON Correct the initial value with the Newton-Raphson method. 3 | % [XI_STAR, ERR, K] = REVISE_*(L1, L2, L3, Q, R, XI, MSTEP, TOL, TYPE) 4 | % returns the result of numerical correction. 5 | % 6 | % Methods 7 | % grad gradient method 8 | % dls damped least-squares method 9 | % newton Newton-Raphson method 10 | % 11 | % Input parameters 12 | % L1, L2, L3 section length 13 | % Q, R desired end rotation and translation 14 | % XI initial value 15 | % MSTEP allowed maximum steps of iterations 16 | % TOL error tolerance 17 | % TYPE set 'plot' to visualise the numerical correction 18 | % 19 | % Output parameters 20 | % XI_STAR final value 21 | % ERR final error 22 | % K steps of iterations 23 | % 24 | % Example 25 | % L1 = 1; L2 = 1; L3 = 1; 26 | % alpha = 15*pi/16; omega = [0.48; sqrt(3)/10; -0.86]; 27 | % q = [cos(alpha/2); sin(alpha/2)*omega]; 28 | % r = [-0.4; 1.1; 0.8]; 29 | % xi_0 = arc2xi(L1, L2, L3, pi.*[1, 2, 1, 2, 1, 2].*rand(1, 6)); 30 | % [xi, err, noi] = revise_grad(L1, L2, L3, q, r, xi_0, 2000, 1e-2, 'plot'); 31 | % [xi, err, noi] = revise_dls(L1, L2, L3, q, r, xi_0, 2000, 1e-2, 'plot'); 32 | % [xi, err, noi] = revise_newton(L1, L2, L3, q, r, xi_0, 200, 1e-2, 'plot'); 33 | 34 | Td = [q2rot(q), r; 0, 0, 0, 1]; 35 | omg_e = NaN(1, mstep); 36 | v_e = NaN(1, mstep); 37 | e = NaN(1, mstep); 38 | k = 0; 39 | while k < mstep 40 | Tt = get_end(L1, L2, L3, xi); 41 | V = up_vee(logm(Tt \ Td)); 42 | omg_e(k+1) = norm(V(1: 3)); 43 | v_e(k+1) = norm(V(4: 6)); 44 | e(k+1) = norm(V); 45 | if e(k+1) < tol 46 | break; 47 | else 48 | % Update. 49 | J = jacobian3cc(L1, L2, L3, xi); % When at zero position, rank(J) = 4. 50 | xi = xi + (J.' * J) \ J' * V; 51 | xi = arc2xi(L1, L2, L3, xi2arc(L1, L2, L3, xi)); 52 | k = k + 1; 53 | end 54 | end 55 | if k == mstep 56 | Tt = get_end(L1, L2, L3, xi); 57 | V = up_vee(logm(Tt \ Td)); 58 | omg_e(k+1) = norm(V(1: 3)); 59 | v_e(k+1) = norm(V(4: 6)); 60 | e(k+1) = norm(V); 61 | end 62 | xi_star = xi; 63 | err = e(k+1); 64 | if isequal(type, 'plot') 65 | revise_plot(0: k, omg_e(1:k+1), v_e(1:k+1), e(1: k+1), tol, 3); 66 | end 67 | end -------------------------------------------------------------------------------- /revise_grad.m: -------------------------------------------------------------------------------- 1 | function [xi_star, err, k] = revise_grad(L1, L2, L3, q, r, xi, mstep, tol, type) 2 | %REVISE_GRAD Correct the initial value with the gradient method. 3 | % [XI_STAR, ERR, K] = REVISE_*(L1, L2, L3, Q, R, XI, MSTEP, TOL, TYPE) 4 | % returns the result of numerical correction. 5 | % 6 | % Methods 7 | % grad gradient method 8 | % dls damped least-squares method 9 | % newton Newton-Raphson method 10 | % 11 | % Input parameters 12 | % L1, L2, L3 section length 13 | % Q, R desired end rotation and translation 14 | % XI initial value 15 | % MSTEP allowed maximum steps of iterations 16 | % TOL error tolerance 17 | % TYPE set 'plot' to show the error of numerical correction 18 | % 19 | % Output parameters 20 | % XI_STAR final value 21 | % ERR final error 22 | % K steps of iterations 23 | % 24 | % Example 25 | % L1 = 1; L2 = 1; L3 = 1; 26 | % alpha = 15*pi/16; omega = [0.48; sqrt(3)/10; -0.86]; 27 | % q = [cos(alpha/2); sin(alpha/2)*omega]; 28 | % r = [-0.4; 1.1; 0.8]; 29 | % xi_0 = arc2xi(L1, L2, L3, pi.*[1, 2, 1, 2, 1, 2].*rand(1, 6)); 30 | % [xi, err, noi] = revise_grad(L1, L2, L3, q, r, xi_0, 2000, 1e-2, 'plot'); 31 | % [xi, err, noi] = revise_dls(L1, L2, L3, q, r, xi_0, 2000, 1e-2, 'plot'); 32 | % [xi, err, noi] = revise_newton(L1, L2, L3, q, r, xi_0, 200, 1e-2, 'plot'); 33 | 34 | Td = [q2rot(q), r; 0, 0, 0, 1]; 35 | omg_e = NaN(1, mstep); 36 | v_e = NaN(1, mstep); 37 | e = NaN(1, mstep); 38 | k = 0; 39 | while k < mstep 40 | Tt = get_end(L1, L2, L3, xi); 41 | V = up_vee(logm(Tt \ Td)); 42 | omg_e(k+1) = norm(V(1: 3)); 43 | v_e(k+1) = norm(V(4: 6)); 44 | e(k+1) = norm(V); 45 | if e(k+1) < tol 46 | break; 47 | else 48 | % Update. 49 | J = jacobian3cc(L1, L2, L3, xi); % When at zero position, rank(J) = 4. 50 | lamd = 0.2; 51 | xi = xi + lamd * J' * V; 52 | xi = arc2xi(L1, L2, L3, xi2arc(L1, L2, L3, xi)); 53 | k = k + 1; 54 | end 55 | end 56 | if k == mstep 57 | Tt = get_end(L1, L2, L3, xi); 58 | V = up_vee(logm(Tt \ Td)); 59 | omg_e(k+1) = norm(V(1: 3)); 60 | v_e(k+1) = norm(V(4: 6)); 61 | e(k+1) = norm(V); 62 | end 63 | xi_star = xi; 64 | err = e(k+1); 65 | if isequal(type, 'plot') 66 | revise_plot(0: k, omg_e(1:k+1), v_e(1:k+1), e(1: k+1), tol, 1); 67 | end 68 | end -------------------------------------------------------------------------------- /revise_dls.m: -------------------------------------------------------------------------------- 1 | function [xi_star, err, k] = revise_dls(L1, L2, L3, q, r, xi, mstep, tol, type) 2 | %REVISE_DLS Correct the initial value with the damped least-squares method. 3 | % [XI_STAR, ERR, K] = REVISE_*(L1, L2, L3, Q, R, XI, MSTEP, TOL, TYPE) 4 | % returns the result of numerical correction. 5 | % 6 | % Methods 7 | % grad gradient method 8 | % dls damped least-squares method 9 | % newton Newton-Raphson method 10 | % 11 | % Input parameters 12 | % L1, L2, L3 section length 13 | % Q, R desired end rotation and translation 14 | % XI initial value 15 | % MSTEP allowed maximum steps of iterations 16 | % TOL error tolerance 17 | % TYPE set 'plot' to show the error of numerical correction 18 | % 19 | % Output parameters 20 | % XI_STAR final value 21 | % ERR final error 22 | % K steps of iterations 23 | % 24 | % Example 25 | % L1 = 1; L2 = 1; L3 = 1; 26 | % alpha = 15*pi/16; omega = [0.48; sqrt(3)/10; -0.86]; 27 | % q = [cos(alpha/2); sin(alpha/2)*omega]; 28 | % r = [-0.4; 1.1; 0.8]; 29 | % xi_0 = arc2xi(L1, L2, L3, pi.*[1, 2, 1, 2, 1, 2].*rand(1, 6)); 30 | % [xi, err, noi] = revise_grad(L1, L2, L3, q, r, xi_0, 2000, 1e-2, 'plot'); 31 | % [xi, err, noi] = revise_dls(L1, L2, L3, q, r, xi_0, 2000, 1e-2, 'plot'); 32 | % [xi, err, noi] = revise_newton(L1, L2, L3, q, r, xi_0, 200, 1e-2, 'plot'); 33 | 34 | Td = [q2rot(q), r; 0, 0, 0, 1]; 35 | omg_e = NaN(1, mstep); 36 | v_e = NaN(1, mstep); 37 | e = NaN(1, mstep); 38 | k = 0; 39 | while k < mstep 40 | Tt = get_end(L1, L2, L3, xi); 41 | V = up_vee(logm(Tt \ Td)); 42 | omg_e(k+1) = norm(V(1: 3)); 43 | v_e(k+1) = norm(V(4: 6)); 44 | e(k+1) = norm(V); 45 | if e(k+1) < tol 46 | break; 47 | else 48 | % Update. 49 | J = jacobian3cc(L1, L2, L3, xi); % When at zero position, rank(J) = 4. 50 | M = J.' * J; 51 | xi = xi + (M + max(diag(M)) * eye(6)) \ J' * V; 52 | xi = arc2xi(L1, L2, L3, xi2arc(L1, L2, L3, xi)); 53 | k = k + 1; 54 | end 55 | end 56 | if k == mstep 57 | Tt = get_end(L1, L2, L3, xi); 58 | V = up_vee(logm(Tt \ Td)); 59 | omg_e(k+1) = norm(V(1: 3)); 60 | v_e(k+1) = norm(V(4: 6)); 61 | e(k+1) = norm(V); 62 | end 63 | xi_star = xi; 64 | err = e(k+1); 65 | if isequal(type, 'plot') 66 | revise_plot(0: k, omg_e(1:k+1), v_e(1:k+1), e(1: k+1), tol, 2); 67 | end 68 | end -------------------------------------------------------------------------------- /frame.m: -------------------------------------------------------------------------------- 1 | function frame(fh, A, alpha) 2 | %FRAME draws a coordinate frame with specified position (and orientation) 3 | % and transparency. 4 | % 5 | % FRAME(FH, A, ALPHA) displays the plot in target figure FH. The position 6 | % (and orientation) is specified by A and the transparency is specified 7 | % by ALPHA. 8 | % 9 | % Example 10 | % frame(1, [eye(3), zeros(3, 1); 0, 0, 0, 1], 0.3); 11 | 12 | figure(fh); 13 | len = 0.2; 14 | lw = 6; 15 | new_red = [238, 0, 0]/255; 16 | new_green = [0, 238, 0]/255; 17 | new_blue = [0, 0, 238]/255; 18 | if size(A) == [4, 4] 19 | cod = [len .* eye(3); 1, 1, 1]; 20 | org = A * [0; 0; 0; 1]; 21 | cod = A * cod; 22 | plot3([org(1), cod(1, 1)], [org(2), cod(2, 1)], [org(3), cod(3, 1)], ... 23 | '-', 'LineWidth', lw, 'Color', [new_red, alpha]); 24 | hold on; 25 | plot3([org(1), cod(1, 2)], [org(2), cod(2, 2)], [org(3), cod(3, 2)], ... 26 | '-', 'LineWidth', lw, 'Color', [new_green, alpha]); 27 | plot3([org(1), cod(1, 3)], [org(2), cod(2, 3)], [org(3), cod(3, 3)], ... 28 | '-', 'LineWidth', lw, 'Color', [new_blue, alpha]); 29 | elseif size(A) == [3, 3] 30 | cod = len .* eye(3); 31 | org = [0; 0; 0]; 32 | cod = A * cod; 33 | plot3([org(1), cod(1, 1)], [org(2), cod(2, 1)], [org(3), cod(3, 1)], ... 34 | '-', 'LineWidth', lw, 'Color', [new_red, alpha]); 35 | hold on; 36 | plot3([org(1), cod(1, 2)], [org(2), cod(2, 2)], [org(3), cod(3, 2)], ... 37 | '-', 'LineWidth', lw, 'Color', [new_green, alpha]); 38 | plot3([org(1), cod(1, 3)], [org(2), cod(2, 3)], [org(3), cod(3, 3)], ... 39 | '-', 'LineWidth', lw, 'Color', [new_blue, alpha]); 40 | elseif size(A) == [2, 3] 41 | cod = len .* eye(2); 42 | org = A(:, 3); 43 | cod = org + A(:, 1:2) * cod; 44 | plot([org(1), cod(1, 1)], [org(2), cod(2, 1)], ... 45 | '-', 'LineWidth', lw, 'Color', [new_red, alpha]); 46 | hold on; 47 | plot([org(1), cod(1, 2)], [org(2), cod(2, 2)], ... 48 | '-', 'LineWidth', lw, 'Color', [new_blue, alpha]); 49 | end 50 | pos = [50, 50, 680, 600]; 51 | xlabel('$x$', 'Interpreter', 'latex'); 52 | ylabel('$y$', 'Interpreter', 'latex'); 53 | zlabel('$z$~~~~~~~~', 'Rotation', 0, 'Interpreter', 'latex'); 54 | pbaspect([1, 1, 1]); 55 | daspect([1, 1, 1]); 56 | xticks([-2, -1, 0, 1, 2]); 57 | yticks([-2, -1, 0, 1, 2]); 58 | zticks([-2, -1, 0, 1, 2, 3]); 59 | set(gcf, 'Position', pos); 60 | set(gca, 'FontName', 'Times New Roman', 'FontSize', 20, ... 61 | 'XColor', 'k', 'YColor', 'k', 'ZColor', 'k', ... 62 | 'LineWidth', 0.7); 63 | end -------------------------------------------------------------------------------- /circles3c.m: -------------------------------------------------------------------------------- 1 | function circles3c(fh, L1, L2, L3, xi, type, colour) 2 | %CIRCLES3C Visualises the 3-section constant-curvature robot with given 3 | % model parameters and a specified colour. 4 | % CIRCLES3C(FH, L1, L2, L3, XI, TYPE, COLOUR) displays the plot in target 5 | % figure FH. The robot is described by the section lengths L1, L2, L3 and 6 | % the overall exponential coordinate XI. Line styles are specified in the 7 | % character string TYPE, line colours are specified in the triple COLOUR. 8 | % 9 | % Example 10 | % L1 = 1; L2 = 1; L3 = 1; 11 | % xi_1 = [-1.60; 0.08; 1.20; -0.20; 0.60; 0.20]; 12 | % circles3c(1, L1, L2, L3, xi_1, '--', [0.1, 0.1, 0.1]); 13 | % xi_2 = [-0.39; 0.48; -1.13; 0.47; 1.79; -0.17]; 14 | % circles3c(1, L1, L2, L3, xi_2, '-', [0.2, 0.2, 0.2]); 15 | % view(75, 9); 16 | 17 | tmp = xi2arc(L1, L2, L3, xi); 18 | arc = [tmp(1: 2), L1, tmp(3: 4), L2, tmp(5: 6), L3]; 19 | Tstd = eye(4); 20 | figure(fh); 21 | frame(fh, Tstd, 1); 22 | lw = 2; 23 | ms = 11; 24 | 25 | for k = 1: length(arc)/3 26 | kp = arc((k-1)*3+1); 27 | ph = arc((k-1)*3+2); 28 | L = arc((k-1)*3+3); 29 | if kp*L < 1e-3 30 | % Line. 31 | cx = [0, 0]; 32 | cy = [0, 0]; 33 | cz = [0, L]; 34 | c1 = [1, 1]; 35 | cod = Tstd * [cx; cy; cz; c1]; 36 | plot3(cod(1, :), cod(2, :), cod(3, :), type, 'LineWidth', lw, 'Color', colour); 37 | hold on; 38 | Tstd = Tstd * expm(up_hat([0; 0; 0; 0; 0; L])); 39 | plot3(Tstd(1, 4), Tstd(2, 4), Tstd(3, 4), '.', 'MarkerSize', ms, 'Color', colour); 40 | hold on; 41 | else 42 | % Circle. 43 | cr = 1 / kp; 44 | % Arc. 45 | ax = cos(ph).*(1+cos(linspace(pi-kp*L, pi, 20))); 46 | ay = sin(ph).*(1+cos(linspace(pi-kp*L, pi, 20))); 47 | az = sin(linspace(pi-kp*L, pi, 20)); 48 | a1 = ones(size(ax)); 49 | cod = Tstd * [cr .* [ax; ay; az]; a1]; 50 | plot3(cod(1, :), cod(2, :), cod(3, :), type, 'LineWidth', lw, 'Color', colour); 51 | hold on; 52 | Tstd = Tstd * expm(up_hat([-kp*L*sin(ph); kp*L*cos(ph); 0; 0; 0; L])); 53 | plot3(Tstd(1, 4), Tstd(2, 4), Tstd(3, 4), '.', 'MarkerSize', ms, 'Color', colour); 54 | hold on; 55 | end 56 | end 57 | figure(fh); 58 | plot3(Tstd(1, 4), Tstd(2, 4), Tstd(3, 4), '.', 'MarkerSize', ms, 'Color', colour); 59 | 60 | pos = [50, 50, 680, 600]; 61 | xlabel('$x$', 'Interpreter', 'latex'); 62 | ylabel('$y$', 'Interpreter', 'latex'); 63 | zlabel('$z$~~~~~~~~', 'Rotation', 0, 'Interpreter', 'latex'); 64 | pbaspect([1, 1, 1]); 65 | daspect([1, 1, 1]); 66 | xticks([-2, -1, 0, 1, 2]); 67 | yticks([-2, -1, 0, 1, 2]); 68 | zticks([-2, -1, 0, 1, 2, 3]); 69 | set(gcf, 'Position', pos); 70 | set(gca, 'FontName', 'Times New Roman', 'FontSize', 20, ... 71 | 'XColor', 'k', 'YColor', 'k', 'ZColor', 'k', ... 72 | 'LineWidth', 0.7); 73 | end 74 | 75 | function frame(fh, A, alpha) 76 | figure(fh); 77 | len = 0.2; 78 | lw = 6; 79 | new_red = [238, 0, 0]/255; 80 | new_green = [0, 238, 0]/255; 81 | new_blue = [0, 0, 238]/255; 82 | if size(A) == [4, 4] 83 | cod = [len .* eye(3); 1, 1, 1]; 84 | org = A * [0; 0; 0; 1]; 85 | cod = A * cod; 86 | plot3([org(1), cod(1, 1)], [org(2), cod(2, 1)], [org(3), cod(3, 1)], ... 87 | '-', 'LineWidth', lw, 'Color', [new_red, alpha]); 88 | hold on; 89 | plot3([org(1), cod(1, 2)], [org(2), cod(2, 2)], [org(3), cod(3, 2)], ... 90 | '-', 'LineWidth', lw, 'Color', [new_green, alpha]); 91 | plot3([org(1), cod(1, 3)], [org(2), cod(2, 3)], [org(3), cod(3, 3)], ... 92 | '-', 'LineWidth', lw, 'Color', [new_blue, alpha]); 93 | elseif size(A) == [3, 3] 94 | cod = len .* eye(3); 95 | org = [0; 0; 0]; 96 | cod = A * cod; 97 | plot3([org(1), cod(1, 1)], [org(2), cod(2, 1)], [org(3), cod(3, 1)], ... 98 | '-', 'LineWidth', lw, 'Color', [new_red, alpha]); 99 | hold on; 100 | plot3([org(1), cod(1, 2)], [org(2), cod(2, 2)], [org(3), cod(3, 2)], ... 101 | '-', 'LineWidth', lw, 'Color', [new_green, alpha]); 102 | plot3([org(1), cod(1, 3)], [org(2), cod(2, 3)], [org(3), cod(3, 3)], ... 103 | '-', 'LineWidth', lw, 'Color', [new_blue, alpha]); 104 | elseif size(A) == [2, 3] 105 | cod = len .* eye(2); 106 | org = A(:, 3); 107 | cod = org + A(:, 1:2) * cod; 108 | plot([org(1), cod(1, 1)], [org(2), cod(2, 1)], ... 109 | '-', 'LineWidth', lw, 'Color', [new_red, alpha]); 110 | hold on; 111 | plot([org(1), cod(1, 2)], [org(2), cod(2, 2)], ... 112 | '-', 'LineWidth', lw, 'Color', [new_blue, alpha]); 113 | end 114 | end -------------------------------------------------------------------------------- /circles3.m: -------------------------------------------------------------------------------- 1 | function circles3(fh, L1, L2, L3, xi, type) 2 | %CIRCLES3 Visualises the 3-section constant-curvature robot with given 3 | % model parameters. 4 | % CIRCLES3(FH, L1, L2, L3, XI, TYPE) displays the plot in target figure 5 | % FH. The robot is described by the section lengths L1, L2, L3 and the 6 | % overall exponential coordinate XI. Line colours and styles are 7 | % specified in the character string TYPE. 8 | % 9 | % Example 10 | % L1 = 1; L2 = 1; L3 = 1; 11 | % xi_1 = [-1.60; 0.08; 1.20; -0.20; 0.60; 0.20]; 12 | % circles3(1, L1, L2, L3, xi_1, 'k--'); 13 | % xi_2 = [-0.39; 0.48; -1.13; 0.47; 1.79; -0.17]; 14 | % circles3(1, L1, L2, L3, xi_2, 'k-'); 15 | % view(75, 9); 16 | 17 | arc = xi2arc(L1, L2, L3, xi); 18 | para = [arc(1: 2), L1, arc(3: 4), L2, arc(5: 6), L3]; 19 | Tstd = eye(4); 20 | figure(fh); 21 | frame(fh, Tstd, 1); 22 | % extend = 0.2; 23 | lw = 4; 24 | ms = 22; 25 | % Use smaller line width and marker size if not clear. 26 | % lw = 2; 27 | % ms = 11; 28 | pos = [50, 50, 680, 600]; 29 | if isempty(type) 30 | type = 'k-'; 31 | end 32 | 33 | for k = 1: length(para)/3 34 | kp = para((k-1)*3+1); 35 | ph = para((k-1)*3+2); 36 | L = para((k-1)*3+3); 37 | if kp*L < 1e-3 38 | % Line. 39 | cx = [0, 0]; 40 | cy = [0, 0]; 41 | cz = [0, L]; 42 | c1 = [1, 1]; 43 | cod = Tstd * [cx; cy; cz; c1]; 44 | plot3(cod(1, :), cod(2, :), cod(3, :), type, 'LineWidth', lw); 45 | hold on; 46 | Tstd = Tstd * expm(up_hat([0; 0; 0; 0; 0; L])); 47 | plot3(Tstd(1, 4), Tstd(2, 4), Tstd(3, 4), [type(1), '.'], 'MarkerSize', ms); 48 | hold on; 49 | else 50 | % Circle. 51 | % cx = cos(ph).*(1+cos(pi-kp*L-extend*kp*L: 0.02: pi+extend*kp*L)); 52 | % cy = sin(ph).*(1+cos(pi-kp*L-extend*kp*L: 0.02: pi+extend*kp*L)); 53 | % cz = sin(pi-kp*L-extend*kp*L: 0.02: pi+extend*kp*L); 54 | % c1 = ones(size(cx)); 55 | % cr = 1 / kp; 56 | % cod = Tstd * [cr .* [cx; cy; cz]; c1]; 57 | % plot3(cod(1, :), cod(2, :), cod(3, :), type); 58 | % hold on; 59 | 60 | % Arc. 61 | cr = 1 / kp; 62 | ax = cos(ph).*(1+cos(linspace(pi-kp*L, pi, 20))); 63 | ay = sin(ph).*(1+cos(linspace(pi-kp*L, pi, 20))); 64 | az = sin(linspace(pi-kp*L, pi, 20)); 65 | a1 = ones(size(ax)); 66 | cod = Tstd * [cr .* [ax; ay; az]; a1]; 67 | plot3(cod(1, :), cod(2, :), cod(3, :), type, 'LineWidth', lw); 68 | hold on; 69 | Tstd = Tstd * expm(up_hat([-kp*L*sin(ph); kp*L*cos(ph); 0; 0; 0; L])); 70 | plot3(Tstd(1, 4), Tstd(2, 4), Tstd(3, 4), [type(1), '.'], 'MarkerSize', ms); 71 | hold on; 72 | end 73 | % frame(fh, Tstd, 0.8); 74 | end 75 | figure(fh); 76 | frame(fh, Tstd, 0.8); % The end frame. 77 | plot3(Tstd(1, 4), Tstd(2, 4), Tstd(3, 4), 'k.', 'MarkerSize', ms); 78 | xlabel('$x$', 'Interpreter', 'latex'); 79 | ylabel('$y$', 'Interpreter', 'latex'); 80 | zlabel('$z$~~~~~~~~', 'Rotation', 0, 'Interpreter', 'latex'); 81 | pbaspect([1, 1, 1]); 82 | daspect([1, 1, 1]); 83 | xticks([-2, -1, 0, 1, 2]); 84 | yticks([-2, -1, 0, 1, 2]); 85 | zticks([-2, -1, 0, 1, 2, 3]); 86 | set(gcf, 'Position', pos); 87 | set(gca, 'FontName', 'Times New Roman', 'FontSize', 20, ... 88 | 'XColor', 'k', 'YColor', 'k', 'ZColor', 'k', ... 89 | 'LineWidth', 0.7); 90 | end 91 | 92 | function frame(fh, A, alpha) 93 | figure(fh); 94 | len = 0.2; 95 | lw = 6; 96 | new_red = [238, 0, 0]/255; 97 | new_green = [0, 238, 0]/255; 98 | new_blue = [0, 0, 238]/255; 99 | if size(A) == [4, 4] 100 | cod = [len .* eye(3); 1, 1, 1]; 101 | org = A * [0; 0; 0; 1]; 102 | cod = A * cod; 103 | plot3([org(1), cod(1, 1)], [org(2), cod(2, 1)], [org(3), cod(3, 1)], ... 104 | '-', 'LineWidth', lw, 'Color', [new_red, alpha]); 105 | hold on; 106 | plot3([org(1), cod(1, 2)], [org(2), cod(2, 2)], [org(3), cod(3, 2)], ... 107 | '-', 'LineWidth', lw, 'Color', [new_green, alpha]); 108 | plot3([org(1), cod(1, 3)], [org(2), cod(2, 3)], [org(3), cod(3, 3)], ... 109 | '-', 'LineWidth', lw, 'Color', [new_blue, alpha]); 110 | elseif size(A) == [3, 3] 111 | cod = len .* eye(3); 112 | org = [0; 0; 0]; 113 | cod = A * cod; 114 | plot3([org(1), cod(1, 1)], [org(2), cod(2, 1)], [org(3), cod(3, 1)], ... 115 | '-', 'LineWidth', lw, 'Color', [new_red, alpha]); 116 | hold on; 117 | plot3([org(1), cod(1, 2)], [org(2), cod(2, 2)], [org(3), cod(3, 2)], ... 118 | '-', 'LineWidth', lw, 'Color', [new_green, alpha]); 119 | plot3([org(1), cod(1, 3)], [org(2), cod(2, 3)], [org(3), cod(3, 3)], ... 120 | '-', 'LineWidth', lw, 'Color', [new_blue, alpha]); 121 | elseif size(A) == [2, 3] 122 | cod = len .* eye(2); 123 | org = A(:, 3); 124 | cod = org + A(:, 1:2) * cod; 125 | plot([org(1), cod(1, 1)], [org(2), cod(2, 1)], ... 126 | '-', 'LineWidth', lw, 'Color', [new_red, alpha]); 127 | hold on; 128 | plot([org(1), cod(1, 2)], [org(2), cod(2, 2)], ... 129 | '-', 'LineWidth', lw, 'Color', [new_blue, alpha]); 130 | end 131 | end -------------------------------------------------------------------------------- /micsolver.m: -------------------------------------------------------------------------------- 1 | function [nos, noi] = micsolver(L1, L2, L3, q, r, tol, is_a_sol) 2 | %MICSOLVER Multi-solution solver for the inverse kinematics of 3-section 3 | % constant-curvature robots. 4 | % [NOS, NOI] = MICSOLVER(L1, L2, L3, Q, R, TOL, IS_A_SOL) returns the 5 | % result of solving the 3-section inverse kinematics problem. The 6 | % function uses preset resolutions or numerical methods to address the 7 | % inverse kinematics problem. The function exits after one solution is 8 | % found. 9 | % 10 | % Input parameters 11 | % L1, L2, L3 section length 12 | % Q, R desired end rotation and translation 13 | % TOL error tolerance 14 | % IS_A_SOL function handle 15 | % This function is used to judge if the given parameter is a 16 | % solution to the inverse kinematics problem. The function has 17 | % two inputs (ERR, XI) and one output in boolean type. 18 | % 19 | % Output parameters 20 | % NOS number of solutions 21 | % NOI number of iterations in numerical correction 22 | % 23 | % Example 24 | % L1 = 1; L2 = 1; L3 = 1; 25 | % xi = arc2xi(L1, L2, L3, pi.*[1,2,1,2,1,2].*rand(1, 6)); 26 | % T = get_end(L1, L2, L3, xi); 27 | % q = rot2q(T(1:3, 1:3)); 28 | % r = T(1:3, 4); 29 | % tol = 1e-2; fun = @(e, x) e < tol; 30 | % tic; 31 | % [nos, ~] = micsolver(L1, L2, L3, q, r, tol, fun); 32 | % rt = toc*1000; 33 | % if nos 34 | % fprintf('A solution is found in %.2f ms.\n', rt); 35 | % end 36 | 37 | noi = 0; % Number of iterations. 38 | nos = 0; % Number of solutions. 39 | solns = micsolver__(L1, L2, L3, q, r, 0.03, [1, 2]); 40 | for soln = solns 41 | [xi, err, cnt] = revise_newton(L1, L2, L3, q, r, soln2xi(L1, L2, L3, soln), 6, tol, 'noplot'); 42 | noi = noi + cnt; 43 | if is_a_sol(err, xi) % A solution is found. 44 | nos = nos + 1; 45 | break; % Only one solution is needed, use this line. 46 | end 47 | end 48 | if nos == 0 % Can't find solutions in coarser resolution, use finer resolution. 49 | solns = micsolver__(L1, L2, L3, q, r, 0.01, [5, 2]); 50 | for soln = solns 51 | [xi, err, cnt] = revise_newton(L1, L2, L3, q, r, soln2xi(L1, L2, L3, soln), 6, tol, 'noplot'); 52 | noi = noi + cnt; 53 | if is_a_sol(err, xi) % A solution is found. 54 | nos = nos + 1; 55 | break; 56 | end 57 | end 58 | end 59 | if nos == 0 % Can't find solutions in coarser resolution, use finer resolution. 60 | solns = micsolver__(L1, L2, L3, q, r, 0.001, [5, 5]); 61 | for soln = solns 62 | [xi, err, cnt] = revise_newton(L1, L2, L3, q, r, soln2xi(L1, L2, L3, soln), 4, tol, 'noplot'); 63 | noi = noi + cnt; 64 | if is_a_sol(err, xi) % A solution is found. 65 | nos = nos + 1; 66 | break; 67 | end 68 | end 69 | end 70 | if nos == 0 % Use damped least-squares method. 71 | for soln = solns 72 | [xi, err, cnt] = revise_dls(L1, L2, L3, q, r, soln2xi(L1, L2, L3, soln), 200, tol, 'noplot'); 73 | noi = noi + cnt; 74 | if is_a_sol(err, xi) % A solution is found. 75 | nos = nos + 1; 76 | break; 77 | end 78 | end 79 | end 80 | end 81 | 82 | function solns = micsolver__(L1, L2, L3, q, r, par, noc) 83 | a = q(1); b = q(2); c = q(3); d = q(4); 84 | B = [d, a, b; -a, d, c; -b, -c, d]; 85 | n0 = B.' * r; 86 | n = n0 / norm(n0); 87 | r0 = (1/2 + 1/pi) .* (L3 * d) / norm(n0) .* n; % I prefer gamma = 1/2+1/pi. 88 | 89 | norm_r0 = norm(r0); 90 | norm_r01 = sqrt(1 - norm_r0^2); 91 | 92 | u = [n(2); -n(1); 0] / norm([n(2); -n(1); 0]); 93 | v = cross(n, u); 94 | P = [u, v, n]; 95 | 96 | zeta = 0: par: 1; 97 | npar = length(zeta); 98 | err = nan(1, npar); 99 | err_r = nan(1, npar); 100 | err_t = nan(1, npar); 101 | solns = nan(9, npar); 102 | is = nan(1, npar); 103 | nts = 0; 104 | for i = 1: npar 105 | t = zeta(i); 106 | r3 = r0 + norm_r01 .* (P * [sin(2*pi*t); cos(2*pi*t); 0]); 107 | 108 | if d ~= 0 % When d==0, the solution is exact, we do not need corrections. 109 | for cor_idx = 1: noc(1) 110 | % One-step correction. 111 | tmp = r3 - (n0.' * r3 - rho(r3(3), L3) * d) / ((n0.' + [0, 0, L3*d*(1/acos(r3(3)) - 1/sqrt(1-r3(3)^2))]) * r0) * r0; 112 | r3 = tmp / norm(tmp); 113 | end 114 | end 115 | 116 | r1 = solve_r1(L1, q, r, r3, noc(2)); 117 | [r2r, r2t] = solve_r2(L1, L3, q, r, r3, r1); 118 | 119 | e_r = get_err(r1, r2r, r3, L1, L2, L3, q, r); 120 | e_t = get_err(r1, r2t, r3, L1, L2, L3, q, r); 121 | 122 | if e_r < e_t 123 | r2 = r2r; 124 | e = e_r; 125 | else 126 | r2 = r2t; 127 | e = e_t; 128 | end 129 | 130 | if i == 1 131 | % No operation here. 132 | elseif i == 2 133 | nts = nts + 1; 134 | solns(:, nts) = soln; 135 | is(nts) = 1; 136 | else % i > 2 137 | if e > err(i-1) && err(i-1) <= err(i-2) % Local minimum. 138 | nts = nts + 1; 139 | solns(:, nts) = soln; 140 | is(nts) = i-1; 141 | end 142 | end 143 | soln = [r1; r2; r3]; 144 | err(i) = e; 145 | err_r(i) = e_r; 146 | err_t(i) = e_t; 147 | end 148 | 149 | if zeta(i) ~= 1 150 | % Check if the last point is a local minimum. 151 | if err(1) > err(i) && err(i) <= err(i-1) % Yes, it is. 152 | nts = nts + 1; 153 | solns(:, nts) = soln; 154 | is(nts) = i; 155 | end 156 | % Check if the first point is a local minimum. 157 | if err(2) > err(1) && err(1) <= err(i) % Yes, it is. 158 | solns = solns(:, 1:nts); 159 | is = is(1:nts); 160 | else 161 | solns = solns(:, 2:nts); 162 | is = is(2:nts); 163 | end 164 | else % The last point coincide with the first. 165 | % Check if the first (also the last) point is a local minimum. 166 | if err(2) > err(1) && err(1) <= err(i-1) % Yes, it is. 167 | solns = solns(:, 1:nts); 168 | is = is(1:nts); 169 | else 170 | solns = solns(:, 2:nts); 171 | is = is(2:nts); 172 | end 173 | end 174 | 175 | ts = zeta(is); 176 | [~, idx] = sort(abs(ts - 0.5)); 177 | solns = solns(:, idx); 178 | end 179 | -------------------------------------------------------------------------------- /main_demo2.m: -------------------------------------------------------------------------------- 1 | clear; 2 | clc; 3 | close all; 4 | 5 | % Robot section lengths. 6 | L1 = 1; 7 | L2 = 1; 8 | L3 = 1; 9 | 10 | % Generate a path. 11 | smp = 30; % Number of sample points. 12 | tolerance = 6e-4; 13 | R0 = rot([1;0;0], pi/3); 14 | h = 2.7; 15 | yp = -0.2; 16 | [x, y, z] = generate_path([-0.5; yp; h], R0, [1; 1; 0], smp); 17 | rs = [x; y; z]; 18 | qs = nan(4, smp); 19 | for smp_idx = 1: smp 20 | qs(:, smp_idx) = rot2q(R0); 21 | end 22 | 23 | % Solve the problem. 24 | n_success = nan(smp, 1); 25 | t_runtime = nan(smp, 1); 26 | n_iteration = nan(smp, 1); 27 | is_a_sol = @(e, x) e < tolerance; 28 | rvsd_solns = cell(1, length(x)); 29 | for smp_idx = 1: smp 30 | r = rs(:, smp_idx); 31 | q = qs(:, smp_idx); 32 | tic; 33 | [sol, nos, noi] = micsolverd(L1, L2, L3, q, r, 0.03, [5, 5], tolerance, 5, is_a_sol, 'noplot', 'noplot'); 34 | t_runtime(smp_idx) = toc; 35 | n_success(smp_idx) = nos; 36 | n_iteration(smp_idx) = noi; 37 | 38 | rvsd_solns{smp_idx} = sol; 39 | if n_success(smp_idx) == 0 40 | fprintf('Sample: %d has no solution.\n', smp_idx); 41 | end 42 | end 43 | fprintf('##### INVERSE KINEMATICS #####\n\n'); 44 | fprintf('Runtime (ms): '); disp(1000*sum(t_runtime)); 45 | 46 | % Distance planning. 47 | tic; 48 | [path, cost] = dp(rvsd_solns, @lossfcn); 49 | runtime_dp = toc; 50 | fprintf('##### DISTANCE PLANNING #####\n\n'); 51 | fprintf('Number of Cases: '); disp(size(path, 1)); 52 | fprintf('Cost: '); disp(cost.'); 53 | fprintf('Runtime (ms): '); disp(1000 * runtime_dp); 54 | 55 | % Time allocation. 56 | chs = path{1}; 57 | xis = nan(smp, 6); 58 | for smp_idx = 1: smp 59 | sol = rvsd_solns{smp_idx}; 60 | xis(smp_idx, :) = sol(:, chs(smp_idx)).'; 61 | end 62 | tic; 63 | ts = allocate_time(xis.'); 64 | runtime_alloc = toc; 65 | fprintf('##### TIME ALLOCATION #####\n\n'); 66 | fprintf('Runtime (ms): '); disp(1000 * runtime_alloc); 67 | 68 | % Visualisation. 69 | c_begin = [238, 0, 255]/255; 70 | c_end = [0, 238, 255]/255; 71 | cmap = [linspace(c_begin(1), c_end(1), smp).', ... 72 | linspace(c_begin(2), c_end(2), smp).', ... 73 | linspace(c_begin(3), c_end(3), smp).']; 74 | calpha = linspace(1, 1, smp); 75 | 76 | % Case 1. 77 | fh = figure(1); 78 | subplot(1, 2, 1); 79 | chs = path{1}; 80 | plot3([-1, 0.8, -1, 0.8], ones(1, 4)*yp, ones(1, 4)*h, '-', 'Color', [ones(1, 3)*0.5, 0.2], 'LineWidth', 10); 81 | hold on; 82 | for smp_idx = ceil(sqrt(1: 140: smp^2)) 83 | colour = [cmap(smp_idx, :), calpha(smp_idx)]; 84 | sol = rvsd_solns{smp_idx}; 85 | xi = sol(:, chs(smp_idx)); 86 | circles3c(fh, L1, L2, L3, xi, '-', [0.1, 0.1, 0.1]); 87 | for i = 1: size(sol, 2) 88 | if i ~= chs(smp_idx) 89 | circles3c(fh, L1, L2, L3, sol(:, i), '-', [0.8, 0.8, 0.8, 0.4]); 90 | end 91 | end 92 | frame(fh, [R0, rs(:, smp_idx); 0, 0, 0, 1], colour(4)); 93 | end 94 | pbaspect([1, 1, 1]); 95 | daspect([1, 1, 1]); 96 | xlabel('$x$', 'Interpreter', 'latex'); 97 | ylabel('$y$', 'Interpreter', 'latex'); 98 | zlabel('$z$~~~~~~~', 'Rotation', 0, 'Interpreter', 'latex'); 99 | view(-52, 15); 100 | xlim([-1.55, 1.35]); 101 | xticks([-1, 0, 1]); 102 | ylim([-1.5, 1.4]); 103 | yticks([-1, 0, 1]); 104 | zlim([-0.1, 3.1]); 105 | zticks([0, 1, 2, 3]); 106 | set(gca, 'LineWidth', 1.2, 'TickLength', [0.006, 0.01], ... 107 | 'Box', 'off', 'XColor', 'k', 'YColor', 'k', 'ZColor', 'k', 'Color', 'w', ... 108 | 'FontName', 'Times New Roman', 'FontSize', 16); 109 | 110 | subplot(1, 2, 2); 111 | act_len = xi2len(xis.'); 112 | intv = ts(1): 0.01: ts(end); 113 | for i = 1: 9 114 | plot(intv, spline(ts, act_len(i,:), intv), '-', 'LineWidth', 2); 115 | hold on; 116 | end 117 | xlim([ts(1), ts(end)]); 118 | xlabel('Time (s)'); 119 | ylim([-50, 50]); 120 | ylabel('Actuation (mm)'); 121 | axis square; 122 | set(gca, 'LineWidth', 1.2, 'TickLength', [0.006, 0.01], ... 123 | 'Box', 'off', 'XColor', 'k', 'YColor', 'k', 'Color', 'w', ... 124 | 'FontName', 'Times New Roman', 'FontSize', 16); 125 | 126 | set(gcf, 'Position', [100, 100, 800, 300]); 127 | 128 | % Case 2. 129 | fh = figure(2); 130 | subplot(1, 2, 1); 131 | chs = path{2}; 132 | plot3([-1, 0.8, -1, 0.8], ones(1, 4)*yp, ones(1, 4)*h, '-', 'Color', [ones(1, 3)*0.5, 0.2], 'LineWidth', 10); 133 | hold on; 134 | for smp_idx = ceil(sqrt(1: 140: smp^2)) 135 | colour = [cmap(smp_idx, :), calpha(smp_idx)]; 136 | sol = rvsd_solns{smp_idx}; 137 | xi = sol(:, chs(smp_idx)); 138 | circles3c(fh, L1, L2, L3, xi, '-', [0.1, 0.1, 0.1]); 139 | for i = 1: size(sol, 2) 140 | if i ~= chs(smp_idx) 141 | circles3c(fh, L1, L2, L3, sol(:, i), '-', [0.8, 0.8, 0.8, 0.4]); 142 | end 143 | end 144 | frame(fh, [R0, rs(:, smp_idx); 0, 0, 0, 1], colour(4)); 145 | end 146 | pbaspect([1, 1, 1]); 147 | daspect([1, 1, 1]); 148 | xlabel('$x$', 'Interpreter', 'latex'); 149 | ylabel('$y$', 'Interpreter', 'latex'); 150 | zlabel('$z$~~~~~~~', 'Rotation', 0, 'Interpreter', 'latex'); 151 | view(-52, 15); 152 | xlim([-1.55, 1.35]); 153 | xticks([-1, 0, 1]); 154 | ylim([-1.5, 1.4]); 155 | yticks([-1, 0, 1]); 156 | zlim([-0.1, 3.1]); 157 | zticks([0, 1, 2, 3]); 158 | set(gca, 'LineWidth', 1.2, 'TickLength', [0.006, 0.01], ... 159 | 'Box', 'off', 'XColor', 'k', 'YColor', 'k', 'ZColor', 'k', 'Color', 'w', ... 160 | 'FontName', 'Times New Roman', 'FontSize', 16); 161 | 162 | subplot(1, 2, 2); 163 | chs = path{2}; 164 | xis = nan(smp, 6); 165 | for smp_idx = 1: smp 166 | sol = rvsd_solns{smp_idx}; 167 | xis(smp_idx, :) = sol(:, chs(smp_idx)).'; 168 | end 169 | ts = allocate_time(xis.'); 170 | act_len = xi2len(xis.'); 171 | intv = ts(1): 0.01: ts(end); 172 | for i = 1: 9 173 | plot(intv, spline(ts, act_len(i,:), intv), '-', 'LineWidth', 2); 174 | hold on; 175 | end 176 | xlim([ts(1), ts(end)]); 177 | xlabel('Time (s)'); 178 | ylim([-50, 50]); 179 | ylabel('Actuation (mm)'); 180 | axis square; 181 | set(gca, 'LineWidth', 1.2, 'TickLength', [0.006, 0.01], ... 182 | 'Box', 'off', 'XColor', 'k', 'YColor', 'k', 'Color', 'w', ... 183 | 'FontName', 'Times New Roman', 'FontSize', 16); 184 | 185 | set(gcf, 'Position', [200, 200, 800, 300]); 186 | 187 | % Loss function for planning. 188 | function loss = lossfcn(xi1, xi2) 189 | loss = norm(xi2len(xi1) - xi2len(xi2)); 190 | end 191 | 192 | % Generate a straight line path. 193 | function [x, y, z] = generate_path(centre, orientation, scale, sample) 194 | x0 = [linspace(0, 1, sample); zeros(1, sample); zeros(1, sample)]; 195 | x1 = centre + orientation * (scale .* x0); 196 | x = x1(1, :); 197 | y = x1(2, :); 198 | z = x1(3, :); 199 | end 200 | 201 | % Rotation matrix from exponential coordiante. 202 | function mat = rot(axis, angle) 203 | mat = expm(up_hat(axis) * angle); 204 | end -------------------------------------------------------------------------------- /micsolverd.m: -------------------------------------------------------------------------------- 1 | function [sol, nos, noi] = micsolverd(L1, L2, L3, q, r, par, noc, tol, mstep, is_a_sol, plot_ec, plot_it) 2 | %MICSOLVERD Multi-solution solver (debug) for the inverse kinematics of 3 | % 3-section constant-curvature robots. 4 | % [SOL, NOS, NOI] = MICSOLVERD(L1, L2, L3, Q, R, ... 5 | % PAR, NOC, TOL, MSTEP, IS_A_SOL, PLOT_EC, PLOT_IT) returns the result 6 | % of solving the 3-section inverse kinematics problem. 7 | % 8 | % Input parameters 9 | % L1, L2, L3 section length 10 | % Q, R desired end rotation and translation 11 | % PAR length of the partition 12 | % Scalar. The interval [0, 1) is partitioned into subintervals 13 | % with length PAR. A smaller PAR means finer resolution. We 14 | % recommend PAR = 0.03 for better efficiency and PAR = 0.01 for 15 | % more solutions. 16 | % NOC number of corrections 17 | % A 1-by-2 array. The model parameters hat r_3 and hat r_1 are 18 | % corrected for NOC(1) and NOC(2) times after the approximation, 19 | % respectively. Large NOC provides closer initial value and more 20 | % computations. We recommend NOC = [1, 2] for better efficiency 21 | % and NOC = [5, 5] for better estimation. 22 | % TOL error tolerance 23 | % MSTEP allowed maximum steps of iterations 24 | % IS_A_SOL function handle 25 | % This function is used to judge if the given parameter is a 26 | % solution to the inverse kinematics problem. The function has 27 | % two inputs (ERR, XI) and one output in boolean type. 28 | % PLOT_EC set 'plot' to visualise the traversal 29 | % PLOT_IT set 'plot' to visualise the numerical correction 30 | % 31 | % Output parameters 32 | % SOL solutions 33 | % A 6-by-NOS array. Each column is the overall exponential 34 | % coordinate XI. 35 | % NOS number of solutions 36 | % NOI number of iterations in numerical correction 37 | % 38 | % Example 39 | % L1 = 1; L2 = 1; L3 = 1; 40 | % alpha = 15*pi/16; omega = [0.48; sqrt(3)/10; -0.86]; 41 | % q = [cos(alpha/2); sin(alpha/2)*omega]; 42 | % r = [-0.4; 1.1; 0.8]; 43 | % tol = 1e-2; fun = @(e, x) e < tol; 44 | % [sol, ~, ~] = micsolverd(L1, L2, L3, q, r, ... 45 | % 0.01, [5, 5], tol, 10, fun, ... 46 | % 'plot', 'plot'); 47 | % for eta = 1: size(sol, 2) 48 | % fh = figure(); 49 | % circles3(fh, L1, L2, L3, sol(:, eta), 'k-'); 50 | % view(119, 20); 51 | % end 52 | 53 | noi = 0; % Number of iterations. 54 | nos = 0; % Number of solutions. 55 | solns = micsolver__(L1, L2, L3, q, r, par, noc, plot_ec); 56 | sol = zeros(6, size(solns, 2)); 57 | for soln = solns 58 | [xi, err, cnt] = revise_newton(L1, L2, L3, q, r, soln2xi(L1, L2, L3, soln), mstep, tol, plot_it); 59 | noi = noi + cnt; 60 | if is_a_sol(err, xi) % A solution is found. 61 | nos = nos + 1; 62 | sol(:, nos) = xi; 63 | end 64 | end 65 | sol = sol(:, 1:nos); 66 | end 67 | 68 | 69 | 70 | function solns = micsolver__(L1, L2, L3, q, r, par, noc, type) 71 | a = q(1); b = q(2); c = q(3); d = q(4); 72 | B = [d, a, b; -a, d, c; -b, -c, d]; 73 | n0 = B.' * r; 74 | n = n0 / norm(n0); 75 | r0 = (1/2 + 1/pi) .* (L3 * d) / norm(n0) .* n; % I prefer gamma = 1/2+1/pi. 76 | 77 | norm_r0 = norm(r0); 78 | norm_r01 = sqrt(1 - norm_r0^2); 79 | 80 | u = [n(2); -n(1); 0] / norm([n(2); -n(1); 0]); 81 | v = cross(n, u); 82 | P = [u, v, n]; 83 | 84 | zeta = 0: par: 1; 85 | npar = length(zeta); 86 | err = nan(1, npar); 87 | err_r = nan(1, npar); 88 | err_t = nan(1, npar); 89 | solns = nan(9, npar); 90 | is = nan(1, npar); 91 | nts = 0; 92 | for i = 1: npar 93 | t = zeta(i); 94 | r3 = r0 + norm_r01 .* (P * [sin(2*pi*t); cos(2*pi*t); 0]); 95 | 96 | if d ~= 0 % When d==0, the solution is exact, we do not need corrections. 97 | for cor_idx = 1: noc(1) 98 | % One-step correction. 99 | tmp = r3 - (n0.' * r3 - rho(r3(3), L3) * d) / ((n0.' + [0, 0, L3*d*(1/acos(r3(3)) - 1/sqrt(1-r3(3)^2))]) * r0) * r0; 100 | r3 = tmp / norm(tmp); 101 | end 102 | end 103 | 104 | r1 = solve_r1(L1, q, r, r3, noc(2)); 105 | [r2r, r2t] = solve_r2(L1, L3, q, r, r3, r1); 106 | 107 | e_r = get_err(r1, r2r, r3, L1, L2, L3, q, r); 108 | e_t = get_err(r1, r2t, r3, L1, L2, L3, q, r); 109 | 110 | if e_r < e_t 111 | r2 = r2r; 112 | e = e_r; 113 | else 114 | r2 = r2t; 115 | e = e_t; 116 | end 117 | 118 | if i == 1 119 | % No operation here. 120 | elseif i == 2 121 | nts = nts + 1; 122 | solns(:, nts) = soln; 123 | is(nts) = 1; 124 | else % i > 2 125 | if e > err(i-1) && err(i-1) <= err(i-2) % Local minimum. 126 | nts = nts + 1; 127 | solns(:, nts) = soln; 128 | is(nts) = i-1; 129 | end 130 | end 131 | soln = [r1; r2; r3]; 132 | err(i) = e; 133 | err_r(i) = e_r; 134 | err_t(i) = e_t; 135 | end 136 | 137 | if zeta(i) ~= 1 138 | % Check if the last point is a local minimum. 139 | if err(1) > err(i) && err(i) <= err(i-1) % Yes, it is. 140 | nts = nts + 1; 141 | solns(:, nts) = soln; 142 | is(nts) = i; 143 | end 144 | % Check if the first point is a local minimum. 145 | if err(2) > err(1) && err(1) <= err(i) % Yes, it is. 146 | solns = solns(:, 1:nts); 147 | is = is(1:nts); 148 | else 149 | solns = solns(:, 2:nts); 150 | is = is(2:nts); 151 | end 152 | else % The last point coincide with the first. 153 | % Check if the first (also the last) point is a local minimum. 154 | if err(2) > err(1) && err(1) <= err(i-1) % Yes, it is. 155 | solns = solns(:, 1:nts); 156 | is = is(1:nts); 157 | else 158 | solns = solns(:, 2:nts); 159 | is = is(2:nts); 160 | end 161 | end 162 | 163 | ts = zeta(is); 164 | [~, idx] = sort(abs(ts - 0.5)); 165 | solns = solns(:, idx); 166 | if isequal(type, 'plot') 167 | fprintf('Candidate $t$ includes: '); 168 | disp(ts(idx)); 169 | traverse_plot(norm_r01, zeta, err_r, err_t, err, is); 170 | traverse_plot2(r0, norm_r01, P, zeta, err_r, err_t, err, is) 171 | end 172 | 173 | end 174 | 175 | function traverse_plot2(r0, norm_r01, P, zeta, err_r, err_t, err, is) 176 | lwb = 4; lwt = 2; % Line width for error curves. 177 | lwb2 = 3; lwt2 = 2; % Line width for legends. 178 | ms = 26; lwbox = 2; % Box of equilibria. 179 | new_red = [238, 0, 0]/255; 180 | new_blue = [0, 0, 238]/255; 181 | 182 | figure(221); 183 | err_scale = -1/max(max(err_r), max(err_t)); 184 | 185 | radius = 1; 186 | data = [radius.*sin(2*pi*zeta); radius.*cos(2*pi*zeta); 0.*zeta]; 187 | data = r0 + norm_r01 .* (P * data); 188 | plot3(data(1, :), data(2, :), data(3, :), 'k-', 'LineWidth', lwt); 189 | hold on; 190 | 191 | data = [radius.*sin(2*pi*zeta); radius.*cos(2*pi*zeta); err_r*err_scale]; 192 | data = r0 + norm_r01 .* (P * data); 193 | plot3(data(1, :), data(2, :), data(3, :), '--', 'LineWidth', lwt, 'Color', new_red); 194 | 195 | data = [radius.*sin(2*pi*zeta); radius.*cos(2*pi*zeta); err_t*err_scale]; 196 | data = r0 + norm_r01 .* (P * data); 197 | plot3(data(1, :), data(2, :), data(3, :), '--', 'LineWidth', lwt, 'Color', new_blue); 198 | 199 | data = [radius.*sin(2*pi*zeta); radius.*cos(2*pi*zeta); err*err_scale]; 200 | data = r0 + norm_r01 .* (P * data); 201 | plot3(data(1, :), data(2, :), data(3, :), 'k--', 'LineWidth', lwb); 202 | 203 | for i = is 204 | t = zeta(i); 205 | data = [radius.*sin(2*pi*(t)); radius.*cos(2*pi*(t)); err(i)*err_scale]; 206 | data = r0 + norm_r01 .* (P * data); 207 | plot3(data(1, :), data(2, :), data(3, :), ... 208 | 'ks', 'MarkerSize', 12, 'LineWidth', 2); 209 | end 210 | [X,Y,Z] = sphere(256); surf(X, Y, Z, (Z+1)/2, 'LineStyle', 'none', 'FaceAlpha', 0.7); 211 | colormap(linspace(0.4, 1, 256).'.*ones(256, 3)); 212 | xlabel('$c_3$', 'Interpreter', 'latex'); 213 | ylabel('$-b_3$', 'Interpreter', 'latex'); 214 | zlabel('$a_3$~~~~~~~~', 'Rotation', 0, 'Interpreter', 'latex'); 215 | pbaspect([1, 1, 1]); 216 | daspect([1, 1, 1]); 217 | xlim([-1.5, 1.5]); 218 | ylim([-1.5, 1.5]); 219 | zlim([-1.5, 1.5]); 220 | xticks([-1, 0, 1]); 221 | yticks([-1, 0, 1]); 222 | zticks([-1, 0, 1]); 223 | view(-60, 20); 224 | set(gcf, 'Position', [50, 50, 680, 600]); 225 | set(gca, 'XColor', 'k', 'YColor', 'k', 'ZColor', 'k', ... 226 | 'LineWidth', 0.7, 'FontName', 'Times New Roman', 'FontSize', 24); 227 | end 228 | 229 | function traverse_plot(radius, zeta, err_r, err_t, err, is) 230 | lwb = 4; lwt = 2.5; % Line width for error curves. 231 | lwb2 = 3; lwt2 = 2.5; % Line width for legends. 232 | ms = 26; lwbox = 2.5; % Box of equilibria. 233 | new_red = [238, 0, 0]/255; 234 | new_blue = [0, 0, 238]/255; 235 | % new_blue = [14,65,156]/255; 236 | % new_red = [204,26,26]/255; 237 | 238 | figure(220); 239 | err_scale = 1/max(max(err_r), max(err_t)); 240 | zoff = sqrt(1-radius^2); 241 | plot3(radius.*sin(2*pi*zeta), radius.*cos(2*pi*zeta), 0.*zeta+zoff, 'k-', 'LineWidth', lwt); 242 | hold on; 243 | plot3(radius.*sin(2*pi*zeta), radius.*cos(2*pi*zeta), err_r*err_scale+zoff, '--', 'LineWidth', lwt, 'Color', new_red); 244 | plot3(radius.*sin(2*pi*zeta), radius.*cos(2*pi*zeta), err_t*err_scale+zoff, '--', 'LineWidth', lwt, 'Color', new_blue); 245 | plot3(radius.*sin(2*pi*zeta), radius.*cos(2*pi*zeta), err*err_scale+zoff, 'k--', 'LineWidth', lwb); 246 | for i = is 247 | t = zeta(i); 248 | plot3(radius.*sin(2*pi*(t)), radius.*cos(2*pi*(t)), err(i)*err_scale+zoff, ... 249 | 'ks', 'MarkerSize', 12, 'LineWidth', 2); 250 | end 251 | % [X,Y,Z] = sphere(256); surf(X, Y, Z, (Z+1)/2, 'LineStyle', 'none', 'FaceAlpha', 0.7); 252 | % colormap(linspace(0.4, 1, 256).'.*ones(256, 3)); 253 | pbaspect([1, 1, 1]); 254 | daspect([1, 1, 1]); 255 | xlim([-1.2, 1.2]); 256 | ylim([-1.2, 1.2]); 257 | zlim([-0.4, 2]); 258 | xticks([]); 259 | yticks([]); 260 | zticks([]); 261 | set(gcf, 'Position', [50, 50, 680, 600]); 262 | set(gca, 'XColor', 'k', 'YColor', 'k', 'ZColor', 'k', ... 263 | 'LineWidth', 0.7, 'FontName', 'Times New Roman', 'FontSize', 24); 264 | 265 | 266 | 267 | figure(219); 268 | plot(zeta, err_r, '--', 'LineWidth', lwt, 'Color', new_red); 269 | hold on; 270 | plot(zeta, err_t, '--', 'LineWidth', lwt, 'Color', new_blue); 271 | plot(zeta, err, 'k--', 'LineWidth', lwb); 272 | for i = is 273 | t = zeta(i); 274 | plot(t, err(i), 'ks', 'MarkerSize', ms, 'LineWidth', lwbox); 275 | end 276 | xlabel('$s$', 'Interpreter', 'latex', 'Position', [0.5,-0.3,-1]); 277 | ylabel('Error', 'Units', 'normalized', 'Position', [-0.042,0.5,0]); 278 | xlim([0, 1]); 279 | xticks(0: 0.2: 1); 280 | rc = plot([2,3], [2,2], '--', 'LineWidth', lwt2, 'Color', new_red); 281 | bc = plot([2,3], [1,1], '--', 'LineWidth', lwt2, 'Color', new_blue); 282 | kc = plot([2,3], [0,0], 'k--', 'LineWidth', lwb2); 283 | legend([rc, bc, kc], ... 284 | "$~e(\hat{\mbox{\boldmath$r$}}_2^{\prime})~~~$", "$~e(\hat{\mbox{\boldmath$r$}}_2^{\prime\prime})~~~$", "$~e$", ... 285 | 'Interpreter', 'latex', 'Box', 'off', 'Position', [0.6,0.87,0.36,0.1], 'NumColumns', 3, 'EdgeColor', 'none', ... 286 | 'FontName', 'Times New Roman', 'FontSize', 26); 287 | set(gcf, 'Position', [10, 50, 1250, 400]); 288 | set(gca, 'Position', [0.07,0.22,0.885,0.625], 'XColor', 'k', 'YColor', 'k', ... 289 | 'TickLength', [0.005, 0], 'Box', 'on', 'LineWidth', 1.2, ... 290 | 'FontName', 'Times New Roman', 'FontSize', 28, 'LabelFontSizeMultiplier', 1); 291 | 292 | end 293 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## An actuator space optimal kinematic path tracking framework for tendon-driven continuum robots: Theory, algorithm and validation 2 | 3 |
Ke Qiu, Hongye Zhang, Jingyu Zhang, Rong Xiong, Haojian Lu, Yue Wang
State Key Laboratory of Industrial Control and Technology, Zhejiang University
4 | 5 |
6 |
7 | 8 | This repository implements the algorithms presented in our article. If you enjoy this repository and use it, please cite our paper. 9 | 10 | Our previous work (title: An Efficient Multi-solution Solver for the Inverse Kinematics of 3-Section Constant-Curvature Robots) appears in proceedings of Robotics: Science and Systems 2023. 11 | 12 | ``` 13 | @INPROCEEDINGS{Qiu-RSS-23, 14 | AUTHOR = {Ke Qiu AND Jingyu Zhang AND Danying Sun AND Rong Xiong AND Haojian LU AND Yue Wang}, 15 | TITLE = {An Efficient Multi-solution Solver for the Inverse Kinematics of 3-Section Constant-Curvature Robots}, 16 | BOOKTITLE = {Proceedings of Robotics: Science and Systems}, 17 | YEAR = {2023}, 18 | ADDRESS = {Daegu, Republic of Korea}, 19 | MONTH = {July}, 20 | DOI = {10.15607/RSS.2023.XIX.091} 21 | } 22 | ``` 23 | 24 | 25 |
26 | 27 | ### Demo 28 | 29 | **(main_demo.m)** Results of multiple solutions obtained by our algorithm. 30 | 31 | 32 | 33 | **(main_demo2.m)** Results of tracking a straight line path in two different configurations obtained by our algorithm. 34 | 35 | 36 | 37 |
38 | 39 | ### Package overview 40 | 41 | **Demo (2 files)** 42 | 43 | - main_demo.m 44 | - main_demo2.m 45 |
46 | 47 | **Solver (7 files)** 48 | public: 49 | 50 | - micsolver.m 51 | - micsolverd.m 52 | 53 | private: 54 | - rho.m 55 | - soln2xi.m 56 | - get_err.m 57 | - solve_r1.m 58 | - solve_r2.m 59 |
60 | 61 | **Planner (2 files)** 62 | public: 63 | - dp.m 64 | - allocate_time.m 65 |
66 | 67 | **Numerical methods (5 files)** 68 | public: 69 | 70 | - revise_grad.m 71 | - revise_dls.m 72 | - revise_newton.m 73 | 74 | private: 75 | - revise_plot.m 76 | - jacobian3cc.m 77 |
78 | 79 | **Quaternion operations (3 files)** 80 | 81 | - up_plus.m 82 | - up_oplus.m 83 | - up_star.m 84 |
85 | 86 | **Lie algebra operations (4 files)** 87 | 88 | - up_hat.m 89 | - up_vee.m 90 | - exphat.m 91 | - veelog.m 92 |
93 | 94 | **Conversions (6 files)** 95 | - arc2q.m 96 | - q2arc.m 97 | - arc2xi.m 98 | - xi2arc.m 99 | - xi2len.m 100 | - q2rot.m 101 | - rot2q.m 102 |
103 | 104 | **Other tools (6 files)** 105 | 106 | - circles3.m 107 | - circles3c.m 108 | - frame.m 109 | - get_end.m 110 | - collision_indicator.m 111 | - collision_marker.m 112 |
113 | 114 | ### Functions 115 | 116 | #### Solver 117 | 118 | **micsolver.m** 119 | 120 | `MICSOLVER` Multi-solution solver for the inverse kinematics of 3-section constant-curvature robots. 121 | 122 | `[NOS, NOI] = MICSOLVER(L1, L2, L3, Q, R, TOL, IS_A_SOL)` returns the result of solving the 3-section inverse kinematics problem. The function uses preset resolutions or numerical methods to address the inverse kinematics problem. The function exits after one solution is found. 123 | 124 | *Input parameters* 125 | `L1, L2, L3` section length 126 | `Q, R` desired end rotation and translation 127 | `TOL` error tolerance 128 | `IS_A_SOL` function handle 129 | It is used to judge if the given parameter is a solution to the inverse kinematics problem. The function has two inputs `(ERR, XI)` and one output in boolean type. 130 | 131 | *Output parameters* 132 | `NOS` number of solutions 133 | `NOI` number of iterations in numerical correction 134 | 135 | *Example* 136 | ```matlab 137 | L1 = 1; L2 = 1; L3 = 1; 138 | xi = arc2xi(L1, L2, L3, pi.*[1,2,1,2,1,2].*rand(1, 6)); 139 | T = get_end(L1, L2, L3, xi); 140 | q = rot2q(T(1:3, 1:3)); 141 | r = T(1:3, 4); 142 | tol = 1e-2; fun = @(e, x) e < tol; 143 | tic; 144 | [nos, ~] = micsolver(L1, L2, L3, q, r, tol, fun); 145 | rt = toc*1000; 146 | if nos 147 | fprintf('A solution is found in %.2f ms.\n', rt); 148 | end 149 | ``` 150 | 151 |
152 | 153 | **micsolverd.m** 154 | 155 | `MICSOLVERD` Multi-solution solver (debug) for the inverse kinematics of 3-section constant-curvature robots. 156 | 157 | `[SOL, NOS, NOI] = MICSOLVERD(L1, L2, L3, Q, R, PAR, NOC, TOL, MSTEP, IS_A_SOL, PLOT_EC, PLOT_IT)` returns the result of solving the 3-section inverse kinematics problem. 158 | 159 | *Input parameters* 160 | `L1, L2, L3` section length 161 | `Q, R` desired end rotation and translation 162 | `PAR` length of the partition 163 | Scalar. The interval $[0, 1)$ is partitioned into subintervals with length `PAR`. A smaller `PAR` means finer resolution. We recommend `PAR = 0.03` for better efficiency and `PAR = 0.01` for more solutions. 164 | `NOC` number of corrections 165 | A 1-by-2 array. The model parameters $\hat{\boldsymbol{r}}_3$ and hat $\hat{\boldsymbol{r}}_1$ are corrected for `NOC(1)` and `NOC(2)` times after the approximation, respectively. Large `NOC` provides closer initial value and more computations. We recommend `NOC = [1, 2]` for better efficiency and `NOC = [5, 5]` for better estimation. 166 | `TOL` error tolerance 167 | `MSTEP` allowed maximum steps of iterations 168 | `IS_A_SOL` function handle 169 | This function is used to judge if the given parameter is a solution to the inverse kinematics problem. The function has two inputs `(ERR, XI)` and one output in boolean type. 170 | `PLOT_EC` set `'plot'` to visualise the traversal 171 | `PLOT_IT` set `'plot'` to visualise the numerical correction 172 | 173 | *Output parameters* 174 | `SOL` solutions 175 | A 6-by-`NOS` array. Each column is the overall exponential coordinate `XI`. 176 | `NOS` number of solutions 177 | `NOI` number of iterations in numerical correction 178 | 179 | *Example* 180 | ```matlab 181 | L1 = 1; L2 = 1; L3 = 1; 182 | alpha = 15*pi/16; omega = [0.48; sqrt(3)/10; -0.86]; 183 | q = [cos(alpha/2); sin(alpha/2)*omega]; 184 | r = [-0.4; 1.1; 0.8]; 185 | tol = 1e-2; fun = @(e, x) e < tol; 186 | [sol, ~, ~] = micsolverd(L1, L2, L3, q, r, ... 187 | 0.01, [5, 5], tol, 10, fun, ... 188 | 'plot', 'plot'); 189 | for eta = 1: size(sol, 2) 190 | fh = figure(); 191 | circles3(fh, L1, L2, L3, sol(:, eta), 'k-'); 192 | view(119, 20); 193 | end 194 | ``` 195 | 196 |
197 | 198 | **rho.m** 199 | 200 | `RHO` Computes the linear distance between two ends of a circular arc. 201 | 202 | This is a private function of our solver. 203 | 204 |
205 | 206 | **soln2xi.m** 207 | 208 | `SOLN2XI` Converts the output of our solver to an exponential coordinate. 209 | 210 | This is a private function of our solver. 211 | 212 |
213 | 214 | **get_err.m** 215 | 216 | `GET_ERR` Computes the error between desired and current end pose. 217 | 218 | This is a private function of our solver. 219 | 220 |
221 | 222 | **solve_r1.m** 223 | 224 | `SOLVE_R1` Computes the model parameter of the 1st section. 225 | 226 | This is a private function of our solver. 227 | 228 |
229 | 230 | **solve_r2.m** 231 | 232 | `SOLVE_R2` Computes the model parameter of the 2nd section using rotational and translational constraints. 233 | 234 | This is a private function of our solver. 235 | 236 |
237 | 238 | #### Planner 239 | 240 | **dp.m** 241 | 242 | `DP` Finds the shortest path in a graph. 243 | 244 | `[PATH, COST] = DP(XISC, LOSSFCN)` returns the paths and corresponding costs using the Dijkstra's algorithm. The cell array `XISC` defines the vertices. The function handle `LOSSFCN` defines the weight of two adjacent edges. The output `PATH` and `COST` are cell arrays. 245 | 246 |
247 | 248 | **allocate_time.m** 249 | 250 | `ALLOCATE_TIME` allocates optimal time to a given sequence of concatenated parameters, considering the actuator velocity constraints. 251 | 252 | `TS = ALLOCATE_TIME(XIS)` returns the time array `TS`. 253 | 254 |
255 | 256 | #### Numerical methods 257 | 258 | **revise_*.m** 259 | 260 | `REVISE_*` Correct the initial value with a numerical method. 261 | 262 | `[XI_STAR, ERR, K] = REVISE_*(L1, L2, L3, Q, R, XI, MSTEP, TOL, TYPE)` returns the result of numerical correction. 263 | 264 | *Methods* 265 | `grad` gradient method 266 | `dls` damped least square method 267 | `newton` Newton-Raphson method 268 | 269 | *Input parameters* 270 | `L1, L2, L3` section length 271 | `Q, R` desired end rotation and translation 272 | `XI` initial value 273 | `MSTEP` allowed maximum steps of iterations 274 | `TOL` error tolerance 275 | `TYPE` set `'plot'` to visualise the numerical correction 276 | 277 | *Output parameters* 278 | `XI_STAR` final value 279 | `ERR` final error 280 | `K` steps of iterations 281 | 282 | *Example* 283 | ```matlab 284 | L1 = 1; L2 = 1; L3 = 1; 285 | alpha = 15*pi/16; omega = [0.48; sqrt(3)/10; -0.86]; 286 | q = [cos(alpha/2); sin(alpha/2)*omega]; 287 | r = [-0.4; 1.1; 0.8]; 288 | xi_0 = arc2xi(L1, L2, L3, pi.*[1, 2, 1, 2, 1, 2].*rand(1, 6)); 289 | [xi, err, noi] = revise_grad(L1, L2, L3, q, r, xi_0, 2000, 1e-2, 'plot'); 290 | [xi, err, noi] = revise_dls(L1, L2, L3, q, r, xi_0, 2000, 1e-2, 'plot'); 291 | [xi, err, noi] = revise_newton(L1, L2, L3, q, r, xi_0, 200, 1e-2, 'plot'); 292 | ``` 293 | 294 |
295 | 296 | **revise_plot.m** 297 | 298 | `REVISE_PLOT` Visualises the numerical correction. 299 | 300 | This is a private function of numerical methods. 301 | 302 |
303 | 304 | **jacobian3cc.m** 305 | 306 | `JACOBIAN3CC` Computes the Jacobian matrix when the forward kinematics is expressed by the product of exponentials formula. 307 | 308 | `J = JACOBIAN3CC(L1, L2, L3, XI)` returns the 6-by-6 Jacobian matrix. 309 | 310 |
311 | 312 | #### Quaternion operations 313 | 314 | **up_plus.m** 315 | 316 | `UP_PLUS` Computes the matrix for left multiplications of quaternions. 317 | 318 | `Q_UP_PLUS = UP_PLUS(Q)` returns the left multiplication matrix of `Q`. 319 | 320 |
321 | 322 | **up_oplus.m** 323 | 324 | `UP_OPLUS` Computes the matrix for right multiplications of quaternions. 325 | 326 | `Q_UP_OPLUS = UP_OPLUS(Q)` returns the right multiplication matrix of `Q`. 327 | 328 |
329 | 330 | **up_star.m** 331 | 332 | `UP_STAR` Computes the quaternion conjugation. 333 | 334 | `Q_UP_STAR = UP_STAR(Q)` returns the conjugation of `Q`. 335 | 336 |
337 | 338 | #### Lie algebra operations 339 | 340 | **up_hat.m** 341 | 342 | `UP_HAT` Computes the Lie algebra of a vector. 343 | 344 | `M = UP_HAT(V)` is an element of $\mathsf{so}_3$ or $\mathsf{se}_3$, where `V` is an element of $\mathbb{R}^3$ or $\mathbb{R}^6$, respectively. 345 | 346 |
347 | 348 | **up_vee.m** 349 | 350 | `UP_VEE` Computes the vector of a Lie algebra. 351 | 352 | `V = UP_VEE(M)` is an element of $\mathbb{R}^3$ or $\mathbb{R}^6$, where `M` is an element of $\mathsf{so}_3$ or $\mathsf{se}_3$, respectively. 353 | 354 |
355 | 356 | **exphat.m** 357 | 358 | `EXPHAT` Composition of the hat map and the matrix exponential. 359 | 360 | `M = EXPHAT(V)` is a matrix in $\mathsf{SO}_3$ or $\mathsf{SE}_3$ and is computed using Rodrigues' formula. The vector `V` is in $\mathbb{R}^3$ or $\mathbb{R}^4$. The hat map sends `V` to an element of $\mathsf{so}_3$ or $\mathsf{se}_3$. 361 | 362 |
363 | 364 | **veelog.m** 365 | 366 | `VEELOG` Composition of the matrix logarithm and the vee map. 367 | 368 | `V = VEELOG(M)` is a vector in $\mathbb{R}^3$ or $\mathbb{R}^4$ and is computed using Rodrigues' formula. The matrix `M` is in $\mathsf{SO}_3$ or $\mathsf{SE}_3$. The vee map sends an element of $\mathsf{so}_3$ or $\mathsf{se}_3$ to a vector. 369 | 370 |
371 | 372 | #### Conversions 373 | 374 | **arc2q.m** 375 | 376 | `ARC2Q` Converts arc parameters to a quaternion. 377 | 378 | `Q = ARC2Q(KAPPA, PHI, L)` computes the quaternion `Q` representing the end rotation of a 1-section constant-curvature robot with curvature `KAPPA`, bending angle `PHI`, and section length `L`. 379 | 380 | *Example* 381 | ```matlab 382 | kappa = 2*pi/3; 383 | phi = pi/3; 384 | q = arc2q(kappa, phi, 1); 385 | ``` 386 | 387 |
388 | 389 | **q2arc.m** 390 | 391 | `Q2ARC` Converts a quaternion to arc parameters. 392 | 393 | `ARC = Q2ARC(Q, L)` computes the arc parameters of a 1-section constant-curvature robot, including the curvature `KAPPA` and bending angle `PHI`. The section length is `L`. The quaternion `Q` represents the end rotation. 394 | 395 | *Example* 396 | ```matlab 397 | q = [1/2; -3/4; sqrt(3)/4; 0]; 398 | arc = q2arc(q, 1); 399 | ``` 400 | 401 |
402 | 403 | **arc2xi.m** 404 | 405 | `ARC2XI` Converts the arc parameters of 3 sections to the exponential coordinate. 406 | 407 | `XI = ARC2XI(L1, L2, L3, ARC)` computes the exponential coordinate `XI` of a 3-section constant-curvature robot. The section lengths are `L1, L2` and `L3`, respectively. The parameter `ARC` is an array containing curvatures and bending angles of each section. 408 | 409 | *Example* 410 | ```matlab 411 | k1 = 4*sqrt(5)/5; p1 = atan(2); 412 | k2 = sqrt(37)/5; p2 = -pi+atan(6); 413 | k3 = sqrt(10)/5; p3 = -atan(3); 414 | xi = arc2xi(1, 1, 1, [k1, p1, k2, p2, k3, p3]); 415 | ``` 416 | 417 |
418 | 419 | **xi2arc.m** 420 | 421 | `XI2ARC` Converts the exponential coordinate to the arc parameters of 3 sections. 422 | 423 | `ARC = XI2ARC(L1, L2, L3, XI)` computes the arc parameter `ARC` of a 3-section constant-curvature robot. The section lengths are `L1, L2` and `L3`, respectively. The parameter `XI` is the overall exponential coordinate. 424 | 425 | *Example* 426 | ```matlab 427 | xi = [-1.6; 0.8; 1.2; -0.2; 0.6; 0.2]; 428 | arc = xi2arc(1, 1, 1, xi); 429 | ``` 430 | 431 |
432 | 433 | **xi2len.m** 434 | 435 | `XI2LEN` Converts the concatenated parameter to the actuator lengths. 436 | 437 | `LEN = XI2LEN(XI)` computes the actuator lengths `LEN` of a 3-section constant-curvature robot. The concatenated parameter `XI` is defined in our article. 438 | 439 | *Example* 440 | ```matlab 441 | xi = [-1.6; 0.8; 1.2; -0.2; 0.6; 0.2]; 442 | len = xi2len(xi); 443 | ``` 444 | 445 |
446 | 447 | **q2rot.m** 448 | 449 | `Q2ROT` Converts a quaternion to a rotation matrix. 450 | 451 | `R = Q2ROT(Q)` returns the rotation matrix that is equivalent to the quaternion. 452 | 453 |
454 | 455 | **rot2q.m** 456 | 457 | `ROT2Q` Converts a rotation matrix to a quaternion. 458 | 459 | `Q = ROT2Q(R)` returns the quaternion that is equivalent to the rotation matrix. 460 | 461 |
462 | 463 | #### Other tools 464 | 465 | **circles3.m** 466 | 467 | `CIRCLES3` Visualises the 3-section constant-curvature robot with given model parameters. 468 | 469 | `CIRCLES3(FH, L1, L2, L3, XI, TYPE)` displays the plot in target figure `FH`. The robot is described by the section lengths `L1, L2, L3` and the overall exponential coordinate `XI`. Line colours and styles are specified in the character string `TYPE`. 470 | 471 | *Example* 472 | ```matlab 473 | L1 = 1; L2 = 1; L3 = 1; 474 | xi_1 = [-1.60; 0.08; 1.20; -0.20; 0.60; 0.20]; 475 | circles3(1, L1, L2, L3, xi_1, 'k--'); 476 | xi_2 = [-0.39; 0.48; -1.13; 0.47; 1.79; -0.17]; 477 | circles3(1, L1, L2, L3, xi_2, 'k-'); 478 | view(75, 9); 479 | ``` 480 | 481 |
482 | 483 | **circles3c.m** 484 | 485 | `CIRCLES3` Visualises the 3-section constant-curvature robot with given model parameters and a specified colour. 486 | 487 | `CIRCLES3C(FH, L1, L2, L3, XI, TYPE, COLOUR)` displays the plot in target figure `FH`. The robot is described by the section lengths `L1, L2, L3` and the overall exponential coordinate `XI`. Line styles are specified in the character string `TYPE`, line colours are specified in the triple `COLOUR`. 488 | 489 | *Example* 490 | ```matlab 491 | L1 = 1; L2 = 1; L3 = 1; 492 | xi_1 = [-1.60; 0.08; 1.20; -0.20; 0.60; 0.20]; 493 | circles3c(1, L1, L2, L3, xi_1, '--', [0.1, 0.1, 0.1]); 494 | xi_2 = [-0.39; 0.48; -1.13; 0.47; 1.79; -0.17]; 495 | circles3c(1, L1, L2, L3, xi_2, '-', [0.2, 0.2, 0.2]); 496 | view(75, 9); 497 | ``` 498 | 499 |
500 | 501 | **frame.m** 502 | 503 | `FRAME` draws a coordinate frame with specified position (and orientation) and transparency. 504 | 505 | `FRAME(FH, A, ALPHA)` displays the plot in target figure `FH`. The position (and orientation) is specified by `A` and the transparency is specified by `ALPHA`. 506 | 507 | *Example* 508 | ```matlab 509 | frame(1, [eye(3), zeros(3, 1); 0, 0, 0, 1], 0.3); 510 | ``` 511 | 512 |
513 | 514 | **get_end.m** 515 | 516 | `GET_END` Computes the end pose of a 3-section constant-curvature robot. 517 | 518 | `T = GET_END(L1, L2, L3, XI)` returns the 4-by-4 matrix of end pose. 519 | 520 | *Example* 521 | ```matlab 522 | xi = [-1.6; 0.8; 1.2; -0.2; 0.6; 0.2]; 523 | T = get_end(1, 1, 1, xi); 524 | ``` 525 | 526 |
527 | 528 | **collision_indicator.m** 529 | 530 | `COLLISION_INDICATOR` Computes the minimal distance between the sample points and the spherical obstacles. 531 | 532 | `COLLIDE = COLLISION_INDICATOR(L1, L2, L3, XI, RO, ROR, SMP)` returns the minimal distance `COLLIDE`. If `COLLIDE > 0`, then no collision occurs. If `COLLIDE < 0`, then the robot collides with obstacles. The robot is described by the section lengths `L1, L2, L3` and the overall exponential coordinate `XI`. The obstacles are spheres centring at `RO` with radius `ROR`. The sample points are distributed uniformly along the robot curve with the number `SMP`. 533 | 534 | *Example* 535 | ```matlab 536 | L1 = 1; L2 = 1; L3 = 1; 537 | xi = [-1.6; 0.8; 1.2; -0.2; 0.6; 0.2]; 538 | ro = [0.8; 0.7; 0.6]; 539 | ror = 0.4; 540 | collide = collision_indicator(L1, L2, L3, xi, ro, ror, 10); 541 | ``` 542 | 543 |
544 | 545 | **collision_marker.m** 546 | 547 | `COLLISION_MARKER` Visualises the collision part in yellow. 548 | 549 | `COLLISION_MARKER(L1, L2, L3, XI, RO, ROR)` draws the part of the robot that is collided with obstacles. The robot is described by the section lengths `L1, L2, L3` and the overall exponential coordinate `XI`. The obstacles are spheres centring at `RO` with radius `ROR`. 550 | 551 | *Example* 552 | ```matlab 553 | L1 = 1; L2 = 1; L3 = 1; 554 | xi = [-1.6; 0.8; 1.2; -0.2; 0.6; 0.2]; 555 | ro = [0.8; 0.7; 0.6]; 556 | ror = 0.4; 557 | circles3(1, L1, L2, L3, xi, 'k-'); 558 | collision_marker(L1, L2, L3, xi, ro, ror); 559 | view(75, 9); 560 | ``` 561 | 562 |
563 | 564 | --------------------------------------------------------------------------------