Как определить поворот 2 сегментов руки на основе целевой точки?

Question

У меня есть 2-сегментная роботизированная рука, которая должна достигать определенной точки, настраивая ее суставы (углы).

Вот чертеж установки:

Моя рука расположена в середине эскиза таким образом мое происхождение (ширина/2,0). Значения, которые я знаю: Длина: Первый сегмент (L1): 140 мм. Второй сегмент (L2): 180 мм. Расстояние между точкой начала и конечной точкой. (используя dist()).

Эти длины образуют треугольник. Таким образом, используя законы тригонометрического, я могу найти углы треугольника, которые являются углами, которые я хочу использовать, чтобы расположить руку так, чтобы она достигла целевой точки.

Теперь я хочу нарисовать треугольник на экране, используя среду обработки для моделирования. Я применил некоторые преобразования, чтобы нарисовать треугольник, но я не получаю правильный рисунок.

Это мой код:

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);} 

Я надеюсь, что мое письмо ясно и извините за путаницу.

Answer 1

Это будет гораздо более активное участие, чем вы описываете. Если вы знаете начальную точку, длину сегментов руки и целевую точку, то вы можете использовать inverse kinematics, чтобы придумать углы ваших сегментов, чтобы попасть в целевую точку. Это не так просто, как применение простой тригонометрии.

Это, как говорится, если вы уже знать углы одного из сегментов, то вы действительно можете использовать основные триг, чтобы выяснить, последний:

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); 
} 

Однако, вы не можете понять out все ваших углов, просто используя базовый триггер. Для этого вам нужно использовать что-то более активное, как обратная кинематика.

Вы можете Google что-то вроде «Обработки инверсной кинематики» для связки результатов, но here отличный пример:

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); 
}