From dc5d46dcaeee70b1701b8b2a84396c019721aa71 Mon Sep 17 00:00:00 2001 From: mio Date: Wed, 14 Jan 2026 16:07:09 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86config.yaml=EF=BC=8C?= =?UTF-8?q?=E6=89=80=E6=9C=89=E5=8F=82=E6=95=B0=E9=83=BD=E4=BB=8Econfig?= =?UTF-8?q?=E6=96=87=E4=BB=B6=E4=B8=AD=E5=8A=A0=E8=BD=BD=EF=BC=8C=E4=BC=98?= =?UTF-8?q?=E5=8C=96=E4=BA=86=E6=96=87=E4=BB=B6=E7=BB=93=E6=9E=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- soft_arm_sim/config/config.yaml | 14 ++ soft_arm_sim/launch/simulate.launch.py | 26 ++- .../simulation_node.cpython-310.pyc | Bin 4322 -> 5309 bytes .../soft_arm_sim/base/simulation_node.py | 181 ++++++++++++------ .../pcc_kinematics.cpython-310.pyc | Bin 3233 -> 4541 bytes .../soft_arm_sim/model/pcc_kinematics.py | 141 +++++++++----- 6 files changed, 242 insertions(+), 120 deletions(-) create mode 100644 soft_arm_sim/config/config.yaml diff --git a/soft_arm_sim/config/config.yaml b/soft_arm_sim/config/config.yaml new file mode 100644 index 0000000..4e0d722 --- /dev/null +++ b/soft_arm_sim/config/config.yaml @@ -0,0 +1,14 @@ +soft_arm_simulator: + ros__parameters: + # --- 机器人物理参数 --- + num_sections: 3 # PCC 段数 + section_length: 0.240 # 每段长度 (m) + disks_per_section: 3 # 每段的圆盘数量 + disk_radius: 0.033 # 绳索孔距圆心的半径 (m) (用于运动学计算) + + # --- 视觉参数 --- + visual_disk_radius: 0.04 # 圆盘实际显示半径 (m) (可视化的红色圆盘大小) + visual_disk_thickness: 0.005 # 圆盘厚度 (m) + + # --- 仿真参数 --- + sim_rate: 30.0 # 仿真频率 (Hz) \ No newline at end of file diff --git a/soft_arm_sim/launch/simulate.launch.py b/soft_arm_sim/launch/simulate.launch.py index 3b11191..ef49fdd 100644 --- a/soft_arm_sim/launch/simulate.launch.py +++ b/soft_arm_sim/launch/simulate.launch.py @@ -5,14 +5,20 @@ from launch.substitutions import Command from launch_ros.actions import Node def generate_launch_description(): + # 定义包名，方便后续路径拼接 pkg_name = 'soft_arm_sim' + # 获取安装后的 share 目录路径 share_dir = get_package_share_directory(pkg_name) - # 获取 xacro 文件路径 + # 1. 路径定义 + # 定位 xacro 模型文件 (用于 robot_state_publisher) xacro_file = os.path.join(share_dir, 'urdf', 'soft_arm.urdf.xacro') + # 定位 yaml 配置文件 (用于模拟器参数) + config_file = os.path.join(share_dir, 'config', 'soft_arm_params.yaml') - # 1. Robot State Publisher - # 使用 Command 进行转换，这样更稳定，且能在终端看到详细报错 + # 2. Robot State Publisher 节点 + # 作用：发布静态 TF 树 (如 base_link)，并向 Rviz 提供 robot_description 话题。 + # Command(['xacro ', xacro_file]) 会在运行时动态解析 xacro 文件生成 XML 字符串。 robot_description = Command(['xacro ', xacro_file]) node_robot_state_publisher = Node( @@ -22,20 +28,26 @@ def generate_launch_description(): parameters=[{'robot_description': robot_description}] ) - # 2. 仿真节点 + # 3. 自定义仿真节点 (Simulator) + # 这是我们编写的核心 Python 节点。 node_simulator = Node( package=pkg_name, - executable='simulator', - output='screen' + executable='simulator', # setup.py 中 entry_points 定义的可执行文件名 + name='soft_arm_simulator', # 节点名，必须与 yaml 文件中的根键一致，否则参数加载失败 + output='screen', + parameters=[config_file] # <--- 关键：在这里加载 .yaml 参数文件 ) - # 3. Rviz2 + # 4. Rviz2 节点 + # 启动可视化界面 node_rviz = Node( package='rviz2', executable='rviz2', name='rviz2', + # (可选) 可以在这里添加 arguments=['-d', rviz_config_path] 来加载保存的 rviz 配置 ) + # 返回 Launch 描述符，ROS 2 会并行启动列表中的所有节点 return LaunchDescription([ node_robot_state_publisher, node_simulator, diff --git a/soft_arm_sim/soft_arm_sim/base/__pycache__/simulation_node.cpython-310.pyc b/soft_arm_sim/soft_arm_sim/base/__pycache__/simulation_node.cpython-310.pyc index a61a9f5dbc3d146c8bb85561ec02d620abb38791..690b092f40ae32f45ac6faa90fef4c7a5b355862 100644 GIT binary patch literal 5309 zcmZu#U5p#ob)Gvj91ee%OYN^@DYR@SWs>d|`7Me>%G#10!SY6tgd}C1PG)(B+~E#6 z)SaQ_-3;3#D@RUXJ4x%pg&haGfRR`M;`+%@>BkOJdpFXN-za?h+F~NKZZ}dwLu5lJ<4J!43b@hhM zsBQ#C*KC;GOe52^8df*k$SS!R*xg(s*UdNbiq8avZn079mKr6+TS2)y)0k29Y*6XW zHfB|w1=U7Xt#za^htdw_y9cLtU4Z%fjtNtx$9?#csFnahbUg`n?!U))TJS@`Nmm%QYd~ZLB@}Xb`&b=RWhq zAc*}#p?TTwdT6vShOry_VUIdq3S04-=sxcEye@>zs7d>uevFifK7)#kJOL11pWfhlAIa_hLl*=iukaBs&m53X%xR7Wa%-g|i9n8_O`ONdQRxvK^l@o2B zp*|B=cqN|gRO2I^xp=;_z-ON~hdSn2jF)&d(f1AXS_XeKKE{t=mizeJzR9#U^xzzi zPe78{WoNW+7KiBBIllK1KF{Y-vswme?#fFTTjL7}JBAUR`}Z-oa}u%#_O-nS`67*f zMdM2e16_VKLu0sx?{#!tRwLXC#}!>CI{aj^*c*WLnsjxdJM^(+Ic1%|>$T&}_RSN| z{O4D1{_ANu%l&98a{8V)93cxNa)itMLDW9~`5&x(YqWP-R-f{tfg3o}jo+SoY5PTirRZm*^pVK=>>896UQ8mBk z$G#i*J07oH_2bRjPNjCJy-(I?*I$(Smv`z9$xJkWqNMFOG{$kDJ+B$K!gKnraJycN z#!}mhr`XI>ZZPoPcUHMDrA23JV$!lStw+o?&2EY^jv(_>-LrA)Q;rBDIX3~Ghehb} zrW?^#%5lrtxXTN=34&}G$a41lxwVJSpA{KmbB~`rd%-!scImRPRP)!qa(?adx$|db zr71ip!08V*0zcaHgdlawd{?0viRF>BOElPsn!@kXtrG?6Rcv;-({zJi!)#cVEa5ZS{toB9Nfq|@wa3drdeag8S zimg?3d#*;S8v{S!V=lF=dzlS4@>Wxe!+D(^T*yj)TRe!J%;1e41u3$kiFfiV8)bI* zD>B_Q+4yI_SIp^#o^sZEm2B0ttTJ0*bH+)1XPIs)F6so6R}SxAQ^zWY4}D2`AHf@a z4J2V5ZK%gA(RTsS01ha(z)lP3xE~>X}K5 zFKAb0xp4`O9H3(kPGj{OsK~cSqO%@%G=m`QQKW^60f|2S5J)?HAt~J^SXt z%hztd@Qcy2Z{2?G`a5sG_S^4#Z!+o*?OZH_tk*4R;#<4Y05**c<@402kl}Nv)idH0 z3TbqswjjMq=1arpPYXKgn4F>7XF;C88b zS1IxvKqM#uUXGNu&z{tWOkuwPY&P*`_O+qWh5}y(@_ zH&r>4n2C*#m2)ZyqZ-c;*MYZkj-(Q6BAH7i*@8`qh z3%ZXoM?lmJLV%jl3$IEm4jsCi>e?!=rX1QIQ(vR&_rznUi7c_!rss0tS0n;ygglbMxdUu=#y%#h7spnXpDY{!lLz7{fIr2sk<0<01Bb8}E|@}u z9`+8b#UBFQW;p1@;vB8UV#$PLSKe=*e}cw8!yEk(NHtpl0?x5Hw#4-Rt7eM21#7ig zo>h%1tFjY@Jps$IUNLs=`+x;c`G_X@w!??apR}zExB;Z0Fm-6e4AhKuGBKFvEtqm< zXvGGkS(s6_V>%g_FqIYzH0xxO3^x`v)a=;kXLtCfp%-q9Oz|AUo zm1j^Mapn>VG3SqJLp!m#wV?6rmM*@|?I#V@zM$x$qD#HwX&(c9e4hKzJ}}1*lP8uk z#cXG}a}>L>coC*J<|^Py6RsMQSz1VmESS8asmbS(+}?4RJWQVxOwi~Pm@ChX30DC} zzGB=~1xFAw=03u+sMVbN)AD3ubRO7yFhOVz2!Q^eZNOVnNVQF+#b=U&lf{UF()2?p z$C@rUMP5N)=oq>zkS_VGDiMUMaxpEJXmq+Rl_wK|;FH5*{86k}ia(YV={x{es^KhY z4d>WphM^lt38yRpMoUf=GyMVfj3fbjelI-%5(r9cI`zGeCn{{`Wk6VU(8o=i0MNU? zerEK;8xwDM`)AiiZ{HgI{aYz^O(lP|+iyL4`@4X;=MG+c{heDcjDGrh?c!7ZPVJqW zzdCsF?a}pHqo02N;FZ4}{nc{^w|;Z`)%|JrDag$sN~YL-jARPJ?uYa)g4GS71T~Tp zfk4$K(nG;r={mZJuMoKavMy~3-8RCW*M9HD@BZzHjqjXp|I0tq^IxaypHQ)&Aa^Nq z9&!$kVr7w|(G-7(uM_zOHS4`d>OeXBkv~0uZvD*Ji_(m@`<^t`&YVF6s4^8%r=FPz z0i!3Lq@mv=@^K=6u9})*5CT`+alX(c8EZ?nA0;%L+e*2cc+DOEC?SUCQ-kZpRa$LeCT=FC*;1_mhRg)Qln;I+W9E)h$q) zS@==;z8~gY0T34nf*(#u_-|!-+%-u?>OC%4+5$e9db{>xJ5A^2KQ)gbIE-J>O;F|C~qCCV#h;5&;%as*D@_V8<3NXnjC?`M;C)PTM;W&F9 zW*qAP1s3D~3CZ|>7mQ*#$W=U-X4ks<=a4Jf5kl0AXa@al$~*j6Er!&i@AqVpdr>UH zZIuBa&)povJiOYg>*BAmrZg!rm^uiZL7H9H@BKS!>JUZfoxWL-iI*X(v-SEh@gnuh zt2I~D6m%STVA{kAGMu})oXX(FY1?euLyi-RZHh*s6$(kn4UX^b!r0IIJIW01_E6@k zlDc9P^Sg4D!7Wf86#G5oyRht*+w{iMlFf@x(v~u?d+0^SEnSS7K6V%Nv4R_{OyY=f zsB(Wa^^b}bTnaB(={K8G*{{lPm2Qw z90d_gVww;Se%wnk9?*C|LgHaByzmFWyY~V}XsQ4So*9C3uG21>X3M|xd9UqrfA`$) zE5FX>nnOc|0M{okZq)R#Z*%{Vwg{#Ij|5Nf5kM$(gf5|^M~D!L9U&5IDU`dJNazvJ zl~AQ(sCjy5c-heOhN$$p+=56q7v`xPk)8xu7TATbNMV#WOqHHYgbjhJw{eCN#73wF zk7_wP<$|NYIo zKQ6H{SR6La9=~z&;&0uhN;$~{+gmP6G~2HEwa~WXo0*^Dnlf9eh>0Ay_4UMPIBe5p z_N8VJ-&HQpO~N!ec!GTZCecJw5+#wy{8k=VT2VB_k{W-i&bP5t_`HnyYXA}Pgq9eR zNbEr2HZ$z$NTlMp04R+MR9=UYc;GpKo(%UZ1AdA7RqodY{FxI1R;FK_9b6?w1feak z0;>3>JGgv1_%S;&LI$8BB{FqFCz0x|&*r&@9RZO@jbMWv2aP?m2Megp0boX80w}~J zWwXsMADsh(!*ROHf#a$X)V}X7|@UH>Fr>1ksHSbl2ScQO7%2naUdtv)0|XaO!PG;aP4}{ z-@GH@q6_R<0MPl;_w~hHKM&5Pc{~9lm2ox&B2m|y?6Sisn~$yRL^jb~f4ku_Cv@X8 z*(+p!{6ltLGT4#$&+K!jY6&@)kT4-EA(tMd$&)Z0&Kw*DC=yH7Bui0<2Dd?SVvgj< zJ~6&#K0h}E348Q-0Z58nr6q*~P!7dDL%xphnJ3!NG*n#rzRW|@K-;1U$bwyrGF06Z z*cqy=B8dw2Fvk+d5q|>WI>g_j#$)4Inthxfh-C8hZ;yIooOeUqL<@PB+q0~&+#Tn< zQ#tPd2Y9!ym(niZ&!6D5OBp@tO~70_wf162yOf|9o22NZWY6m?F=PcDzp$8whAK*TMH%c07%I6vLH=bgdD7;0upLRhq)Z?%TiyC;OPE) z+o;S$BJ9`$t!(%NywVI0Mp+7f)FE7hzv*l6CmkX|Ym7|zPZR#mgkEoWhv1`4zIp!W z|AK)QY^%fFxky5tx3SVP!;X<;a2o}j% z@B1kn0~5S>5IhX+Iv#pYWBe6_R}uIwLi#4!np53eYx?fSgFAo6rj?(aXV}mfwm?t&0M3=FQ+L^0=vqNI2asf{&3Y5|i^JC9Th`3!w=wb#MzT#-bN$c> zYfYb>M~_yot-E2Z;U*HalSK6$zsZ&KJPH!U+1he_8vkR>P8`A!>elvJy%u~}8RAWx zy^pi6#>Wd&Z5!2#m?UF88Jb7?!ZtSmB= 6: - self.current_config[0] = (data[0], data[1], 0.24) - self.current_config[1] = (data[2], data[3], 0.24) - self.current_config[2] = (data[4], data[5], 0.24) + # 简单校验：确保数据长度足够覆盖所有段。 + # 因为每段需要 2 个控制量 (弯曲角 theta, 偏转角 phi) + if len(data) >= 2 * self.num_sections: + for i in range(self.num_sections): + idx = i * 2 + # 更新状态配置。注意：此处假设长度 section_length 是不可变的。 + # 如果要做伸缩机器人，可以在这里更新第 3 个参数。 + self.current_config[i] = (data[idx], data[idx+1], self.section_length) def update_loop(self): + """ + 主循环：计算运动学 -> 发布 TF -> 发布 Marker + """ + # 1. 调用数学模型计算所有位置 + # transforms: 所有 Disk 的 4x4 矩阵 + # curve_points: 骨架曲线上的点坐标 (用于画线) transforms, curve_points = self.kinematics.forward(self.current_config) - # --- 策略：TF 用真实时间（保住 base_link），Marker 用 0 时间（保住同步）--- - real_time = self.get_clock().now().to_msg() - # 1. 发布 TF (这是为了系统健全性，不用于显示) - disk_names = [ - "sec1_disk1", "sec1_disk2", "sec1_disk3", - "sec2_disk1", "sec2_disk2", "sec2_disk3", - "sec3_disk1", "sec3_disk2", "sec3_disk3" - ] + # 2. 动态生成 disk 名字 + # 这种生成方式 (secX_diskY) 必须与 URDF 或后续的控制逻辑对应。 + disk_names = [] + for s in range(1, self.num_sections + 1): + for d in range(1, self.disks_per_section + 1): + disk_names.append(f"sec{s}_disk{d}") + + # 3. 发布 TF 变换 + # 使用 min() 是为了防止数学模型计算出的 transform 数量与预期的 name 数量不一致导致数组越界。 + count = min(len(transforms), len(disk_names)) - for i, T in enumerate(transforms): + for i in range(count): t = TransformStamped() - t.header.stamp = real_time # 真实时间，保证 base_link 不丢 - t.header.frame_id = "base_link" - t.child_frame_id = disk_names[i] + t.header.stamp = real_time + t.header.frame_id = "base_link" # 所有变换都是相对于世界/基座的 + t.child_frame_id = disk_names[i] # 子坐标系名字 - t.transform.translation.x = T[0, 3] - t.transform.translation.y = T[1, 3] - t.transform.translation.z = T[2, 3] + # 填充位置 + t.transform.translation.x = transforms[i][0, 3] + t.transform.translation.y = transforms[i][1, 3] + t.transform.translation.z = transforms[i][2, 3] - r = Rotation.from_matrix(T[:3, :3]) + # 填充旋转 (将旋转矩阵转换为四元数) + r = Rotation.from_matrix(transforms[i][:3, :3]) q = r.as_quat() t.transform.rotation.x = q[0] t.transform.rotation.y = q[1] @@ -81,45 +131,49 @@ class SoftArmSimulator(Node): self.tf_broadcaster.sendTransform(t) - # 2. 发布全套 Marker (Line + Disks) + # 4. 发布可视化 Marker self.publish_all_visuals(transforms, curve_points) def publish_all_visuals(self, transforms, curve_points): + """ + 辅助函数：构建并发布 MarkerArray 消息，用于在 Rviz 中画出圆盘和线条。 + """ marker_array = MarkerArray() + zero_time = Time() # Marker 时间戳通常设为 0，表示“最新” - # 通用设置 - zero_time = Time() # 强制零时间，立刻渲染 - - # --- A. 创建 Disk Markers (替代 RobotModel) --- + # --- A. 构建 Disk Markers (圆柱体) --- for i, T in enumerate(transforms): disk = Marker() disk.header.stamp = zero_time disk.header.frame_id = "base_link" - disk.ns = "disks" - disk.id = i + 1 # ID 从 1 开始 - disk.type = Marker.CYLINDER - disk.action = Marker.ADD + disk.ns = "disks" # 命名空间 + disk.id = i + 1 # 唯一 ID + disk.type = Marker.CYLINDER # 形状：圆柱 + disk.action = Marker.ADD # 动作：添加/修改 - # 尺寸 (80mm 直径, 5mm 厚) - disk.scale.x = 0.08 - disk.scale.y = 0.08 - disk.scale.z = 0.005 + # 设置尺寸 (来自参数) + disk.scale.x = self.visual_disk_r * 2 # 直径 = 半径 * 2 + disk.scale.y = self.visual_disk_r * 2 + disk.scale.z = self.visual_disk_h - # 颜色 (根据段区分) - disk.color.a = 1.0 - if i < 3: # Sec 1: Red - disk.color.r, disk.color.g, disk.color.b = 1.0, 0.0, 0.0 - elif i < 6: # Sec 2: Green - disk.color.r, disk.color.g, disk.color.b = 0.0, 1.0, 0.0 - else: # Sec 3: Blue - disk.color.r, disk.color.g, disk.color.b = 0.0, 0.0, 1.0 + disk.color.a = 1.0 # 不透明度 - # 位置 + # 颜色逻辑：为了区分不同的 PCC 段，给每段设置不同的颜色 (红/绿/蓝 循环) + # i // self.disks_per_section 计算当前 disk 属于第几段 + section_idx = i // self.disks_per_section + + if section_idx % 3 == 0: + disk.color.r, disk.color.g, disk.color.b = 1.0, 0.0, 0.0 # 红 + elif section_idx % 3 == 1: + disk.color.r, disk.color.g, disk.color.b = 0.0, 1.0, 0.0 # 绿 + else: + disk.color.r, disk.color.g, disk.color.b = 0.0, 0.0, 1.0 # 蓝 + + # 设置位姿 (直接使用数学模型计算出的矩阵) disk.pose.position.x = T[0, 3] disk.pose.position.y = T[1, 3] disk.pose.position.z = T[2, 3] - # 姿态 r = Rotation.from_matrix(T[:3, :3]) q = r.as_quat() disk.pose.orientation.x = q[0] @@ -127,28 +181,29 @@ class SoftArmSimulator(Node): disk.pose.orientation.z = q[2] disk.pose.orientation.w = q[3] + # lifetime=0 表示永久显示，直到收到新的更新覆盖它 disk.lifetime.sec = 0 disk.lifetime.nanosec = 0 marker_array.markers.append(disk) - # --- B. 创建 Backbone Marker (白色中轴) --- + # --- B. 构建 Backbone Marker (中心连线) --- line = Marker() line.header.stamp = zero_time line.header.frame_id = "base_link" line.ns = "backbone" line.id = 0 - line.type = Marker.LINE_STRIP + line.type = Marker.LINE_STRIP # 形状：线条带 line.action = Marker.ADD - line.scale.x = 0.008 + line.scale.x = 0.008 # 线条粗细 line.color.a = 1.0 - line.color.r = 0.9 + line.color.r = 0.9 # 浅灰色 line.color.g = 0.9 line.color.b = 0.9 line.lifetime.sec = 0 line.lifetime.nanosec = 0 - # 填充点 + # 填充线条的点 for p_np in curve_points: p = Point() p.x, p.y, p.z = float(p_np[0]), float(p_np[1]), float(p_np[2]) @@ -156,12 +211,12 @@ class SoftArmSimulator(Node): marker_array.markers.append(line) - # 发布 + # 统一发布 self.marker_pub.publish(marker_array) def main(args=None): rclpy.init(args=args) node = SoftArmSimulator() - rclpy.spin(node) + rclpy.spin(node) # 保持节点运行，直到按 Ctrl+C node.destroy_node() rclpy.shutdown() \ No newline at end of file diff --git a/soft_arm_sim/soft_arm_sim/model/__pycache__/pcc_kinematics.cpython-310.pyc b/soft_arm_sim/soft_arm_sim/model/__pycache__/pcc_kinematics.cpython-310.pyc index 1f4f702f18aac23439fd47ad08ebee4ea43d19c8..688308dd0c4befce1121e8fc6a5982ccebd9f1d2 100644 GIT binary patch literal 4541 zcmZu!>vI#=72lUuYe_ba@iZYcnQdm;N^Ob>O`DVkj`a`Hz6dX;#udZ-rc3ImM@iOX1yDW?)6D`;8uC`q<3k^ z8Jk*L_|QA^vG>s#=hTQh_dt#APp0~l*+T5gfs;&(J3o#&{-`&3w|wTN^Y9)x>T7AU zPmMZPM!d@xVyd?^hNN9cO zY_dOINGHr4`D3=CY3XdbplJ%-m59vKuZ=&xb=_t=YO@mL9<`?pjUZm8rk8oBN%)gk z5=&AkP~fI{ouA=LazU`fL0%X7IOAzuv}9e{1E<7)U=OKP)@A(5`#GpJu$?Ofu~I~v zr(P!q2?{t;6Q_mEkuVlwW7GcPi3aKh^x^jbQ(l(>N#Zk)qb zg=_P|9JQ=)^a`G$&26#I$InL1%Quow`E-GdJVSop^r-qq;ZU*=e^$*ON=;=H|O*i0D`8^U8`Ii_OGu-D+8CDg$3`E z(`$?G;O>Bw%EcR%tJlf{gXN3wt}QQk%eU9%RtPy#WB++D{ojAwdD(z9xtPHIDik;| zp+YQUi{B=QVQr%wt$jXQwuNlo7L&)4wirKX+F~LHVy3fpAZ{4(W3e`aV$S9PF-PnG z)1fUw33jl09h*O3^L@6EDA zQdpaW{-JP)kH`v7Kj}M)(;;^7A^apM%18N5UjABA+JF?mi?2K7c3u*WKi!BKn?)z+ z%C(2UrVby!M#tg-9|8eLiX#Zofx3qQBad@~!fV`{9VOY4h=X90I{_6|U>b-FxCmPO zj4;FV+>tF-0D&YFM(G+vcwnz>E=s1-$$U$s5p3f(< zy766fVhXdTu?=nGNg~McobhcCJ5)CejGZ=~paDt_4F>Fh*^b5yy#dyLi5VtYHx!Wg zPN7Zc6eRxmQw^5cyrx1WMiaObL&nR%y&DZ?9R}{Ojtl^VWvqZD%!sf=5cb(xQeX|i zQpg&m7E6X@TBvFO@QJ;`Hh?)S)J|X@lZx*{WZgpEqA&5JH8)$|~^Fy*lGv zIa7Y{xqI~{jXwO;U3$0C-Yj;jktrD+IJ2$5$YwsCNHX&o-v=)iTDPGcLPpo}xpcN* zZnUTDvu(xH-i~LANz*s!QxLJyuI&^&iK6j#vMTG3n8ZSptlB0he1w03hh4Wf*mdKY zeKFqnA)Qa5JfDsCCpFCuX&Q=wVusogO*>MIXR0GXP1AD;O*2~QK;)dp4~Xm}vhH`R zMgC<_G!w2dZ&NslS0qJNUToXKMiLlldT8pac+F=(no7%%j&c$uq@)LR1w~~*Z-JA9 z?Z@_`y!pA&|J!=aQXI!x<~N&$s)eO<;}VsQEJ$NGlW9^|NR)?>Dp##to@QOZgp@u_E39ynj0#t!?4{LyOhXaHBpny~)TWG;P zzd;H@&AN?}4@Rjq4e*0BK%d_~tXM(%L>eOqY{8oCrBHnr28INMR;Zn;a~U{3&XvOK zL}6y{J(hyC;Zg)CFVfDHS}g&mZDnIUmSSWqL8o&_N1h>1=_T(77SA=-kJW;4>OTD1 zZHNTlh0CiGa2Ds24;t;i?pcp05DyckX^C96H=RPU=$tx_Z#X!ab9UI7_=tU(4frLL z?^x;^QC6Ki$I3v^?&o69sy=TQXZKSZohoo=uoni!OtX=Fx+K3{ZDS{$IP+TJ3 z2NdD>w#|`Vm4t6?zij>~DTp*`yZ{0OdXCyux*9(wLOAOuv)8Hhs1)`S3{awCpT3mB pLQ-UlL0qcYfPDF5zJUl_82?dXY-Nw1Q|T+1GYNPUu1)B8>;FR+vXlS- literal 3233 zcmZuz-ESOM6`yX?_I~-! zY@=ASQiR>mL<(u8AWBoHjViT$Nh?s0(@^D&2OjtX?knEiScq4UkRte<8SjUk+|fOA z&Y649z4zSnJ7?-6BesI3|2rt}9Z{5jkTBd-2rr67eyI4|S2q=3-QWux zlIH7>bV#btE+_?K52Ka2BGKX2w{{BJ0G51Zf&2MnKEqtYH#e09&A0pv-nxIp&-%Gd zwqW=pusSO3AD=sSAt*-Wdab*2;;n!FWasbC?~&2&`R`nP@s0M@^F?iFJ>og~IrQ*J zkeJ1atHd0Xf#;J7IPNPj&gKfLTwh)WV>$B#lZ? zD@DuFqET+GS(Vv@6P{mghMy{81PlE)m|3n@gPCf%K2up*pJ|Bt3idrC>Wg)v;c53` zU98T8^`*$Y7|tv<%N0MBJW+MSa`m84t@}Y`rco@qtL0iy^`deyoNio_mg|;l<;ZpE zsG$L3FoWgU`q6=>rh8+GO3TDd6+9zpT!94(8ts54lZ?^u%}%}%mj$QC zc5II;vE`e`G1oVbDbQpG8c3OOrKBFxW{1Zlv$DN$S%tVbyZ^RIYDvXvxpXz(j4fIz zHW3Bc15$=`$=H!Nhf^XP-{L7<;yCe0UzbT_c%Wc67vW9Y*>4YSZ+CD1rv1T{_Fq2U z-MO}V?_Ou~#_pHbyIW|JR+Z|NQmyvjP{V6nOFy^rgV}NY&#h`V(F}0d6i;8o(7+7NbXd%Ic2kW6Z{*@hLXN z#+mU@GxCUMMD@dQV-))J6T^}HsOcgdxz{W({UCoCLd+24oWi`MP@wxr2rCHoW6B19 zNqK#&WyBh35E8~X0y@^WkSq|?X3VzvHe<@#kyyud^X9WK_u&4`_O0J{u3qkZaMgJU z`M^1Jtq_?*cR}INTRZK~?zVsTPG{?vh5p3Zh9|tL zgM92Z>g8G#&N>h7|8{rha_9D)?#&N7Z@&XWFwyz-=g!oFKVNHq^ghwMf9l+OcVHy} z(mqTyaTGg~R;64E8(uL;E=f=wEZBpalJ;Q9!?$LnJCRoCzUft(L6~0L379y1d!rP| z#in>O*x&wlF(@Pe*}BH^+~PTQoWg%{==KiJK?xI-<5R@z;0jEfq?bvRqUKct*Oj*G zqMS4<#OGXht?5S+w6){-hk^xMn%TUD1NC8;;&-+$KR4|RQrE1L0=s?V-S*A~a-uW!JueKL zXU`_da{$BB(@w*SmitIPFzr=OtOmlFO0nCy^+zO$!BjDd?W72%A6i1dG+ulELo1jG zq{TQC36Nzb0D1%AMS=J>Df@9>7)fj3$ON$wOO}`FTRI zzz?Ilh@*+G!>TmW>2hp8%7!iED)piV8%fTRX2VTnX*JwZrM`$#YtHvn^ZTkp!70uC zEdDdB6@C>&Gc;!Nuc)JZl#gqcI)&t6p~~i%2Ffp6e0}24(A~E(SI`BOX7LRoR6^3c z@O|PQmkqv!53iypzn_+ZWh_BDDs}K~<5KE$0z{&JoDpfANplkYiS{hz`PeJ}2N5GZ AAOHXW diff --git a/soft_arm_sim/soft_arm_sim/model/pcc_kinematics.py b/soft_arm_sim/soft_arm_sim/model/pcc_kinematics.py index 93e4c34..ca3f60d 100644 --- a/soft_arm_sim/soft_arm_sim/model/pcc_kinematics.py +++ b/soft_arm_sim/soft_arm_sim/model/pcc_kinematics.py @@ -1,47 +1,65 @@ import numpy as np class PCCSection: - def __init__(self, length=0.240, disk_num=3, disk_radius=0.033): - self.L0 = length - self.n_disks = disk_num - self.d_per_segment = length / disk_num + """ + PCCSection 类：表示柔性臂的单个独立分段 (Segment)。 + 每个分段由若干个 Disk (圆盘) 组成，且假设该段内部曲率恒定。 + """ + def __init__(self, length, disk_num, disk_radius): + self.L0 = length # 该段的总长度 (弧长) + self.n_disks = disk_num # 该段包含的 Disk 数量 + self.r = disk_radius # Disk 半径 (用于物理计算，此处暂未深度使用) + self.d_per_segment = length / disk_num # 两个 Disk 之间的沿弧长距离 def _get_transform_at_s(self, s, q): """ - 计算圆弧上任意位置 s 处的变换矩阵 - q: [theta, phi, length] + 核心函数：计算沿 PCC 曲线 s 处的齐次变换矩阵 T (4x4)。 + + 参数: + s: 沿曲线的弧长位置 (0 <= s <= L0) + q: 构型空间坐标 [theta, phi, s_total] + theta: 弯曲角度 (决定曲率 k = theta / s_total) + phi: 弯曲方向 (绕 Z 轴的偏转角) + s_total: 当前段的实际弧长 (通常等于 L0，除非考虑伸缩) """ theta, phi, s_total = q + T = np.eye(4) # 初始化单位矩阵 - T = np.eye(4) - - # 奇异点处理：直线状态 + # --- 奇异点处理 --- + # 当 theta 接近 0 时，机械臂处于直立状态。 + # 此时曲率 k -> 0，半径 R -> 无穷大，直接套用 PCC 公式会导致除以零错误。 + # 因此，直立状态下直接简化为沿 Z 轴的平移。 if abs(theta) < 1e-6: T[0, 3] = 0 T[1, 3] = 0 - T[2, 3] = s + T[2, 3] = s # 纯粹沿 Z 轴向上延伸 s 长度 else: - # 常曲率公式 - k = theta / s_total # 曲率 - # 注意：这里的 s 是当前点在弧上的长度 - # 当 s > s_total 时（拉伸），我们假设曲率均匀分布在整个 s_total 上 - # 这里简化处理，直接用 s 计算几何 + # --- PCC 变换公式 --- + # 1. 曲率 k = theta / s_total + # 2. 变换逻辑：先旋转 phi 到弯曲平面 -> 在平面内弯曲 theta -> 旋转 -phi 回去 (等效推导结果如下) - ct = np.cos(theta * (s / s_total)) - st = np.sin(theta * (s / s_total)) - cp = np.cos(phi) - sp = np.sin(phi) + k = theta / s_total + # 预计算三角函数，减少重复计算 + ct = np.cos(theta * (s / s_total)) # 当前位置的切线角度余弦 + st = np.sin(theta * (s / s_total)) # 当前位置的切线角度正弦 + cp = np.cos(phi) # 弯曲平面的方位角余弦 + sp = np.sin(phi) # 弯曲平面的方位角正弦 + + # 旋转矩阵 R (3x3) + # 描述了当前截面相对于底部的旋转姿态 R = np.array([ [cp*cp*(ct-1)+1, cp*sp*(ct-1), cp*st], [cp*sp*(ct-1), sp*sp*(ct-1)+1, sp*st], [-cp*st, -sp*st, ct] ]) + # 位置向量 p (3x1) + # 描述了当前截面中心在空间中的坐标 (x, y, z) p = np.array([ - (cp * (1 - ct)) / k, - (sp * (1 - ct)) / k, - st / k + (cp * (1 - ct)) / k, # x 坐标：在弯曲方向上的投影 + (sp * (1 - ct)) / k, # y 坐标 + st / k # z 坐标：高度 ]) T[:3, :3] = R @@ -50,73 +68,96 @@ class PCCSection: return T def forward_kinematics(self, q): - """返回所有 Disk 的变换矩阵 (用于 TF)""" + """ + 计算该段内所有 Disk 相对于该段底部的变换矩阵。 + 用于放置可视化模型 (Cylinder)。 + """ theta, phi, s_total = q transforms = [] + # 遍历该段内的每一个 Disk (索引从1开始，不包括底座) for i in range(1, self.n_disks + 1): + # 计算第 i 个 disk 在曲线上的弧长位置 s = (s_total / self.n_disks) * i + # 调用核心公式计算变换 transforms.append(self._get_transform_at_s(s, q)) return transforms def get_curve_points(self, q, num_points=10): """ - 返回用于画线的密集点集 (相对坐标) - :param num_points: 这一段生成的点数 (越多越平滑) + 生成用于可视化的骨架曲线点集 (Marker LineStrip)。 + 比 forward_kinematics 采样更密集，使线条看起来更平滑。 """ theta, phi, s_total = q points = [] - # 生成从 0 到 L 的密集点 + # 在 0 到 s_total 之间均匀生成 num_points 个点 s_values = np.linspace(0, s_total, num_points) - for s in s_values: T = self._get_transform_at_s(s, q) - points.append(T[:3, 3]) # 只取位置 xyz - + points.append(T[:3, 3]) # 只提取位置 (x, y, z) return points class SoftArmKinematics: - def __init__(self): - # 3段 PCC - self.sections = [ - PCCSection(length=0.24, disk_num=3), - PCCSection(length=0.24, disk_num=3), - PCCSection(length=0.24, disk_num=3) - ] + """ + SoftArmKinematics 类：多段 PCC 机械臂的高层管理器。 + 负责将多个 PCCSection 串联起来，计算全局坐标。 + """ + def __init__(self, num_sections=3, section_length=0.24, disks_per_section=3, disk_radius=0.033): + """ + 初始化时接收动态参数，支持任意段数和长度的配置。 + """ + self.num_sections = num_sections + self.sections = [] + + # 根据参数动态生成 Section 对象列表 + for _ in range(num_sections): + self.sections.append(PCCSection( + length=section_length, + disk_num=disks_per_section, + disk_radius=disk_radius + )) def forward(self, joint_configs): """ + 计算整个机械臂的全局运动学。 + + 参数: + joint_configs: 列表，包含每一段的配置 [(theta1, phi1, L1), (theta2, phi2, L2), ...] + 返回: - 1. transforms: 用于发布 TF (Base -> Disk) - 2. path_points: 用于发布 Marker (平滑曲线) + all_transforms: 所有 Disk 相对于世界坐标系 (Base) 的变换矩阵列表。 + all_path_points: 整个机械臂骨架曲线的全局坐标点列表。 """ all_transforms = [] all_path_points = [] - T_current_base = np.eye(4) # 当前段基座 + # T_current_base: 当前段的基坐标系。 + # 初始为单位矩阵 (机械臂根部在世界原点) + T_current_base = np.eye(4) + all_path_points.append(T_current_base[:3, 3]) # 添加原点 - # 初始点 (0,0,0) - all_path_points.append(T_current_base[:3, 3]) + # 安全检查：防止输入的配置数量少于定义的段数 (例如只给了1组数据但有3段) + limit = min(len(joint_configs), len(self.sections)) - for i, config in enumerate(joint_configs): - # 1. 计算 TF (Disks) - section_transforms = self.sections[i].forward_kinematics(config) + for i in range(limit): + config = joint_configs[i] - # 2. 计算 曲线点 (Marker) - # 获取当前段内的局部密集点 + # 1. 计算当前段的局部变换 (相对于当前段的底部) + section_transforms = self.sections[i].forward_kinematics(config) local_points = self.sections[i].get_curve_points(config, num_points=15) - # 将局部点转换到全局坐标系 + # 2. 坐标系转换：局部 -> 全局 + # 公式：P_global = T_current_base * P_local for p_local in local_points: - # 点: p_global = R * p_local + t p_global = T_current_base[:3, :3] @ p_local + T_current_base[:3, 3] all_path_points.append(p_global) - # 处理 TF 级联 for T_local in section_transforms: + # 矩阵乘法级联变换 T_global = T_current_base @ T_local all_transforms.append(T_global) - # 更新下一段基座 + # 3. 更新基坐标系 + # 下一段的底部 = 当前段的末端 (即当前段 transform 列表中的最后一个) if section_transforms: T_current_base = all_transforms[-1]