java - 如何根据目标点确定 2 个臂段的旋转？

Question

我有一个 2 段机械臂，需要通过设置其关节(角度)来到达特定点。

这是设置图:

我的 ARM 位于草图的中间，所以我的原点是 (width/2,0)。这些是我所知道的值(value)观:长度:第一段 (L1):140 毫米。第二段 (L2):180 毫米。原点和目标点之间的距离。 (使用 dist() )。

这些长度形成一个三角形。因此，使用三角定律我可以找到三角形的角度，这些角度是我想要用来定位我的 ARM 以使其到达目标点的角度。

现在，我想使用 Processing 环境在屏幕上绘制三角形进行模拟。我应用了一些变换来绘制三角形，但没有得到正确的图形。

这是我的代码:

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

但是，您无法仅使用基本三角函数计算出所有 的角度。为此，您必须使用更复杂的东西，例如反向运动学。

您可以在谷歌上搜索“处理逆向运动学”之类的内容以获得大量结果，但是 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);
}