OpenScad - 如何在旋转后找到机器人 Arm 节点坐标 - 5 轴

Question

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

启用 Animation 以在动态中查看 - FPS:30 STEPS:300

模块

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

Function 旋转点

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

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

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

function rotate3d(pitch, roll, yaw,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];

获取 arm 节点位置坐标的函数

RSGetPos获取右肩的 position

LSGetPos获取左肩的 position

RA1GetPos获取右侧 Arm 第 1 部分的 position（绿色）

LA1GetPos获取左侧 Arm 第 1 部分的 position（绿色）

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

RA2GetPos获取右 Arm 的 position 第 2 部分（红色）我需要水色球的坐标

LA2GetPos获取左侧 Arm 的 position 第 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,0,[0,0,-A1LEN])+[SWIDE/2,0,0]);
function LA1GetPos() = rotate3d( 0, SROT, SROTZ, rotate3d(A1ROT,0,0,[0,0,-A1LEN])+[-SWIDE/2,0,0]);

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

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

color("blue",0.5)translate(LA1GetPos()){
    sphere(50); 
    translate([-100,0,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);}

Arm 部件尺寸

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,0,SROTZ]){
    translate([SWIDE/2,0,0])rotate([0,0,-90])rotate([0,SROT,0])spalla();
    translate([-SWIDE/2,0,0])mirror([0,1,0])rotate([0,0,90])rotate([0,SROT,0])spalla();
}

Answer 1

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

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];
rot3=[0,sin($t*360*2)*100,0];

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

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

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

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

tr=[100,0,0];
rx=[sin($t*360)*100,0,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();