An Efficient Multi-solution Solver for the Inverse Kinematics of 3-Section Constant-Curvature Robots

Package Overview

Demo (1 file)

main_demo.m

Solver (7 files)
public:

micsolver.m
micsolverd.m

private:

rho.m
soln2xi.m
get_err.m
solve_r1.m
solve_r2.m

Numerical Methods (5 files)
public:

revise_grad.m
revise_dls.m
revise_newton.m

private:

revise_plot.m
jacobian3cc.m

Quaternion Operations (3 files)

up_plus.m
up_oplus.m
up_star.m

Lie Algebra Operations (4 files)

up_hat.m
up_vee.m
exphat.m
veelog.m

Conversions (6 files)

arc2q.m
q2arc.m
arc2xi.m
xi2arc.m
q2rot.m
rot2q.m

Other Tools (4 files)

circles3.m
get_end.m
collision_indicator.m
collision_marker.m

Functions

Solver

micsolver.m

MICSOLVER Multi-solution solver for the inverse kinematics of 3-section constant-curvature robots.

[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.

Input parameters
L1, L2, L3 section length
Q, R desired end rotation and translation
TOL error tolerance
IS_A_SOL function handle
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.

Output parameters
NOS number of solutions
NOI number of iterations in numerical correction

Example

L1 = 1; L2 = 1; L3 = 1;
xi = arc2xi(L1, L2, L3, pi.*[1,2,1,2,1,2].*rand(1, 6));
T = get_end(L1, L2, L3, xi);
q = rot2q(T(1:3, 1:3));
r = T(1:3, 4);
tol = 1e-2; fun = @(e, x) e < tol;
tic;
[nos, ~] = micsolver(L1, L2, L3, q, r, tol, fun);
rt = toc*1000;
if nos
    fprintf('A solution is found in %.2f ms.\n', rt);
end

micsolverd.m

MICSOLVERD Multi-solution solver (debug) for the inverse kinematics of 3-section constant-curvature robots.

[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.

Input parameters
L1, L2, L3 section length
Q, R desired end rotation and translation
PAR length of the partition
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.
NOC number of corrections
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.
TOL error tolerance
MSTEP allowed maximum steps of iterations
IS_A_SOL function handle
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.
PLOT_EC set 'plot' to visualise the traversal
PLOT_IT set 'plot' to visualise the numerical correction

Output parameters
SOL solutions
A 6-by-NOS array. Each column is the overall exponential coordinate XI.
NOS number of solutions
NOI number of iterations in numerical correction

Example

L1 = 1; L2 = 1; L3 = 1;
alpha = 15*pi/16; omega = [0.48; sqrt(3)/10; -0.86];
q = [cos(alpha/2); sin(alpha/2)*omega];
r = [-0.4; 1.1; 0.8];
tol = 1e-2; fun = @(e, x) e < tol;
[sol, ~, ~] = micsolverd(L1, L2, L3, q, r, ...
                         0.01, [5, 5], tol, 10, fun, ...
                         'plot', 'plot');
for eta = 1: size(sol, 2)
    fh = figure();
    circles3(fh, L1, L2, L3, sol(:, eta), 'k-');
    view(119, 20);
end

rho.m

RHO Computes the linear distance between two ends of a circular arc.