OpenScad - 如何在旋转后找到机械臂节点坐标 - 5 轴

如何解决OpenScad - 如何在旋转后找到机械臂节点坐标 - 5 轴

我找到了 3 个节点中 2 个的坐标。我需要找到最后一个坐标，即浅绿色球体的坐标。这是我的代码。有人能帮我吗？ 谢谢

启用动画以在运动中查看 - FPS:30 STEPS:300

模块

module spalla(){    
    translate([0,-50,0])
    rotate([0,90,0]){
        rotate([-90,0]){
            cylinder(50,50,40,true,$fn=6);
            difference(){
                union(){
                    translate([0,-20,50])cube([50,10,50],true);
                    translate([0,20,true);
                }
                   translate([0,50])rotate([90,0])cylinder(100,true);
            }
        }
        translate([0,0])rotate([0,A1ROT])braccio();
    }
}
module braccio(){
    translate([A1LEN/2,0])cube([A1LEN,30,30],true);    
    translate([A1LEN,0])rotate([360-A21ROT,A22ROT])avambraccio();
}
module avambraccio(){
   translate([A2LEN/2,0])color("red")cube([A2LEN,true); 
   translate([A2LEN,0])color("aqua")sphere(30);   
}

旋转点的功能

注意：根据您的 Openscad 坐标系，可能需要置换函数的参数。

function rotate3d(pitch,roll,yaw,point) = let(

function rotate3d(roll,pitch,point) = let(

function rotate3d(pitch,point) = let(

    cosa = cos(yaw),sina = sin(yaw),cosb = cos(pitch),sinb = sin(pitch),cosc = cos(roll),sinc = sin(roll),Axx = cosa*cosb,Axy = cosa*sinb*sinc - sina*cosc,Axz = cosa*sinb*cosc + sina*sinc,Ayx = sina*cosb,Ayy = sina*sinb*sinc + cosa*cosc,Ayz = sina*sinb*cosc - cosa*sinc,Azx = -sinb,Azy = cosb*sinc,Azz = cosb*cosc,px = point[0],py = point[1],pz = point[2],rx = Axx*px + Axy*py + Axz*pz,ry = Ayx*px + Ayy*py + Ayz*pz,rz = Azx*px + Azy*py + Azz*pz
    
)[rx,ry,rz];

获取手臂节点位置坐标的函数

RSGetPos 获取右肩位置

LSGetPos 获取左肩位置

RA1GetPos 获取右臂第 1 部分的位置（绿色）

LA1GetPos 获取左臂第 1 部分的位置（绿色）

*** 我想念的功能***

RA2GetPos 获取右臂第 2 部分的位置（红色）我需要浅绿色球体的坐标

LA2GetPos 获取左臂第 2 部分（红色）的位置，我需要浅绿色球体的坐标

function RSGetPos() = [cos(SROTZ)*SWIDE/2,sin(SROTZ)*SWIDE/2,0];
function LSGetPos() = [cos(SROTZ+180)*SWIDE/2,sin(SROTZ+180)*SWIDE/2,0];

function RA1GetPos() = rotate3d( 0,SROT,SROTZ,rotate3d(-A1ROT,[0,-A1LEN])+[SWIDE/2,0]);
function LA1GetPos() = rotate3d( 0,rotate3d(A1ROT,-A1LEN])+[-SWIDE/2,0]);

调用函数获取节点坐标并绘制节点

color("red",0.5)translate(LSGetPos()){
    sphere(50);
    translate([-100,0])rotate($vpr) text(str(LSGetPos()),50);
}

color("blue",0.5)translate(LA1GetPos()){
    sphere(50); 
    translate([-100,0])rotate($vpr) text(str(LA1GetPos()),50);
}


//color("red",0.5)translate(RSGetPos()){sphere(50);rotate($vpr)text(str(RSGetPos()),130);}
//color("blue",0.5)translate(RA1GetPos()){sphere(50);rotate($vpr)text(str(RA1GetPos()),130);}

手臂部件的尺寸

A1LEN=300; //The length of A1 green part of the arm. 
A2LEN=200; //The length of A2 red part of the arm. 
SWIDE=400; //Width of Shoulders 

5 次输入旋转 - 看看我发布的绘图作为参考

SROTZ =  sin($t*360*2)*45;
SROT  =  sin($t*360*4)*45+45;
A1ROT =  sin($t*360*2)*45+45;
A21ROT = sin($t*360*2)*45+45;
A22ROT = sin($t*360*2)*45+45;

/*
SROTZ =0;
SROT  =0;
A1ROT =0;
A21ROT=0;
A22ROT=0;
*/

调用主模块

rotate([0,SROTZ]){
    translate([SWIDE/2,-90])rotate([0,0])spalla();
    translate([-SWIDE/2,0])mirror([0,1,90])rotate([0,0])spalla();
}

解决方法

offer sequential consistency

这是我对我的问题的回答，做了一些简化。

function rotate3d(rot,point) = let(
    roll  = rot[0],pitch=  rot[1],yaw =  rot[2],cosa = cos(yaw),sina = sin(yaw),cosb = cos(pitch),sinb = sin(pitch),cosc = cos(roll),sinc = sin(roll),Axx = cosa*cosb,Axy = cosa*sinb*sinc - sina*cosc,Axz = cosa*sinb*cosc + sina*sinc,Ayx = sina*cosb,Ayy = sina*sinb*sinc + cosa*cosc,Ayz = sina*sinb*cosc - cosa*sinc,Azx = -sinb,Azy = cosb*sinc,Azz = cosb*cosc,px = point[0],py = point[1],pz = point[2],rx = Axx*px + Axy*py + Axz*pz,ry = Ayx*px + Ayy*py + Ayz*pz,rz = Azx*px + Azy*py + Azz*pz
    
)[rx,ry,rz];

module draw(p,r=10){
    translate(p) sphere(r);    
    color( "blue",1)translate(p) rotate($vpr)text(str(p),40);
}

len1=75;
len2=150;
len3=180;
/*
rot1=[0,-45,10];
rot2=[0,-90,0];
rot3=[0,145,0];
*/
rot1=[0,sin($t*360*2)*100,sin($t*360)*100];
rot2=[0,sin($t*360)*100,0];

size1=[len1,30,30];
size2=[len2,30];
size3=[len3,30];

module m1(){ translate([len1/2,0])color("red")cube(size1,true);translate([len1,0])rotate(rot2)m2();}
module m2(){ translate([len2/2,0])color("green")cube(size2,true);translate([len2,0])rotate(rot3)m3();}
module m3(){ translate([len3/2,0])color("pink")cube(size3,true);translate([len3,0]); }

init1=[len1,0];
init2=[len2,0];
init3=[len3,0];

rp1=rotate3d(rot1,init1);
rp2=rotate3d(rot1+rot2,init2)+rp1;
rp3=rotate3d(rot1+rot2+rot3,init3)+rp2;

tr=[100,0];
rx=[sin($t*360)*100,sin($t*360)*100];
xrp1=rotate3d(rx,rp1)+tr;
xrp2=rotate3d(rx,rp2)+tr;
xrp3=rotate3d(rx,rp3)+tr;

draw(xrp1,20);
draw(xrp2,20);
draw(xrp3,20);

translate(tr)rotate(rx)rotate(rot1)m1();