SPM Demo

This tutorial demonstrates the usage of the SPM (Spherical Parallel Manipulator) module in PaceRobots.

1. Geometric Model for a Set of Conception Parameters

First, we need to set the conception parameters in the following order:

  • 1-3: η_{i,b} (i=1,…, 3): joints placement on the base
  • 4-6: η_{i,u} (i=1,…, 3): joints placement on the upper platform
  • 7: β₁: geometry of the base
  • 8: β₁: geometry of the upper platform
  • 9-11: α_{1,i}: proximal link of the ith leg
  • 12-14: α_{2,i}: distal link of the ith leg
using Revise
using PaceRobots: SPM
using MPFI, DynamicPolynomials

setprecision(127)
p = [BigInterval(pi)/4, -BigInterval(pi)/4, 0, BigInterval(pi)/4, -BigInterval(pi)/4, 0, 0, BigInterval(pi)/2, BigInterval(pi)/4, BigInterval(pi)/4, BigInterval(pi)/2, BigInterval(pi)/2, BigInterval(pi)/2, BigInterval(pi)/2]

Compute the Geometric Model

The output of SPM.model() is a vector of polynomials represented using DynamicPolynomials with interval coefficients of precision equal to the precision of the vector p of parameters.

gmod = SPM.model(p)

Check the Precision of the Coefficients

precision(DynamicPolynomials.coefficients(gmod[1])[1])

2. Specialize the Variables χ₁, χ₂, χ₃

chi_val = [BigFloat(0.291824), -0.291824, 0]
sys = SPM.specialize_chi(gmod, chi_val)

3. Specialize the Variables θ₁, θ₂, θ₃

SPM.specialize_theta(gmod, [BigFloat(0),0,0])

4. Solve the Inverse Geometric Problem

This amounts to solving some bivariate polynomials ax² + bx + c. The first argument is a system with the χ variables already specialized. The second argument sign indicates that the solution -b + sign * sqrt(discriminant) / 2*a is kept.

sign = 1
SPM.IGP(sys, sign)
SPM.IGP(sys, -1)

5. Find an Approximation of ST1

Transform the system with interval coefficients to one with rational coefficients (by taking the midpoint of each interval):

gmod_rat = SPM.model_convert2rat(gmod)
SPM.ST1(gmod_rat)

Plot ST1

st1_fig = SPM.plot_ST1(gmod_rat)

Plot ST1 in Physical Space

st1 = SPM.plot_ST1_physical(gmod_rat)

Workspace and Joint Space Definition

# Workspace of choice
Wx = [tan(-0.1), tan(0.1), tan(0.1), tan(-0.1), tan(-0.1)]
Wy = [tan(-0.4325), tan(-0.4325), tan(0.4325), tan(0.4325), tan(-0.4325)]

SPM.plot_workspace!(Wx, Wy, st1_fig)

6. Path-tracking

Uses the function interval_classic_newton of LACE.

Parameters:

  • sys::Vector: A system of polynomial equations represented as a vector of DynamicPolynomials
  • theta_init::Vector: Initial guess for the θ values
  • sign::Int32: Sign flag used in the IGP solver
  • chi_init::Vector: Initial guess for the χ values
  • step_range::Vector: Step size range for θ adjustments
  • prec_range::Vector: Range of precision values for iterative refinement
  • max_nb_loops::Integer: Maximum number of iterations for Newton's method
  • nb_ang_steps::Int32: Number of angular steps for each iteration of χ
  • work_precision: Precision used during intermediate calculations
  • output_precision: Final precision for the solution

Returns:

  • A tuple containing:
    • plots: A list of box plot representations showing solutions and precision intervals
    • kanto_res: The results of the Kantorovich method for each solution
work_precision = 127

theta_init = [BigFloat(1.0;precision=work_precision), BigFloat(1.0;precision=work_precision), BigFloat(1.0;precision=work_precision)]
sign = Int32(1)
point_precision = 127
chi_init = [BigFloat(0.0;precision=point_precision), BigFloat(0.0;precision=point_precision), BigFloat(0.0;precision=point_precision)]
step_range = [1e-2, 1e-2]
prec_range = [14, 14]
max_nb_loops = 10
output_precision = 127
nb_ang_steps = Int32(50)

# Corner points
w1 = [tan(-0.1), tan(-0.4325)]
w2 = [tan(0.1), tan(-0.4325)]
w3 = [tan(0.1), tan(0.4325)]
w4 = [tan(-0.1), tan(0.4325)]

out_fig, kanto = SPM.ST2_angular(gmod, theta_init, sign, chi_init, step_range, prec_range, max_nb_loops, nb_ang_steps, work_precision, output_precision)

Using Existing Figure

out_fig = st1_fig
_, kanto = SPM.ST2_angular(gmod, theta_init, sign, chi_init, step_range, prec_range, max_nb_loops, nb_ang_steps, work_precision, output_precision, w1, w2, w3, w4, fig=out_fig)

Grid-based Path Tracking

out_fig = st1_fig
_, kanto = SPM.ST2_grid(gmod, sign, chi_init, step_range, prec_range, max_nb_loops, work_precision, output_precision; w1=w1, w2=w2, w3=w3, w4=w4, fig=st1_fig)