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

Question

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

这里是设置图：

我的手臂位于草图的中间，所以我的原点是 (width/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);
}

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

您可以在 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);
}