-- 6自由度机械臂URDF建模及URDF导出示例
-- 通用
local r_shell = 32;
local h_motor = 90;
local offset = h_motor / 2 - r_shell
shell = cylinder.new(r_shell, h_motor)
shell:fillet(5, { type = 'circle', min = { r_shell - 1e-2, -1e-2, h_motor - 1e-2 } });
shell:fuse(cylinder.new(r_shell, h_motor / 2):z(h_motor / 2):rx(90));
-- 生成连接柱
function get_pole(r_outer, r2, h)
    local r1 = r_outer - 1
    local h_stair = 2
    local h_cylinder = r1 - r2 + h_stair
    local stair = cylinder.new(r1, h_cylinder):cut(torus.new(r1, r1 - r2):pos(0, 0, h_cylinder))
    stair:fillet(1, { type = 'circle', min = { r1 - 1e-2, -1e-2, h_stair - 1e-2 } });
    local stair_top = stair:copy():rx(180):z(h)
    local pole = cylinder.new(r2, h):fuse(stair):fuse(stair_top)
    return pole
end

-- 基座
local r_base = 50;
local h_base = 35;
base_link = cylinder.new(r_base, h_base);
local R1 = r_base - r_shell
local R0 = R1 + 2
elips = edge.new('elips', { R1 + r_shell, 0, h_base }, { 0, 1, 0 }, R0, R1);
ellipse = face.new(elips);
ellipse:revol({ 0, 0, 0 }, { 0, 0, 1 }, 360)
base_link:cut(ellipse);
base_link:fillet(3, { type = 'bspline_curve', min = { r_base - 1e-2, -1e-2, (h_base - R0) - 1e-2 } });
-- 肩部
sholder = shell:copy():z(h_base);
-- 上臂
local h_upperarm = 150
local r_upperarm = 20
local z_upperarm = h_base + h_motor / 2
upperarm = shell:copy();
upperarm:rot(90, 180, 0);
upperarm:fuse(get_pole(r_shell, r_upperarm, h_upperarm):pos(0, -h_motor / 2, h_motor / 2))
upperarm:fuse(shell:copy():rot(90, 0, 0):pos(0, 0, h_motor + h_upperarm))
upperarm:pos(0, -h_motor / 2, z_upperarm)
-- 前臂
local h_forearm = 120
local r_forearm = 20
local z_forearm = h_base + h_upperarm + r_shell + h_motor + offset
forearm = face.new(edge.new('circ', { 0, 0, 0 }, { 0, 0, 1 }, r_shell));
forearm:revol({ 0, -r_shell, 0 }, { 1, 0, 0 }, -90)
forearm:fuse(get_pole(r_shell, r_forearm, h_forearm))
forearm:fuse(shell:copy():rot(90, 0, 180):pos(0, -h_motor / 2, h_motor / 2 + h_forearm))
forearm:pos(0, -offset, z_forearm + r_shell)
-- 手腕1
local z_wrist1 = z_forearm + h_forearm + h_motor + r_shell
wrist1 = shell:copy():rot(180, 0, 0):pos(0, -h_motor - offset, z_forearm + h_forearm + h_motor + r_shell)
-- 手腕2
local z_wrist2 = z_forearm + h_forearm + 2 * h_motor - offset
wrist2 = shell:copy():rot(90, 0, 0):pos(0, -h_motor / 2 - offset, z_wrist2)
-- 手腕3
local h_flank = 10
wrist3 = cylinder.new(r_shell, h_flank):rot(90, 0, 0):pos(0, -h_motor / 2 - offset + h_flank, z_wrist2)
-- 毫米单位转为米，生成URDF
base_link:scale(1e-3):color('#6495ED'):mass(0.1)
sholder:scale(1e-3):color('#8470FF'):mass(0.4)
upperarm:scale(1e-3):color('#FFC1C1'):mass(1)
forearm:scale(1e-3):color('#FFC100'):mass(0.8)
wrist1:scale(1e-3):color('#FF8247'):mass(0.4)
wrist2:scale(1e-3):color('#FFE7BA'):mass(0.4)
wrist3:scale(1e-3):color('#C1CDC1'):mass(0.1)
joint_axes1 = axes.new({ 0, 0, z_upperarm * 1e-3, 0, 0, 0 }, 0.1)
joint_axes2 = axes.new({ 0, -h_motor * 1e-3, z_upperarm * 1e-3, 90, 0, 0 }, 0.1)
joint_axes3 = axes.new({ 0, -h_motor * 1e-3, z_forearm * 1e-3, 90, 0, 0 }, 0.1)
joint_axes4 = axes.new({ 0, -offset * 1e-3, (z_wrist1 - h_motor / 2) * 1e-3, 90, 0, 0 }, 0.2)
joint_axes5 = axes.new({ 0, -(h_motor + offset) * 1e-3, (z_wrist1 - h_motor / 2) * 1e-3, 0, 0, 0 }, 0.2)
joint_axes6 = axes.new({ 0, -(h_motor + offset) * 1e-3, (z_wrist2) * 1e-3, -90, 0, 0 }, 0.1)
joint1 = joint.new("joint1", joint_axes1, "revolute")
joint2 = joint.new("joint2", joint_axes2, "revolute")
joint3 = joint.new("joint3", joint_axes3, "revolute")
joint4 = joint.new("joint4", joint_axes4, "revolute")
joint5 = joint.new("joint5", joint_axes5, "revolute")
joint6 = joint.new("joint6", joint_axes6, "revolute")
urdf = link.new("base_link", base_link)
link1 = link.new("link1", sholder)
link2 = link.new("link2", upperarm)
link3 = link.new("link3", forearm)
link4 = link.new("link4", wrist1)
link5 = link.new("link5", wrist2)
link6 = link.new("link6", wrist3)
urdf:add(joint1):next(link1):add(joint2):next(link2):add(joint3):next(link3):add(joint4):next(link4):add(joint5):next(
    link5):add(joint6):next(link6)
show({ base_link, sholder, upperarm, forearm, wrist1, wrist2, wrist3 })
show({ joint_axes1, joint_axes2, joint_axes3, joint_axes4, joint_axes5, joint_axes6 })
-- urdf:export({ name = 'myrobot', path = 'd:/' })
--[[
ROS2使用方式:
sudo apt update
sudo apt install ros-$ROS_DISTRO-urdf-launch
mkdir -p ~/ws_ros2/src
cp -r /mnt/d/myrobot ~/ws_ros2/src/
cd ~/ws_ros2
colcon build --symlink-install
source install/setup.bash
ros2 launch urdf_launch display.launch.py urdf_package:=myrobot urdf_package_path:=urdf/myrobot.urdf
--]]
