Comment déterminer la rotation de 2 segments de bras en fonction du point cible?

Question

J'ai un bras robotisé à 2 segments qui doit atteindre un point spécifique en configurant ses articulations (angles).

Voici un dessin de la configuration:

Mon bras est positionné au milieu de l'esquisse donc mon origine est (largeur/2,0). Voici les valeurs que je connais: Longueurs: Premier segment (L1): 140 mm. Deuxième segment (L2): 180 mm. Distance entre le point d'origine et le point cible. (en utilisant dist()).

Ces longueurs forment un triangle. Donc, en utilisant des lois de trigonométrie, je peux trouver les angles du triangle qui sont les angles que je veux utiliser pour positionner mon bras afin qu'il atteigne le point cible.

Maintenant, je veux dessiner le triangle sur l'écran en utilisant l'environnement de traitement pour la simulation. J'ai appliqué quelques transformations pour dessiner le triangle mais je n'obtiens pas le dessin correct.

Ceci est mon code:

void draw(){ 
background(100); 
translate(width/2,0); // origin 

// target 
float penX = mouseX-width/2; 
float penY = mouseY; 

// draw points 
ellipse(penX, penY, 20, 20); 
ellipse(0,0,20,20); 
line(0,0,penX,penY); 

// distance from origin to target 
float distance = dist(0,0,penX,penY); 

// first angle (part of S1) 
float b = asin(penY/distance); 
arc(0,0,100,100,0,b); 

// second angle (part of S1) 
float a = acos((pow(L1,2)+pow(distance,2)-pow(L2,2))/(2*L1*distance)); 

// Angle representing first joint 
S1 = a + b; // first joint angle 

// Angle representing second joint 
S2 = acos((pow(L1,2)+pow(L2,2)-pow(distance,2))/(2*L1*L2)); //second joint angle 
//Drawing Triangle: 
rotate(S1); 
line(0,0,120,0); 
translate(120,0); 

rotate(-S2); 
line(0,0,180,0);} 

J'espère que mon écriture est claire et désolé pour toute confusion.

Answer 1

Cela va être beaucoup plus impliqué que ce que vous décrivez. Si vous connaissez le point de départ, la longueur des segments de bras et le point cible, vous pouvez utiliser inverse kinematics pour trouver les angles dont vos segments ont besoin pour atteindre le point cible. Ce n'est pas aussi simple que d'appliquer la trigonométrie simple.

Cela dit, si vous déjà connaître les angles de l'un des segments, vous pouvez en effet utiliser TRIG de base pour déterminer le dernier:

float midX; 
float midY; 

float length1 = 100; 
float length2 = 100; 

float angle1; 

void setup(){ 
    size(500, 500); 
    midX = width/2; 
    midY = height/2; 
} 

void keyPressed(){ 
    angle1 += .05; 
} 

void draw() { 

    background(255); 

    //we already know the angle of the first segment 
    //so we can get the end point of the first segment 
    float endX1 = midX + cos(angle1)*length1; 
    float endY1 = midX + sin(angle1)*length2; 

    //we don't know the angle of the second segment 
    //but we can point it towards a goal point 
    float deltaY = mouseY - endY1; 
    float deltaX = mouseX - endX1; 
    float angle2 = atan2(deltaY, deltaX); 

    //now we figured out the angle, 
    //we can get the end point of the second segment 
    float endX2 = endX1 + cos(angle2)*length2; 
    float endY2 = endY1 + sin(angle2)*length2; 

    //draw the segments 
    line(midX, midY, endX1, endY1); 
    line(endX1, endY1, endX2, endY2); 
} 

Cependant, vous ne pouvez pas figure out tous de vos angles en utilisant simplement trig. Pour cela, vous devez utiliser quelque chose de plus impliqué comme la cinématique inverse.

Vous pouvez google quelque chose comme « cinématique inverse de traitement » pour un groupe de résultats, mais here est un excellent exemple:

int sx,sy,ex,ey,hx,hy,hxo,hyo; 
int armLength,ua,la; 
float uad, lad; 
void setup(){ 
    size(500,500); 
    background(255, 224, 150); 
    sx = width/2; 
    sy = height/2; 
    armLength = int(width/5); 
} 

void draw(){ 
    fill(255); 
    rect(0,0,width,height); 
    upperArm(); 
} 

void upperArm(){ 
    int dx = mouseX - sx; 
    int dy = mouseY - sy; 
    float distance = sqrt(dx*dx+dy*dy); 

    int a = armLength; 
    int b = armLength; 
    float c = min(distance, a + b); 

    float B = acos((b*b-a*a-c*c)/(-2*a*c)); 
    float C = acos((c*c-a*a-b*b)/(-2*a*b)); 

    float D = atan2(dy,dx); 
    float E = D + B + PI + C; 

    ex = int((cos(E) * a)) + sx; 
    ey = int((sin(E) * a)) + sy; 
    print("UpperArm Angle= "+degrees(E)+" "); 

    hx = int((cos(D+B) * b)) + ex; 
    hy = int((sin(D+B) * b)) + ey; 
println("LowerArm Angle= "+degrees((D+B))); 

    stroke(255,0,0,100); 
    fill(240,0,0,200); 
    ellipse(sx,sy,10,10); 
    ellipse(ex,ey,8,8); 
    ellipse(hx,hy,6,6); 
    stroke(0); 
    line(sx,sy,ex,ey); 
    line(ex,ey,hx,hy); 
    //float angle = atan2(dy, dx); 
    //println("angle = " + degrees(angle)) 
    //ex = int((cos(angle) * r)) + sx; 
    //ey = int((sin(angle) * r)) + sy; 
    //line(sx,sy,ex,ey); 
}