| Server IP : 82.64.35.208 / Your IP : 172.20.0.1 Web Server : Apache/2.4.67 (Debian) System : Linux c1d185609d2c 6.12.76-linuxkit #1 SMP Thu May 28 18:54:18 UTC 2026 aarch64 User : root ( 0) PHP Version : 8.3.31 Disable Function : NONE MySQL : OFF | cURL : ON | WGET : OFF | Perl : ON | Python : OFF | Sudo : OFF | Pkexec : OFF Directory : /var/www/html/downloads/ |
Upload File : |
{
"cells": [
{
"cell_type": "markdown",
"id": "7be1d69b",
"metadata": {},
"source": [
"## Chargement des bibliothèques"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "0e42af50",
"metadata": {},
"outputs": [],
"source": [
"import Pkg; \n",
"#Pkg.add(url=\"https://gitlab.inria.fr/NewRUR/RationalUnivariateRepresentation.jl\");\n",
"#Pkg.add(url=\"https://gitlab.inria.fr/pace/rs.jl.git\")\n",
"#Pkg.add(url=\"https://gitlab.inria.fr/ckatsama/mpfi.jl.git\");\n",
"#Pkg.add(url=\"https://github.com/sumiya11/DiscriminantVariety.jl.git\")\n",
"#Pkg.add(url=\"https://gitlab.inria.fr/pace/lace.jl.git\")\n",
"#Pkg.add(url=\"https://gitlab.inria.fr/pace/pace.jl.git\")\n",
"#Pkg.add(url=\"https://gitlab.inria.fr/pace/robotics/pacerobots.jl.git\")\n",
"#Pkg.add(\"DynamicPolynomials\")\n",
"#Pkg.add(\"ProgressMeter\")\n",
"#Pkg.add(\"Makie\")\n",
"#Pkg.add(\"GLMakie\")\n",
"#Pkg.add(\"LinearAlgebra\") \n",
"#Pkg.add(\"GeometryBasics\")\n",
"#Pkg.add(\"AbstractAlgebra\")\n",
"#Pkg.add(\"Distributed\")\n"
]
},
{
"cell_type": "markdown",
"id": "607cda67",
"metadata": {},
"source": [
"## Initialisation des process"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "e86f087c",
"metadata": {},
"outputs": [],
"source": [
"using Makie,GLMakie, LinearAlgebra, GeometryBasics, PaceRobots, DynamicPolynomials,AbstractAlgebra,RationalUnivariateRepresentation,RS,Distributed\n",
"# 1. On lance les processus (8 coeurs ici)\n",
"if nprocs() == 1\n",
" addprocs(8) \n",
"end\n",
"\n",
"# 2. On charge les modules sur TOUS les processus (@everywhere)\n",
"@everywhere begin\n",
" using Pkg\n",
" Pkg.activate(\".\") # Active votre environnement PACE\n",
" using PaceRobots\n",
" using GLMakie, LinearAlgebra\n",
" # Si Hexapod est un sous-module, on s'assure qu'il est accessible\n",
"end\n",
"# On \"envoie\" les constantes de votre notebook à tout le monde\n"
]
},
{
"cell_type": "markdown",
"id": "b11db259",
"metadata": {},
"source": [
"## Utilitaires de conversions"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "85955ef1",
"metadata": {},
"outputs": [],
"source": [
"import DynamicPolynomials as DP\n",
"function convertir_vers_aa(liste_dp, anneau_aa)\n",
" liste_convertie = []\n",
" \n",
" for p_dp in liste_dp\n",
" # On spécifie explicitement que l'on veut les fonctions de DynamicPolynomials\n",
" coeffs = collect(DP.coefficients(p_dp))\n",
" exps = [Vector{Int}(DP.exponents(m)) for m in DP.monomials(p_dp)]\n",
" \n",
" # On construit dans AbstractAlgebra\n",
" push!(liste_convertie, anneau_aa(coeffs, exps))\n",
" end\n",
" \n",
" return liste_convertie\n",
"end"
]
},
{
"cell_type": "markdown",
"id": "6fa08998",
"metadata": {},
"source": [
"## Fonctions de Calcul"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "79cf170f",
"metadata": {},
"outputs": [],
"source": [
"\n",
"\n",
"# --- UTILITAIRES DE CONVERSION ---\n",
"\n",
"# Conversion sécurisée vers Rational{BigInt}\n",
"function to_rat(x)\n",
" # On rationalise d'abord en Int64, puis on convertit en BigInt pour éviter les overflows\n",
" r = rationalize(Int64, x, tol=1e-15)\n",
" return Rational{BigInt}(r)\n",
"end\n",
"\n",
"# Conversion Matrix/Vector vers Rationnels\n",
"to_rat_map(A) = map(to_rat, A)\n",
"\n",
"# Conversion inverse pour l'affichage (Vers Float32 pour Makie)\n",
"to_f32(x) = Float32.(x)\n",
"\n",
"\"\"\"\n",
" to_dyadic_rational(x::Real, k::Int)\n",
"Version robuste supportant k > 64. \n",
"Approche x par num / 2^n avec |num| < 2^k.\n",
"\"\"\"\n",
"function to_dyadic_rat(x::Real, k::Int=23)\n",
" if iszero(x)\n",
" return 0 // 1\n",
" end\n",
"\n",
" # 1. Utilisation de BigFloat pour éviter l'overflow de Float64 sur log2\n",
" # On règle la précision de BigFloat pour être au moins égale à k\n",
" setprecision(BigFloat, max(256, k + 32)) do\n",
" \n",
" val_abs = abs(BigFloat(x))\n",
" \n",
" # 2. Calcul de n_max avec BigFloat pour supporter les très grands/petits nombres\n",
" # n < k - log2(|x|)\n",
" n_max = Int(floor(BigFloat(k) - log2(val_abs)))\n",
" \n",
" # 3. Calcul du dénominateur 2^n\n",
" den = BigInt(2)^n_max\n",
" \n",
" # 4. Calcul du numérateur par multiplication exacte (si x est Rational) \n",
" # ou haute précision\n",
" num = BigInt(round(x * den))\n",
" \n",
" # 5. Sécurité : vérification de la borne 2^k\n",
" borne = BigInt(2)^k\n",
" if abs(num) >= borne\n",
" n_max -= 1\n",
" den = BigInt(2)^n_max\n",
" num = BigInt(round(x * den))\n",
" end\n",
"\n",
" return Rational{BigInt}(num // den)\n",
" end\n",
"end\n",
"\n"
]
},
{
"cell_type": "markdown",
"id": "04dd7abb",
"metadata": {},
"source": [
"## Fonctions de modélisation"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "821f4da6",
"metadata": {},
"outputs": [],
"source": [
"function generate_geometry()\n",
" # --- DONNÉES (Géométrie Gough-Stewart accentuée) ---\n",
"\n",
" R_base = 1.2\n",
" R_plat = 0.7\n",
"\n",
" # Angles très serrés pour bien voir les paires (en radians)\n",
" # Plus l'angle est petit, plus les joints sont proches\n",
" θ_base = deg2rad(8) # Joints de base très proches\n",
" θ_plat = deg2rad(12) # Joints de plateforme un peu plus espacés pour la stabilité\n",
"\n",
" # Base : Les paires sont centrées sur 0°, 120°, 240°\n",
" base_pts_coords = [\n",
" R_base * cos(0 - θ_base), R_base * sin(0 - θ_base), 0,\n",
" R_base * cos(0 + θ_base), R_base * sin(0 + θ_base), 0,\n",
" R_base * cos(2π/3 - θ_base), R_base * sin(2π/3 - θ_base), 0,\n",
" R_base * cos(2π/3 + θ_base), R_base * sin(2π/3 + θ_base), 0,\n",
" R_base * cos(4π/3 - θ_base), R_base * sin(4π/3 - θ_base), 0,\n",
" R_base * cos(4π/3 + θ_base), R_base * sin(4π/3 + θ_base), 0\n",
" ]\n",
"\n",
" # Plateforme : Les paires sont centrées sur 60°, 180°, 300° \n",
" # (Décalage de 60° pour que les vérins se croisent)\n",
" plat_pts_local = [\n",
" R_plat * cos(5π/3 + θ_plat), R_plat * sin(5π/3 + θ_plat), 0,\n",
" R_plat * cos(π/3 - θ_plat), R_plat * sin(π/3 - θ_plat), 0,\n",
" R_plat * cos(π/3 + θ_plat), R_plat * sin(π/3 + θ_plat), 0,\n",
" R_plat * cos(π - θ_plat), R_plat * sin(π - θ_plat), 0,\n",
" R_plat * cos(π + θ_plat), R_plat * sin(π + θ_plat), 0,\n",
" R_plat * cos(5π/3 - θ_plat), R_plat * sin(5π/3 - θ_plat), 0\n",
" ]\n",
"# 2. On applique to_dyadic_rat à chaque élément (.) \n",
" # et on redimensionne en matrice 6 lignes, 3 colonnes (reshape)\n",
" # On transpose (') car reshape remplit en colonne par défaut\n",
" BASE_RAT = reshape(to_dyadic_rat.(base_pts_coords), 3, 6)'\n",
" PLAT_LOC_RAT = reshape(to_dyadic_rat.(plat_pts_local), 3, 6)'\n",
"\n",
" return Matrix{Rational{BigInt}}(BASE_RAT), Matrix{Rational{BigInt}}(PLAT_LOC_RAT)\n",
"end\n",
"\n",
"\"\"\"\n",
"MGI Rationnel : Calcule les carrés des longueurs des jambes.\n",
"p : vecteur position (3 Rationals)\n",
"R : matrice de rotation (3x3 Rationals)\n",
"\"\"\"\n",
"function mgi_rational(p, R,BASE_RAT,PLAT_LOC_RAT)\n",
" L = Vector{Float64}(undef, 6) # Ou BigFloat pour garder de la précision\n",
" for i in 1:6\n",
" p_loc = PLAT_LOC_RAT[i, :]\n",
" p_base = BASE_RAT[i, :]\n",
" \n",
" diff = p + R * p_loc - p_base\n",
" L2 = dot(diff, diff)\n",
" # On doit retourner L, pas L^2\n",
" L[i] = to_dyadic_rat(sqrt(Float64(L2))) \n",
" end\n",
" return L\n",
"end\n",
"function get_rotation_q(from_v, to_v)\n",
" f = normalize(from_v)\n",
" t = normalize(to_v)\n",
" # Cas où les vecteurs sont opposés\n",
" if dot(f, t) < -0.999\n",
" return Quaternionf(0, 1, 0, 0) \n",
" end\n",
" # Produit vectoriel pour trouver l'axe de rotation\n",
" axis = cross(f, t)\n",
" # Calcul du quaternion (w, x, y, z)\n",
" w = 1.0 + dot(f, t)\n",
" return normalize(Quaternionf(axis..., w))\n",
"end\n",
"function generate_static_initial_pose(z_repos,k::Int=23)\n",
" # 1. Translation initiale (exemple : centrée en X,Y et à 1.2m de hauteur)\n",
" # On utilise to_dyadic_rat pour rester cohérent avec votre format rationnel\n",
" pos_init = [to_dyadic_rat(0.0, k), to_dyadic_rat(0.0, k), to_dyadic_rat(Float64(z_repos) , k)]\n",
" \n",
" # 2. Rotation initiale (Matrice Identité rationnelle)\n",
" I3_rat = [1//1 0//1 0//1; \n",
" 0//1 1//1 0//1; \n",
" 0//1 0//1 1//1]\n",
" \n",
" # On retourne un tableau contenant UN SEUL tuple\n",
" return [(pos_init, I3_rat)]\n",
"end\n",
"\n",
"function solution_to_pose(sol, k_prec=23)\n",
" # sol = [x, y, z, a, b, c]\n",
" Tr = [sol[1], sol[2], sol[3]]\n",
" a, b, c = sol[4], sol[5], sol[6]\n",
" \n",
" # 1. Définition de la matrice identité 3x3\n",
" I3 = [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]\n",
" \n",
" # 2. On recrée H tel que défini dans quat_to_pos\n",
" H = [ 0.0 a b ;\n",
" -a 0.0 c ;\n",
" -b -c 0.0 ]\n",
" \n",
" # 3. On recrée invIsubH tel que défini dans quat_to_pos\n",
" # (Note l'ordre particulier des variables dans ton code de référence)\n",
" invIsubH = [ 1.0+c^2 a-b*c b+a*c ;\n",
" -a-b*c 1.0+b^2 c-a*b ;\n",
" -b+a*c -c-a*b 1.0+a^2 ]\n",
" \n",
" # 4. Calcul de R selon ta formule : R = (H + I) * invIsubH / den\n",
" den = 1.0 + a^2 + b^2 + c^2\n",
" R = (H + I3) * invIsubH / den\n",
" \n",
" # Si le résultat est toujours \"6 sur 8 faux\", \n",
" # remplace la ligne suivante par : return (Tr, transpose(R))\n",
" return (Tr, R)\n",
"end\n"
]
},
{
"cell_type": "markdown",
"id": "455fc681",
"metadata": {},
"source": [
"## Fonctions de tracé et d'animation"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "2bfb2f9e",
"metadata": {},
"outputs": [],
"source": [
"function draw_hexapod!(ax, b_pts_input, p_pts_input)\n",
" b_obs = b_pts_input isa Observable ? b_pts_input : Observable(b_pts_input)\n",
" p_obs = p_pts_input isa Observable ? p_pts_input : Observable(p_pts_input)\n",
" \n",
" # Tableau pour stocker tous les composants du robot\n",
" robot_plots = []\n",
" \n",
" faces_indices = [TriangleFace(1, 2, 3), TriangleFace(1, 3, 4), TriangleFace(1, 4, 5), TriangleFace(1, 5, 6)]\n",
" b_mesh = GLMakie.lift(pts -> GeometryBasics.Mesh(pts, faces_indices), b_obs)\n",
" p_mesh = GLMakie.lift(pts -> GeometryBasics.Mesh(pts, faces_indices), p_obs)\n",
" \n",
" # Surfaces et Arêtes\n",
" push!(robot_plots, mesh!(ax, b_mesh, color = (:royalblue, 0.15), transparency = true))\n",
" push!(robot_plots, mesh!(ax, p_mesh, color = (:firebrick, 0.15), transparency = true))\n",
"\n",
" idx_edges = [1,2, 2,3, 3,4, 4,5, 5,6, 6,1]\n",
" push!(robot_plots, linesegments!(ax, GLMakie.lift(pts -> pts[idx_edges], b_obs), color = :blue4, linewidth = 2))\n",
" push!(robot_plots, linesegments!(ax, GLMakie.lift(pts -> pts[idx_edges], p_obs), color = :red4, linewidth = 2))\n",
" \n",
" # Vérins\n",
" for i in 1:6\n",
" pb = GLMakie.lift(pts -> pts[i], b_obs)\n",
" pp = GLMakie.lift(pts -> pts[i], p_obs)\n",
" v_v = GLMakie.lift((b, p) -> Vec3f(p - b), pb, pp)\n",
" rot_v = Makie.lift(v -> get_rotation_q(Vec3f(0,0,1), v), v_v)\n",
"\n",
" push!(robot_plots, meshscatter!(ax, GLMakie.lift((b, v) -> b + 0.25f0*v, pb, v_v), \n",
" marker = Cylinder(Point3f(0,0,-0.5), Point3f(0,0,0.5), 1f0),\n",
" markersize = GLMakie.lift(v -> Vec3f(0.06, 0.06, norm(v)*0.5), v_v), \n",
" rotation = rot_v, color = :gray20))\n",
" \n",
" push!(robot_plots, meshscatter!(ax, GLMakie.lift((p, v) -> p - 0.25f0*v, pp, v_v), \n",
" marker = Cylinder(Point3f(0,0,-0.5), Point3f(0,0,0.5), 1f0),\n",
" markersize = GLMakie.lift(v -> Vec3f(0.03, 0.03, norm(v)*0.5), v_v), \n",
" rotation = rot_v, color = :silver))\n",
" end\n",
"\n",
" # Rotules\n",
" push!(robot_plots, meshscatter!(ax, b_obs, markersize = 0.04, color = :gold))\n",
" push!(robot_plots, meshscatter!(ax, p_obs, markersize = 0.04, color = :gold))\n",
"\n",
" # Repère local (Centre, flèches et axes)\n",
" p_center = GLMakie.lift(pts -> sum(pts) / 6, p_obs)\n",
" v_z = GLMakie.lift(pts -> begin v1, v2 = pts[2]-pts[1], pts[3]-pts[1]; normalize(cross(v1, v2)) end, p_obs)\n",
" v_x = GLMakie.lift((pts, c) -> normalize(pts[1] - c), p_obs, p_center)\n",
" v_y = GLMakie.lift((x, z) -> cross(z, x), v_x, v_z)\n",
"\n",
" push!(robot_plots, meshscatter!(ax, p_center, markersize = 0.07, color = :black))\n",
" \n",
" p_arrows = arrows!(ax, GLMakie.lift(c -> [c, c, c], p_center), \n",
" GLMakie.lift((x, y, z) -> [x, y, z] .* 0.4f0, v_x, v_y, v_z),\n",
" color = [:red, :green, :blue], linewidth = 0.03, arrowsize = 0.06, quality = 8)\n",
" push!(robot_plots, p_arrows)\n",
"\n",
" return robot_plots # On retourne la liste complète\n",
"end\n",
"\n",
"# Ajout de z_init dans les arguments (par exemple avec une valeur par défaut à 1.5)\n",
"function afficher_hexapode(toute_la_trajectoire, poses_mgd, BASE_RAT, PLAT_LOC_RAT, courbe_consigne, z_init=1.5)\n",
" GLMakie.activate!(inline=false)\n",
"\n",
" # --- 1. PRÉPARATION ---\n",
" base_pts_coords = [Point3f(Float32.(BASE_RAT[i, :])...) for i in 1:6]\n",
" pts_trace = [Point3f(Float32.(p)...) for p in courbe_consigne]\n",
" nb_pas = length(toute_la_trajectoire)\n",
"\n",
" fig = Figure(size = (1100, 900), backgroundcolor = :white)\n",
" ax = LScene(fig[1, 1], show_axis = true)\n",
" \n",
" ax_bar = Axis(fig[1, 2], width = 70, xticks = (1:6, string.(1:6)), title = \"L (m)\")\n",
" ylims!(ax_bar, 0.0, 2.5)\n",
"\n",
" # --- 2. OBSERVABLES ---\n",
" idx_pas = Observable(1)\n",
" idx_sol = Observable(1)\n",
" \n",
" obs_p_robot = Observable(Vector{Point3f}(toute_la_trajectoire[1][1]))\n",
" obs_lengths = Observable(zeros(Float32, 6))\n",
" obs_pt_rouge = Observable(pts_trace[1])\n",
" obs_pts_bleus = Observable([sum(s)/6 for s in toute_la_trajectoire[1]])\n",
"\n",
" # --- 3. ÉLÉMENTS GRAPHIQUES ---\n",
" centres_all = [sum(s)/6 for k in 1:nb_pas for s in toute_la_trajectoire[k]]\n",
" couleurs_cloud = [Float32(k) for k in 1:nb_pas for s in toute_la_trajectoire[k]]\n",
" \n",
" plt_cloud = meshscatter!(ax, centres_all, markersize = 0.012, color = couleurs_cloud, \n",
" colormap = :viridis, alpha = 0.2, visible = false)\n",
" \n",
" plt_trace = lines!(ax, pts_trace, color = :orange, linewidth = 5, visible = false)\n",
" plt_pt_rouge = meshscatter!(ax, obs_pt_rouge, markersize = 0.04, color = :red, visible = false)\n",
" plt_pts_bleus = meshscatter!(ax, obs_pts_bleus, markersize = 0.03, color = :blue, visible = false)\n",
"\n",
" robot_plots = draw_hexapod!(ax, Observable(base_pts_coords), obs_p_robot)\n",
" barplot!(ax_bar, 1:6, obs_lengths, color = :royalblue)\n",
"\n",
" # --- 4. INTERFACE ---\n",
" gl_ctrls = fig[2, 1:2] = GridLayout(tellheight = true)\n",
" \n",
" # Utilisation de z_init pour le startvalue du slider Z\n",
" lsgrid_trans = SliderGrid(gl_ctrls[1, 1], \n",
" (label=\"X\", range=-0.5:0.01:0.5, startvalue=0.0), \n",
" (label=\"Y\", range=-0.5:0.01:0.5, startvalue=0.0), \n",
" (label=\"Z\", range=0.5:0.01:2.5, startvalue=z_init)) # <-- Modifié ici\n",
" \n",
" lsgrid_rot = SliderGrid(gl_ctrls[1, 2], \n",
" (label=\"R\", range=-0.5:0.01:0.5, startvalue=0.0), \n",
" (label=\"P\", range=-0.5:0.01:0.5, startvalue=0.0), \n",
" (label=\"W\", range=-0.5:0.01:0.5, startvalue=0.0))\n",
"\n",
" rowgap!(lsgrid_trans.layout, 5)\n",
" rowgap!(lsgrid_rot.layout, 5)\n",
"\n",
" nav_grid = gl_ctrls[2, 1:2] = GridLayout(halign = :center, colgap = 10)\n",
" \n",
" btn_prev = Button(nav_grid[1, 1], label=\"<\", width=30)\n",
" label_info = Label(nav_grid[1, 2], \"S1\", font=:bold, width=60)\n",
" btn_next = Button(nav_grid[1, 3], label=\">\", width=30)\n",
" \n",
" menu_suivi = Menu(nav_grid[1, 4], options = [\"Index Fixe\", \"Plus Proche\"], default = \"Index Fixe\", width = 120)\n",
" \n",
" Label(nav_grid[1, 5], \"Pas :\", font = :bold) \n",
" sl_pas = Slider(nav_grid[1, 6], range = 1:nb_pas, startvalue = 1, width = 150)\n",
" label_pas = Label(nav_grid[1, 7], \"1/$nb_pas\", font = :bold, width = 80)\n",
" \n",
" tgl_robot = Toggle(nav_grid[1, 8], active=true); Label(nav_grid[1, 9], \"Robot\")\n",
" tgl_mgd = Toggle(nav_grid[1, 10], active=false); Label(nav_grid[1, 11], \"Nuage\")\n",
" tgl_trace = Toggle(nav_grid[1, 12], active=false); Label(nav_grid[1, 13], \"Trace\")\n",
"\n",
" # --- 5. LOGIQUE ---\n",
"\n",
" function rafraichir(ancien_robot_pos=nothing)\n",
" t = idx_pas[]\n",
" sols = toute_la_trajectoire[t]\n",
" \n",
" if menu_suivi.selection[] == \"Plus Proche\" && ancien_robot_pos !== nothing\n",
" idx_sol[] = argmin([norm(sum(s)/6 - sum(ancien_robot_pos)/6) for s in sols])\n",
" end\n",
" \n",
" s = clamp(idx_sol[], 1, length(sols))\n",
" idx_sol.val = s \n",
" \n",
" obs_p_robot[] = sols[s]\n",
" obs_lengths[] = [Float32(norm(sols[s][i] - base_pts_coords[i])) for i in 1:6]\n",
" obs_pt_rouge[] = pts_trace[t]\n",
" obs_pts_bleus[] = [sum(sol)/6 for sol in sols]\n",
" \n",
" label_info.text[] = \"S $s/$(length(sols))\"\n",
" label_pas.text[] = \"$t/$nb_pas\"\n",
" end\n",
"\n",
" onany([s.value for s in [lsgrid_trans.sliders...; lsgrid_rot.sliders...]]...) do x, y, z, r, p, w\n",
" trans_rat = [to_dyadic_rat(x, 23), to_dyadic_rat(y, 23), to_dyadic_rat(z, 23)]\n",
" \n",
" S = [ 0//1 -to_dyadic_rat(w,23) to_dyadic_rat(p,23) ; \n",
" to_dyadic_rat(w,23) 0//1 -to_dyadic_rat(r,23) ; \n",
" -to_dyadic_rat(p,23) to_dyadic_rat(r,23) 0//1 ]\n",
" \n",
" I3 = [1//1 0//1 0//1; 0//1 1//1 0//1; 0//1 0//1 1//1]\n",
" R_rat = (I3 - S) * inv(I3 + S)\n",
" \n",
" nouvelle_pose = [Point3f(Float32.((R_rat * collect(PLAT_LOC_RAT[i, :]) + trans_rat))...) for i in 1:6]\n",
" obs_p_robot[] = nouvelle_pose\n",
" obs_lengths[] = [Float32(norm(nouvelle_pose[i] - base_pts_coords[i])) for i in 1:6]\n",
" \n",
" label_info.text[] = \"MANUEL\"\n",
" end\n",
"\n",
" on(sl_pas.value) do t\n",
" p_prev = obs_p_robot[]\n",
" idx_pas[] = Int(t)\n",
" rafraichir(p_prev)\n",
" end\n",
"\n",
" on(btn_next.clicks) do _; idx_sol[] += 1; rafraichir(); end\n",
" on(btn_prev.clicks) do _; idx_sol[] -= 1; rafraichir(); end\n",
" on(menu_suivi.selection) do _; rafraichir(obs_p_robot[]); end\n",
"\n",
" on(tgl_mgd.active) do v; plt_cloud.visible = v; plt_pts_bleus.visible = v; end\n",
" on(tgl_trace.active) do v; plt_trace.visible = v; plt_pt_rouge.visible = v; end\n",
" on(tgl_robot.active) do v; for p in robot_plots; p.visible = v; end; end\n",
"\n",
" rowsize!(fig.layout, 2, Fixed(120)) \n",
" rafraichir()\n",
" \n",
" return fig\n",
"end"
]
},
{
"cell_type": "markdown",
"id": "31baefa7",
"metadata": {},
"source": [
"## Création du robot"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "80283f26",
"metadata": {},
"outputs": [],
"source": [
"BASE_RAT, PLAT_LOC_RAT = generate_geometry()\n",
"\n",
"PLAT_INIT = copy(PLAT_LOC_RAT)\n",
"\n",
"z_repos = Rational{BigInt}(3//2)\n",
"\n",
"for i in 1:6\n",
" PLAT_INIT[i, 3] += z_repos\n",
"end\n",
"\n",
"LEN_INIT=mgi_rational([0//1, 0//1, z_repos], Matrix{Rational{BigInt}}(I, 3, 3),BASE_RAT, PLAT_LOC_RAT)"
]
},
{
"cell_type": "markdown",
"id": "b1576f0e",
"metadata": {},
"source": [
"## Dessin du robot et résolution du modèle géométrique inverse"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "0aa5a656",
"metadata": {},
"outputs": [],
"source": [
"\n",
"global poses_rat = generate_static_initial_pose(z_repos)\n",
"global trace_points = [Point3f(Float32.(p[1])...) for p in poses_rat]\n",
"global historique_longueurs = [mgi_rational(p, R,BASE_RAT,PLAT_LOC_RAT) for (p, R) in poses_rat]\n",
"global toute_la_trajectoire=[[[Point3f(PLAT_INIT[i, :]) for i in 1:6]]]"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "0df2182a",
"metadata": {},
"outputs": [],
"source": [
"fig=afficher_hexapode(toute_la_trajectoire, poses_rat, BASE_RAT, PLAT_LOC_RAT,trace_points)\n",
"display(fig)"
]
},
{
"cell_type": "markdown",
"id": "fed2e5c7",
"metadata": {},
"source": [
"## Resolution du modèle géométrique direct"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "86a7cd9b",
"metadata": {},
"outputs": [],
"source": [
"prec=23;\n",
"sys_quat_generic = Hexapod.eqs_quat(BASE_RAT,PLAT_LOC_RAT)\n",
"# attrappe les positions pour le premier jeu de longeurs de jambes\n",
"sys_spec = deepcopy(sys_quat_generic)\n",
"set_lengths=[[Rational{BigInt}(historique_longueurs[1][i]) for i in 1:6]]\n",
"Hexapod.subs_lengths!(sys_spec, set_lengths[1])\n",
"# Résolution\n",
"solutions_mgd = Hexapod.midpoints(Hexapod.DKP(sys_spec, prec))\n",
"# Conversion des quaternions de solutions en points 3D pour l'affichage\n",
"# Supposons que solutions_mgd est une liste de vecteurs [x, y, z, a, b, c]\n",
"# 1. Convertir tous les vecteurs solutions en couples (Tr, R)\n",
"poses_rat = [solution_to_pose(sol) for sol in solutions_mgd]\n",
"# 2. Pour le MGD, on considère qu'on est au \"Temps 1\" \n",
"# toute_la_trajectoire[1] contiendra la liste de TOUTES les solutions\n",
"solutions_points = map(poses_rat) do (pos_r, R_rat)\n",
" # Calcul des 6 points pour CETTE solution précise\n",
" [Point3f(Float32.(R_rat * collect(PLAT_LOC_RAT[i, :]) + pos_r)...) for i in 1:6]\n",
"end\n",
"# Format final : [ [Sol1, Sol2, Sol3, ...] ]\n",
"toute_la_trajectoire = [ solutions_points ]\n",
"# 3. Mise à jour des variables pour l'affichage\n",
"poses_rat_tmp = generate_static_initial_pose(z_repos)\n",
"trace_points = [Point3f(Float32.(p[1])...) for p in poses_rat_tmp]"
]
},
{
"cell_type": "markdown",
"id": "23e9131a",
"metadata": {},
"source": [
"### Déroulons la résolution"
]
},
{
"cell_type": "markdown",
"id": "66f0479e",
"metadata": {},
"source": [
"#### Equations"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "0d4deeac",
"metadata": {},
"outputs": [],
"source": [
"using AbstractAlgebra\n",
"import DynamicPolynomials as DP\n",
"ma_liste_polynomes=sys_spec\n",
"# 1. Extraction des noms de variables depuis votre système DynamicPolynomials\n",
"vars_dp = variables(ma_liste_polynomes)\n",
"var_names = string.(vars_dp)\n",
"\n",
"# 2. Création de l'anneau avec la bonne syntaxe\n",
"# QQ est le corps des rationnels (compatible Rational{BigInt})\n",
"R_aa, vars_aa = AbstractAlgebra.polynomial_ring(QQ, var_names)\n",
"# Appel de la fonction\n",
"systeme_aa = convertir_vers_aa(ma_liste_polynomes, R_aa)"
]
},
{
"cell_type": "markdown",
"id": "0568e534",
"metadata": {},
"source": [
"#### Simplification"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "4718b2bf",
"metadata": {},
"outputs": [],
"source": [
"rur,sep=zdim_parameterization(systeme_aa,verbose=false,get_separating_element=true);\n",
"rur"
]
},
{
"cell_type": "markdown",
"id": "4c77c0fc",
"metadata": {},
"source": [
"#### Résolution"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "ba4f6814",
"metadata": {},
"outputs": [],
"source": [
"iso=RS.rs_isolate(rur,sep,output_precision=Int32(23))"
]
},
{
"cell_type": "markdown",
"id": "f39dacbc",
"metadata": {},
"source": [
"### Tracé"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "83b73201",
"metadata": {},
"outputs": [],
"source": [
"fig=afficher_hexapode(toute_la_trajectoire, poses_rat, BASE_RAT, PLAT_LOC_RAT,trace_points)\n",
"display(fig)"
]
},
{
"cell_type": "markdown",
"id": "9ae70218",
"metadata": {},
"source": [
"## Planification de trajectoires"
]
},
{
"cell_type": "markdown",
"id": "0cdbcdf6",
"metadata": {},
"source": [
"### Génération d'une trajectoire"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "24ae8498",
"metadata": {},
"outputs": [],
"source": [
"\"\"\"\n",
" generate_lissajous_rational(n_points::Int, a::Real, b::Real, c::Real, k::Int=23)\n",
"\n",
"Génère une trajectoire de Lissajous fermée.\n",
"- a, b, c : fréquences entières pour X, Y, Z (ex: 1, 2, 3)\n",
"- n_points : nombre de points sur la courbe\n",
"- k : précision pour la conversion en rationnels dyadiques\n",
"\"\"\"\n",
"\n",
"function generate_lissajous_rational_full(n_points::Int, a::Int, b::Int, c::Int, wa::Int, wb::Int, wc::Int, k::Int=23)\n",
" # On stocke maintenant des couples (translation, rotation)\n",
" trajectoire_pose = []\n",
" dt = 2π / (n_points - 1)\n",
" \n",
" for i in 1:n_points\n",
" t = (i - 1) * dt\n",
" \n",
" # --- TRANSLATION (Rationnelle) ---\n",
" x_r = to_dyadic_rat(0.5 * sin(a * t), k)\n",
" y_r = to_dyadic_rat(0.5 * sin(b * t), k)\n",
" z_r = to_dyadic_rat(0.5 * cos(c * t) + 1.0, k)\n",
" pos_r = [x_r, y_r, z_r]\n",
" \n",
" # --- ORIENTATION (Rationnelle via Cayley) ---\n",
" # On définit des petits angles de rotation\n",
" u = 0.1 * sin(wa * t)\n",
" v = 0.1 * sin(wb * t)\n",
" w = 0.1 * sin(wc * t)\n",
" \n",
" # Matrice de rotation rationnelle (approximation de Cayley)\n",
" # R = (I - S) * inv(I + S) où S est une matrice antisymétrique\n",
" S = [ 0//1 -to_dyadic_rat(w,k) to_dyadic_rat(v,k) ;\n",
" to_dyadic_rat(w,k) 0//1 -to_dyadic_rat(u,k) ;\n",
" -to_dyadic_rat(v,k) to_dyadic_rat(u,k) 0//1 ]\n",
" \n",
" I3 = [1//1 0//1 0//1; 0//1 1//1 0//1; 0//1 0//1 1//1]\n",
" R_rat = (I3 - S) * inv(I3 + S)\n",
" \n",
" push!(trajectoire_pose, (pos_r, R_rat))\n",
" end\n",
" return trajectoire_pose\n",
"end"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "37dea693",
"metadata": {},
"outputs": [],
"source": [
"# 1. Génération avec ta fonction\n",
"poses_rat = generate_lissajous_rational_full(300, 1, 2, 3,1,1,2) \n",
"# 2. On remplit TA variable trace_points pour l'affichage\n",
"trace_points = [Point3f(Float32.(p[1])...) for p in poses_rat]\n",
"historique_longueurs = [mgi_rational(p, R,BASE_RAT,PLAT_LOC_RAT) for (p, R) in poses_rat]\n",
"toute_la_trajectoire = map(1:length(poses_rat)) do k\n",
" pos_r, R_rat = poses_rat[k] \n",
" # On calcule la position des 6 points de la plateforme pour cette pose\n",
" # Formule : P_global = Rotation * P_local + Translation\n",
" points_au_pas_k = [R_rat * collect(PLAT_LOC_RAT[i, :]) + pos_r for i in 1:6]\n",
" # On convertit en Point3f pour Makie et on encapsule dans une liste de solutions\n",
" return [ [Point3f(Float32.(p)...) for p in points_au_pas_k] ]\n",
"end"
]
},
{
"cell_type": "markdown",
"id": "89eb3bcb",
"metadata": {},
"source": [
"### Tracé"
]
},
{
"cell_type": "markdown",
"id": "333f592b",
"metadata": {},
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"id": "c5bd0a84",
"metadata": {},
"outputs": [],
"source": [
"fig=afficher_hexapode(toute_la_trajectoire, poses_rat, BASE_RAT, PLAT_LOC_RAT,trace_points)\n",
"display(fig)"
]
},
{
"cell_type": "markdown",
"id": "be4b1083",
"metadata": {},
"source": [
"### Calcul des MGD pour chaque jeu de longueurs de jambes"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "b6124438",
"metadata": {},
"outputs": [],
"source": [
"@everywhere begin\n",
" sys_quat_generic = $sys_quat_generic\n",
" prec = $prec\n",
" BASE_RAT = $BASE_RAT\n",
" PLAT_LOC_RAT = $PLAT_LOC_RAT\n",
" historique_longueurs = $historique_longueurs\n",
"end"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "fa2a7703",
"metadata": {},
"outputs": [],
"source": [
"println(\"Calcul du MGD en multi-processus sur $(nprocs()-1) workers...\")\n",
"\n",
"# pmap retourne directement un tableau avec tous les résultats\n",
"toute_la_trajectoire = pmap(1:length(historique_longueurs)) do k\n",
" try\n",
" # Conversion des longueurs en rationnels BigInt pour la précision\n",
" longs_k = [Rational{BigInt}(historique_longueurs[k][i]) for i in 1:6]\n",
" \n",
" # On travaille sur une copie locale au processus (isolée)\n",
" sys_spec = deepcopy(sys_quat_generic)\n",
" Hexapod.subs_lengths!(sys_spec, longs_k)\n",
" \n",
" # Résolution du système polynomial (DKP)\n",
" res_dkp = Hexapod.DKP(sys_spec, prec)\n",
" \n",
" # Récupération des points milieux des intervalles de solution\n",
" l_quat_sols = Hexapod.midpoints(res_dkp)\n",
" \n",
" # Conversion des quaternions de solutions en points 3D pour l'affichage\n",
" sols_au_pas_k = []\n",
" for sol in l_quat_sols\n",
" pts_bruts = Hexapod.quat_to_pos(sol,PLAT_LOC_RAT )\n",
" push!(sols_au_pas_k, [Point3f(p) for p in pts_bruts])\n",
" end\n",
" \n",
" return sols_au_pas_k # Ce résultat remonte dans le tableau final\n",
" \n",
" catch e\n",
" # Si un point échoue (ex: pas de solution réelle), on retourne vide\n",
" return [] \n",
" end\n",
"end\n",
"\n",
"# Vérification du résultat\n",
"nb_reussites = count(x -> !isempty(x), toute_la_trajectoire)\n",
"println(\"Calcul terminé : $nb_reussites / $(length(toute_la_trajectoire)) points calculés avec succès.\")"
]
},
{
"cell_type": "markdown",
"id": "80317f3e",
"metadata": {},
"source": [
"### Affichage"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "6aa13d03",
"metadata": {},
"outputs": [],
"source": [
"fig4=afficher_hexapode(toute_la_trajectoire, poses_rat, BASE_RAT, PLAT_LOC_RAT,trace_points)\n",
"display(fig4)"
]
},
{
"cell_type": "markdown",
"id": "ed949492",
"metadata": {},
"source": [
"### Suivi certifié de trajectoires"
]
},
{
"cell_type": "markdown",
"id": "02f72220",
"metadata": {},
"source": [
"#### Génération des consignes"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "e6affc71",
"metadata": {},
"outputs": [],
"source": [
"set_lengths = [[Rational{BigInt}(historique_longueurs[k][i]) for i in 1:6] for k in 1:length(historique_longueurs)]"
]
},
{
"cell_type": "markdown",
"id": "c78e81bd",
"metadata": {},
"source": [
"#### Choix de la position initiale et suivi de la trajectoire"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "640cb89d",
"metadata": {},
"outputs": [],
"source": [
"#on choisit un point de départ puis on essaye de suivre la trajectoire\n",
"B_list = Hexapod.follow_path(BASE_RAT, PLAT_LOC_RAT, solutions_mgd[1], set_lengths, alg=\"LACE-2\")\n",
"# conversion\n",
"toute_la_trajectoire = [ [ [Point3f(B_list[k][i]) for i in 1:6] ] for k in 1:length(B_list) ]"
]
},
{
"cell_type": "markdown",
"id": "084524f2",
"metadata": {},
"source": [
"#### Affichage"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "bfd4595a",
"metadata": {},
"outputs": [],
"source": [
"fig5=afficher_hexapode(toute_la_trajectoire, poses_rat, BASE_RAT, PLAT_LOC_RAT,trace_points)\n",
"display(fig5)"
]
},
{
"cell_type": "markdown",
"id": "7a9ba568",
"metadata": {},
"source": [
"## Quitter les workers"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "55f3aa77",
"metadata": {},
"outputs": [],
"source": [
"#rmprocs(workers())"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "e8949d72",
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"id": "57a4f3d1",
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"id": "c14b034d",
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Julia 1.12.4",
"language": "julia",
"name": "julia-1.12"
},
"language_info": {
"file_extension": ".jl",
"mimetype": "application/julia",
"name": "julia",
"version": "1.12.4"
}
},
"nbformat": 4,
"nbformat_minor": 5
}