3d 代表机器人&x27;三维中的s形肘角

3d 代表机器人&x27;三维中的s形肘角,3d,robotics,kinematics,inverse-kinematics,3d,Robotics,Kinematics,Inverse Kinematics,我得到了3-D中两点的坐标,即。肩点和目标点(我应该到达的)。我的肩膀到肘部的长度和前臂的长度也会被计算出来。我正在尝试解决未知位置(肘关节的位置)的问题。我用余弦法则来计算肘部的角度。这是我的密码- #include <stdio.h> #include <math.h> #include <stdlib.h> struct point { double x, y, z; }; struct angles { double clock

我得到了3-D中两点的坐标,即。肩点和目标点(我应该到达的)。我的肩膀到肘部的长度和前臂的长度也会被计算出来。我正在尝试解决未知位置(肘关节的位置)的问题。我用余弦法则来计算肘部的角度。这是我的密码-

#include <stdio.h>
#include <math.h>
#include <stdlib.h>

struct point {
     double x, y, z;
};

struct angles {
     double clock_wise;
     double counter_clock_wise;
};

double max(double a, double b) {
     return (a > b) ? a : b;
}
/*
 * Check if the combination can make a triangle by considering the fact that sum
 * of any two sides of a triangle is greater than the remaining side. The
 * overlapping condition of links is handled separately in main().
 */
int valid_triangle(struct point p0, double l0, struct point p1, double l1) {
     double dist = sqrt(pow((fabs(p1.z - p0.z)), 2) + pow((fabs(p1.y - p0.y)), 2) + pow((fabs(p1.x - p0.x)), 2));
     if((max(dist, l0) == dist) && max(dist, l1) == dist) {
          return (dist < (l0 + l1));
     }
     else if((max(dist, l0) == l0) && (max(l0, l1) == l0)) {
          return (l0 < (dist + l1));
     }
     else {
          return (l1 < (dist + l0));
     }
}
/* 
 * Cosine rule is used to find the elbow angle. Positive value indicates a
 * counter clockwise angle while negative value indicates a clockwise angle.
 * Since this problem has at max 2 solutions for any given position of P0 and
 * P1, I am returning a structure of angles which can be used to consider angles
 * from both direction viz. clockwise-negative and counter-clockwise-positive
 */
void return_config(struct point p0, double l0, struct point p1, double l1, struct angles *a) {
     double dist = sqrt(pow((fabs(p1.z - p0.z)), 2) + pow((fabs(p1.y - p0.y)), 2) + pow((fabs(p1.x - p0.x)), 2));
     double degrees = (double) acos((l0 * l0 + l1 * l1 - dist * dist) / (2 * l0 * l1)) * (180.0f / 3.1415f);
     a->clock_wise = -degrees;
     a->counter_clock_wise = degrees;
}
int main() {

     struct point p0, p1;
     struct angles a;
     p0.x = 15, p0.y = 4, p0.z = 0;
     p1.x = 20, p1.y = 4, p1.z = 0;
     double l0 = 5, l1 = 8;

     if(valid_triangle(p0, l0, p1, l1)) {
          printf("Three lengths can make a valid configuration \n");
          return_config(p0, l0, p1, l1, &a);
          printf("Angle of the elbow point (clockwise) = %lf, (counter clockwise) = %lf \n", a.clock_wise, a.counter_clock_wise);
     }
     else {
          double dist = sqrt(pow((fabs(p1.z - p0.z)), 2) + pow((fabs(p1.y - p0.y)), 2) + pow((fabs(p1.x - p0.x)), 2));

          if((dist <= (l0 + l1)) && (dist > l0)) {
               a.clock_wise = -180.0f;
               a.counter_clock_wise = 180.0f;
               printf("Angle of the elbow point (clockwise) = %lf, (counter clockwise) = %lf \n", a.clock_wise, a.counter_clock_wise);
          }
          else if((dist <= fabs(l0 - l1)) && (dist < l0)){
               a.clock_wise = -0.0f;
               a.counter_clock_wise = 0.0f;
               printf("Angle of the elbow point (clockwise) = %lf, (counter clockwise) = %lf \n", a.clock_wise, a.counter_clock_wise);
          }
          else
               printf("Given combination cannot make a valid configuration\n");
     }
     return 0;
}
#包括
#包括
#包括
结构点{
双x,y,z;
};
结构角{
双时钟;
双计数器时钟;
};
双倍最大值(双倍a,双倍b){
返回(a>b)?a:b;
}
/*
*通过考虑和的事实,检查组合是否可以形成三角形
*三角形任意两条边的边长大于其余边的边长。这个
*链接的重叠情况在main()中单独处理。
*/
int有效_三角形(结构点p0,双l0,结构点p1,双l1){
双区=sqrt(pow((fabs(p1.z-p0.z)),2)+pow((fabs(p1.y-p0.y)),2)+pow((fabs(p1.x-p0.x)),2));
如果((最大(距离,l0)=距离)和&max(距离,l1)=距离){
返回(距离<(l0+l1));
}
否则如果((最大(距离,l0)=l0)和&(最大(距离,l0,l1)=l0)){
返回(l0<(dist+l1));
}
否则{
返回(l1<(距离+l0));
}
}
/* 
*余弦规则用于查找弯头角度。正值表示一个值
*逆时针角度,负值表示顺时针角度。
*因为这个问题对于任何给定的P0和
*P1,我返回一个角度的结构,可以用来考虑角度。
*从两个方向,即。顺时针负向和逆时针正向
*/
无效返回配置(结构点p0、双l0、结构点p1、双l1、结构角度*a){
双区=sqrt(pow((fabs(p1.z-p0.z)),2)+pow((fabs(p1.y-p0.y)),2)+pow((fabs(p1.x-p0.x)),2));
双度=(双)acos((l0*l0+l1*l1-dist*dist)/(2*l0*l1))*(180.0f/3.1415f);
a->时钟方向=-度;
a->计数器时钟方向=度;
}
int main(){
结构点p0,p1;
结构角a;
p0.x=15,p0.y=4,p0.z=0;
p1.x=20,p1.y=4,p1.z=0;
双l0=5,l1=8;
if(有效的_三角形(p0,l0,p1,l1)){
printf(“三个长度可以构成有效配置\n”);
返回配置(p0、l0、p1、l1和a);
printf(“弯头点的角度(顺时针)=%lf,(逆时针)=%lf\n”,a.顺时针,a.逆时针);
}
否则{
双区=sqrt(pow((fabs(p1.z-p0.z)),2)+pow((fabs(p1.y-p0.y)),2)+pow((fabs(p1.x-p0.x)),2));
如果((距离l0)){
a、 时钟方向=-180.0f;
a、 计数器时钟方向=180.0f;
printf(“弯头点的角度(顺时针)=%lf,(逆时针)=%lf\n”,a.顺时针,a.逆时针);
}

否则,如果((dist基本问题是,您正在(必要地)求解平面上两个手臂部分之间的角度,但平面本身未指定。解决方案将涉及约束平面本身或将弯头位置约束到某种状态,如“最低可能位置”(这两种方法可能有相同的解决方案)


因此,定义平面,并在该平面上定位三个端点。

破折号tom bang,除了两点和长度外,没有其他信息。我找到了一个参考-θ2=2*atan(sqrt((pow(l0+l1,2)-(dist*dist))/(dist*dist)-pow(l0-l1,2));φ=atan2(p1.y,p1.x);psi=atan2(l1*sin(θ)(l0+l1*cos)(θ2));θ1=φ-psi;因此,return_config将θ2作为“弯头角度”返回θ1是与旋转轴的角度。我还应该返回旋转方向。请问,有人能评论我是否在正确的轨道上吗?是的,你只需要提供你没有的信息。换句话说,选择一些看起来合理的东西并尝试一下。;)