Warning

This page is a work in progress and is not yet complete.

Orbita kinematics

The Orbita 3D actuator is a parallel mechanism composed of 3 active arms and 3 passive arms. The active arms are driven by motors and the passive arms are connected to the platform. The platform is the output of the mechanism and can be inclined in 3D space.

This page describes a mathematical model of the Orbita mechanism, which allows the calculation of the platform's position, velocity, and torque based on the motor positions.

Note

Some references to understand the kinematics of Orbita:
- "On the direct kinematics of a class of spherical three-degree-of freedom parallel manipulators", Gosselin et al. 1992
- "Kinematic analysis, optimization and programming of parallel robotic manipulator", Gosselin 1985

Model definition

axis

Schéma du -ème bras d'Orbita partant du moteur (rotation autour du vecteur unitaire ), le premier bras "montant" (rotation autour du vecteur unitaire ) et le bras "coudé" (rotation autour du vecteur unitaire ).

correspond à l'angle de rotation du moteur (par rapport au repère fixe ).

correspont à l'angle de rotation du bras "coudé" par rapport au bras "montant"

On note que la rotation autour de n'a pas besoin d'être connue, les 3 vecteurs permettent de déterminer l'orientation de la plateforme.

Cinematique inverse

On utilise ici la convention Denavit-Hartenberg pour définir les repères correspondant à chaque articulation.

Le repère du moteur est confondu avec le repère fixe pour

Si on suit la convention, les paramètres et de Denavit-Hartenberg sont nuls. Les (angles entre les axes consécutifs) valent:

et sont des paramètre mécaniques, pour Orbita v1 on a: et

Les (angles de rotation autour des axes ) valents:

Si on ne considère que l'orientation, on obtient une matrice de rotation pour passer de vers :

De même la matrice de rotation passant de vers :

Du coup le vecteur

Le problème de la cinématique inverse revient à trouver les connaissant l'orientation de la plateforme. Sachant que l'orientation de la plateforme est déterminée entièrement par les vecteurs .

Or on a pour

Il faut donc résoudre le système:

Donc si

On note qu'on a aussi:

On a des et , ce qui n'est pas pratique. Mais du coup on peut utiliser le trick de la tangente du demi angle avec:

et donc

on tombe bien sur la même expression que Gosselin:

avec

donc avec des solutions de a forme:

, pour

On a donc une combinaison de 2 solutions par "bras" ce qui fait 8 configurations possibles.

Example python implementation

def SolIK(v, alpha1, alpha2):
    T_i=[]
    for i in range(3):
        Ai=-np.sin(alpha1)*v[i][0]-np.cos(alpha1)*v[i][2]-np.cos(alpha2)
        Bi=np.sin(alpha1)*v[i][1]
        Ci=np.sin(alpha1)*v[i][0]-np.cos(alpha1)*v[i][2]-np.cos(alpha2)
        sol1=(-Bi+np.sqrt(Bi**2-Ai*Ci))/Ai
        sol2=(-Bi-np.sqrt(Bi**2-Ai*Ci))/Ai
        T_i.append([sol1,sol2])
    return T_i

Jacobienne

Note

Ref pour certains détails de calcul: "On the Numerical Solution of the Inverse Kinematic Problem", Angeles 1980.

La Jacobienne est la matrice des dérivées partielles qui lie les vitesses articulaires aux vitesses angulaires (cartésiennes) d'Orbita. En temps normal pour un mécanisme série on étudie la Jacobienne qui donne: , mais comme dans le cas des mécanismes parallèles le problème de la cinématique inverse est beaucoup plus simple que la directe, on étudie ici la Jacobienne inverse, que l'on va quand même appeler .

On a: avec le (pseudo-)vecteur vitesse angulaire (qui est l'axe de rotation instantané et dont la norme représente la vitesse) et la vitesse "articulaire" (les moteurs).

On a vu précédemment que la cinématique inverse était obtenue avec:

Si on dérive cette expression de chaque coté on a:

On a vu aussi que: , avec la matrice de rotation entre une configuration "initiale" et la configuration de la plateforme (rappel: les sont les 3 vecteurs de la plateforme)

Si on défini avec la matrice anti-symétrique telle que: pour tout .

On a

D'après la notation de Angeles (1980), étant le vecteur (3D) invariant de A.

Note

Du coup est vaguement l'axe de rotation, le lien avec le vecteur rotation unitaire au sens de Euler (représentation axe-angle): avec l'angle de rotation autour de .

Angeles (1980) défini avec et le symbole de "Levi-Civita", en suivant la notation d'Einstein, soit en 3D: Donc

est le "tenseur de vitesse angulaire", c'est à dire une matrice de "rotation infinitésimale".

, on a bien

Avec du coup , un vecteur quelconque subissant une rotation.

Si on écrit la dérivée des vecteurs :

On a donc: avec dans le cas Orbita (moteurs colinéaires)

Donc si on dérive et avec on a:

Donc en injectant dans on a

Donc:

Comme on a , la -ème ligne de la Jacobienne est:

La Jacobienne complete est du coup:

On note que la Jacobienne dépend de mais pas de . Elle dépend de l'orientation de la plateforme mais aussi des .

Example python implementation

def get_jacobian(v,thetas,alpha1=np.radians(50)):
    sa1=np.sin(alpha1)
    ca1=np.cos(alpha1)
    st1=np.sin(thetas[0])
    ct1=np.cos(thetas[0])
    st2=np.sin(thetas[1])
    ct2=np.cos(thetas[1])
    st3=np.sin(thetas[2])
    ct3=np.cos(thetas[2])

    row1_denom=-sa1*st1*v[0][0]+sa1*ct1*v[0][1]
    row2_denom=-sa1*st2*v[1][0]+sa1*ct2*v[1][1]
    row3_denom=-sa1*st3*v[2][0]+sa1*ct3*v[2][1]

    row1=[(sa1*st1*v[0][2]+ca1*v[0][1])/row1_denom, (-sa1*ct1*v[0][2]-ca1*v[0][0])/row1_denom,1]
    row2=[(sa1*st2*v[1][2]+ca1*v[1][1])/row2_denom, (-sa1*ct2*v[1][2]-ca1*v[1][0])/row2_denom,1]
    row3=[(sa1*st3*v[2][2]+ca1*v[2][1])/row3_denom, (-sa1*ct3*v[2][2]-ca1*v[2][0])/row3_denom,1]
    return np.array([row1,row2,row3])

Cinématique Directe

On cherche à déterminer l'orientation de la plateforme (les ) en fonction des .

Pour ça on peut se contenter de trouver les vu qu'on a vu précédemment (cf. cinématique inverse) que: avec matrice identité et

Par souci de simplicité on va définir , , ...

On a comme conditions:

qu'on a vu précédemment.

pour et puisque les sont à 120° deux à deux.

puisque les sont des vecteurs unité

et

puisque les sont sur le même plan et à 120°.

Donc on va faire un gros système d'équations avec tout ça et résoudre numériquement

Example python implementation

def forward_kinematics(thetas, initial_guess=(0,1,0,1,0,1), yaw_offset=0):
    disk0=np.array([0,np.radians(120),np.radians(-120)])
    #thetas+=disk0
    theta1,theta2,theta3=thetas
    p_subs={sa1:sin(rad(50)), ca1:cos(rad(50)), sa2:sin(rad(90)), ca2:cos(rad(90)),ct1:cos(theta1),st1:sin(theta1),ct2:cos(theta2),st2:sin(theta2),ct3:cos(theta3),st3:sin(theta3)}
    eqs=[]
    eqs=[Eq(v(1).dot(v(2)).expand().subs(p_subs),-1.0/2.0),Eq(v(2).dot(v(3)).expand().subs(p_subs),-1.0/2.0),Eq(v(3).dot(v(1)).expand().subs(p_subs),-1.0/2.0)]
    eqs.append(Eq(w(1).dot(v(1)),ca2).subs(p_subs))
    eqs.append(Eq(w(2).dot(v(2)),ca2).subs(p_subs))
    eqs.append(Eq(w(3).dot(v(3)),ca2).subs(p_subs))
    eqs.append(Eq(v(1).dot(v(1)),1).subs(p_subs))
    eqs.append(Eq(v(2).dot(v(2)),1).subs(p_subs))
    eqs.append(Eq(v(3).dot(v(3)),1).subs(p_subs))
    eqs.append((v(1)+v(2)+v(3)).subs(p_subs))
    eqs=[e for e in eqs if e not in [True, False] ]

    sols=nsolve(eqs,(cp1,sp1,cp2,sp2,cp3,sp3),initial_guess,dict=True,tol=1e-10)

    sp1_n=float(sols[0][sp1])
    sp2_n=float(sols[0][sp2])
    sp3_n=float(sols[0][sp3])

    cp1_n=float(sols[0][cp1])
    cp2_n=float(sols[0][cp2])
    cp3_n=float(sols[0][cp3])

    phi1=np.arctan2(sp1_n,cp1_n)
    phi2=np.arctan2(sp2_n,cp2_n)
    phi3=np.arctan2(sp3_n,cp3_n)
    print(f'phi1: {phi1} ({np.degrees(phi1)}°) phi2: {phi2} ({np.degrees(phi2)}°) phi3: {phi3} ({np.degrees(phi3)}°)')
    res_subs={sa1:sin(rad(50)), ca1:cos(rad(50)), sa2:sin(rad(90)), ca2:cos(rad(90)),cp1:cos(phi1),sp1:sin(phi1),ct1:cos(theta1),st1:sin(theta1),cp2:cos(phi2),sp2:sin(phi2),ct2:cos(theta2),st2:sin(theta2),cp3:cos(phi3),sp3:sin(phi3),ct3:cos(theta3),st3:sin(theta3)}

    v1sol=np.array(Matrix(v(1).subs(res_subs)).evalf())
    v2sol=np.array(Matrix(v(2).subs(res_subs)).evalf())
    v3sol=np.array(Matrix(v(3).subs(res_subs)).evalf())
    #donc bizarement on a [y, -x, z] (rotation de 90°)
    v1sol=np.array([-v1sol[1][0],v1sol[0][0],v1sol[2][0]],dtype=np.float64)
    v2sol=np.array([-v2sol[1][0],v2sol[0][0],v2sol[2][0]],dtype=np.float64)
    v3sol=np.array([-v3sol[1][0],v3sol[0][0],v3sol[2][0]],dtype=np.float64)

    V_mat=np.matrix([v1sol,v2sol,v3sol])
    #config de base
    b1=np.array([np.cos(np.radians(0)),np.sin(np.radians(0)),0])
    b2=np.array([np.cos(np.radians(120)),np.sin(np.radians(120)),0])
    b3=np.array([np.cos(np.radians(-120)),np.sin(np.radians(-120)),0])

    #dans le repère Orbita:
    yaw_offset=-np.pi/2.0
    #yaw_offset=0
    roffset=Rotation.from_euler('xyz',[0,0,-yaw_offset])
    Qoffset=np.array(roffset.as_matrix())
    b1=Qoffset.dot(b1)
    b2=Qoffset.dot(b2)
    b3=Qoffset.dot(b3)
    B_mat=np.matrix([b1,b2,b3])
    print(B_mat)
    print(V_mat)
    ra,rms=Rotation.align_vectors(V_mat,B_mat)
    return ra.as_euler('xyz', degrees=True)