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

У меня есть 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);}

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


person user3458260    schedule 24.02.2016    source источник
comment
Вы когда-нибудь понимали это?   -  person Kevin Workman    schedule 18.01.2019


Ответы (1)


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

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

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

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

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

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);
}
person Kevin Workman    schedule 24.02.2016
comment
Я использовал закон косинусов, чтобы найти внутренние углы, потому что я знаю длины сторон в любой момент. Разве этого недостаточно? - person user3458260; 24.02.2016
comment
@user3458260 user3458260 Нет. На самом деле вы не знаете всех длин, вы знаете только две длины. - person Kevin Workman; 24.02.2016
comment
@ user3458260 Нет проблем. Инверсная кинематика интересна, но она немного сложнее, чем базовая триггерная система. Кроме того, подумайте об этом так: вы описали четыре точки, а не три: начало координат, конец первого сегмента, конец второго сегмента и конечную точку. Это четырехугольник, а не треугольник. - person Kevin Workman; 24.02.2016
comment
@user3458260 user3458260 Я отредактировал свой ответ, включив в него пример, в котором используется инверсная кинематика для определения вращения двухсегментного рычага. - person Kevin Workman; 24.02.2016
comment
@user3458260 user3458260 Вы также можете перефразировать свой заголовок на что-то вроде того, как определить вращение двух сегментов руки на основе целевой точки? и пометьте свой вопрос тегом inverse-kinematics, кто-нибудь сможет помочь вам более конкретно. - person Kevin Workman; 24.02.2016
comment
Большое спасибо. Сейчас я работаю над пониманием инверсной кинематики. - person user3458260; 25.02.2016