myCobot280pi实机部署部署成功后基于这个ROS2的整体框架想实现什么功能都可以在包内添加或者修改文件来进行实现。1.先在官网下一个官方文件启动socket命令行如下git clone https://github.com/elephantrobotics/pymycobot.git2.下载后进入demo文件夹下启动Server.py记录自己的ip和port3.然后回到上位机ubuntu系统回到ROS2/MyCobot280/src文件夹下新建python文件interpolation.py直线插补和圆弧插补实现函数和line_arc_execute_demo.py实机运行代码。因为第1部分的marker可视化仿真程序和execute真机执行程序都用到直线和圆弧生成代码所以将直线和圆弧生成代码独立成interpolation.py。interpolation.py代码如下import math def generate_line_points(start_xyz, end_xyz, num_points50): 生成直线插补点. 输入/输出单位由调用方决定可统一使用 m 或 mm。 x0, y0, z0 start_xyz x1, y1, z1 end_xyz points [] for i in range(num_points 1): s i / num_points x x0 s * (x1 - x0) y y0 s * (y1 - y0) z z0 s * (z1 - z0) points.append((x, y, z)) return points def generate_arc_points_xy(center_xyz, radius, theta_start, theta_end, num_points100): 生成 XY 平面圆弧插补点z 保持不变。 cx, cy, cz center_xyz points [] for i in range(num_points 1): s i / num_points theta theta_start s * (theta_end - theta_start) x cx radius * math.cos(theta) y cy radius * math.sin(theta) z cz points.append((x, y, z)) return points def polyline_length(points): 计算折线总长度。 if len(points) 2: return 0.0 total 0.0 for i in range(1, len(points)): x0, y0, z0 points[i - 1] x1, y1, z1 points[i] ds math.sqrt((x1 - x0) ** 2 (y1 - y0) ** 2 (z1 - z0) ** 2) total ds return total def offset_point(xyz, dx0.0, dy0.0, dz0.0): x, y, z xyz return (x dx, y dy, z dz)建立的line_arc_execute_demo.py文件实机运行代码如下注意将ip和port改成自己机器的代码先从初始位置运行到工作区然后先进行直线插补再进行圆弧插补最后回到初始位置import math import time import rclpy from rclpy.node import Node from mycobot_interp_demo.interpolation import ( generate_line_points, generate_arc_points_xy, polyline_length, ) # 优先使用 280 专用 socket 类若当前 pymycobot 版本没有则回退到通用类 try: from pymycobot import MyCobot280Socket as SocketClient except ImportError: from pymycobot import MyCobotSocket as SocketClient class LineArcExecuteDemo(Node): def __init__(self): super().__init__(line_arc_execute_demo) # ---------- 连接参数 ---------- self.declare_parameter(robot_ip, 192.168.1.110) self.declare_parameter(robot_port, 9000) # ---------- 执行参数 ---------- self.declare_parameter(speed, 10) self.declare_parameter(send_interval, 0.30) # 点间隔略小配合更密的点列 self.declare_parameter(send_mode, 0) # 先用 0更稳 # ---------- 你的官方初始位关节角 ---------- self.declare_parameter(init_j1, 90.0) self.declare_parameter(init_j2, 0.0) self.declare_parameter(init_j3, 0.0) self.declare_parameter(init_j4, 0.0) self.declare_parameter(init_j5, -90.0) self.declare_parameter(init_j6, 90.0) # ---------- 你的官方初始位末端坐标 ---------- self.declare_parameter(init_x, 102.7) self.declare_parameter(init_y, -25.1) self.declare_parameter(init_z, 412.67) self.declare_parameter(init_rx, 170.72) self.declare_parameter(init_ry, 89.59) self.declare_parameter(init_rz, 140.4) # ---------- 直线插补参数 ---------- # 比你当前版本略扩大但仍保守 self.declare_parameter(line_dx, 40.0) self.declare_parameter(line_dy, 20.0) self.declare_parameter(line_dz, 0.0) self.declare_parameter(line_num_points, 18) # ---------- 圆弧插补参数 ---------- # 让圆弧从直线终点开始在 XY 平面走 90° 圆弧 self.declare_parameter(arc_radius, 30.0) self.declare_parameter(arc_theta_start, -math.pi / 2.0) self.declare_parameter(arc_theta_end, 0.0) self.declare_parameter(arc_num_points, 18) # ---------- 工作位与安全范围 ---------- # 初始位 z412.67 接近上限所以先降到更安全的工作高度再做插补 self.declare_parameter(work_z, 388.0) self.declare_parameter(z_min, -70.0) self.declare_parameter(z_max, 410.0) # ---------- 是否回官方初始位 ---------- self.declare_parameter(return_to_init_pose, True) robot_ip self.get_parameter(robot_ip).value robot_port int(self.get_parameter(robot_port).value) self.speed int(self.get_parameter(speed).value) self.send_interval float(self.get_parameter(send_interval).value) self.send_mode int(self.get_parameter(send_mode).value) self.init_angles [ float(self.get_parameter(init_j1).value), float(self.get_parameter(init_j2).value), float(self.get_parameter(init_j3).value), float(self.get_parameter(init_j4).value), float(self.get_parameter(init_j5).value), float(self.get_parameter(init_j6).value), ] self.init_xyz ( float(self.get_parameter(init_x).value), float(self.get_parameter(init_y).value), float(self.get_parameter(init_z).value), ) self.init_rpy ( float(self.get_parameter(init_rx).value), float(self.get_parameter(init_ry).value), float(self.get_parameter(init_rz).value), ) self.line_dx float(self.get_parameter(line_dx).value) self.line_dy float(self.get_parameter(line_dy).value) self.line_dz float(self.get_parameter(line_dz).value) self.line_num_points int(self.get_parameter(line_num_points).value) self.arc_radius float(self.get_parameter(arc_radius).value) self.arc_theta_start float(self.get_parameter(arc_theta_start).value) self.arc_theta_end float(self.get_parameter(arc_theta_end).value) self.arc_num_points int(self.get_parameter(arc_num_points).value) self.work_z float(self.get_parameter(work_z).value) self.z_min float(self.get_parameter(z_min).value) self.z_max float(self.get_parameter(z_max).value) self.return_to_init_pose bool(self.get_parameter(return_to_init_pose).value) # ---------- 连接机器人Socket ---------- self.mc SocketClient(robot_ip, robot_port) if hasattr(self.mc, connect): try: self.mc.connect() except Exception: pass time.sleep(2.0) self.get_logger().info(fconnected by socket: ip{robot_ip}, port{robot_port}) self.timer self.create_timer(2.0, self.run_once) self.executed False def run_once(self): if self.executed: return self.executed True try: self.execute_demo() except Exception as e: self.get_logger().error(fexecute_demo failed: {e}) def enable_robot(self): if hasattr(self.mc, power_on): try: self.mc.power_on() self.get_logger().info(power_on() sent) except Exception as e: self.get_logger().warn(fpower_on failed: {e}) time.sleep(1.0) if hasattr(self.mc, state_on): try: self.mc.state_on() self.get_logger().info(state_on() sent) except Exception as e: self.get_logger().warn(fstate_on failed: {e}) time.sleep(1.0) def clamp_z(self, z): if z self.z_min: self.get_logger().warn( fz{z:.2f} below z_min{self.z_min:.2f}, clamp to z_min ) return self.z_min if z self.z_max: self.get_logger().warn( fz{z:.2f} above z_max{self.z_max:.2f}, clamp to z_max ) return self.z_max return z def send_angles_safe(self, angles_deg): self.get_logger().info(fsend_angles {angles_deg}) self.mc.send_angles(angles_deg, self.speed) time.sleep(3.0) def send_xyz_keep_rpy(self, xyz_mm, rpy_deg): x, y, z xyz_mm rx, ry, rz rpy_deg target [x, y, z, rx, ry, rz] self.mc.send_coords(target, self.speed, self.send_mode) time.sleep(self.send_interval) def execute_path(self, points, rpy_deg, tagpath): for i, p in enumerate(points): self.get_logger().info( f{tag}[{i:02d}] ({p[0]:.2f}, {p[1]:.2f}, {p[2]:.2f}) ) self.send_xyz_keep_rpy(p, rpy_deg) def execute_demo(self): self.enable_robot() # 1) 回官方初始位关节角回去更稳 self.get_logger().info(go to official init joint pose) self.send_angles_safe(self.init_angles) # 2) 降到安全工作位 work_start ( self.init_xyz[0], self.init_xyz[1], self.clamp_z(self.work_z) ) self.get_logger().info( fwork_start ({work_start[0]:.2f}, {work_start[1]:.2f}, {work_start[2]:.2f}) ) self.send_xyz_keep_rpy(work_start, self.init_rpy) time.sleep(1.0) # 3) 生成直线 line_end ( work_start[0] self.line_dx, work_start[1] self.line_dy, self.clamp_z(work_start[2] self.line_dz) ) line_points generate_line_points( work_start, line_end, num_pointsself.line_num_points ) line_len polyline_length(line_points) self.get_logger().info( fline: start{work_start}, end{line_end}, fpoints{len(line_points)}, length{line_len:.2f} mm ) # 4) 生成圆弧 # 圆心设置为 line_end 正上方一个半径这样 arc 起点正好是 line_end arc_center ( line_end[0], line_end[1] self.arc_radius, line_end[2] ) arc_points generate_arc_points_xy( arc_center, radiusself.arc_radius, theta_startself.arc_theta_start, theta_endself.arc_theta_end, num_pointsself.arc_num_points ) arc_len polyline_length(arc_points) arc_end arc_points[-1] self.get_logger().info( farc: center{arc_center}, end{arc_end}, fpoints{len(arc_points)}, length{arc_len:.2f} mm ) # 5) 执行直线跳过起点 self.get_logger().info(start line execution) self.execute_path(line_points[1:], self.init_rpy, tagline) time.sleep(0.8) # 6) 执行圆弧跳过首点因为首点就是 line_end self.get_logger().info(start arc execution) self.execute_path(arc_points[1:], self.init_rpy, tagarc) time.sleep(0.8) # 7) 按原轨迹反向返回到安全工作位 # 这样比“从 arc 终点直接跳回安全区”更平滑 #self.get_logger().info(return along reverse arc) #reverse_arc list(reversed(arc_points[:-1])) # 回到 line_end #self.execute_path(reverse_arc, self.init_rpy, tagarc_back) #time.sleep(0.6) #self.get_logger().info(return along reverse line) #reverse_line list(reversed(line_points[:-1])) # 回到 work_start #self.execute_path(reverse_line, self.init_rpy, tagline_back) #time.sleep(0.8) # 8) 再回官方初始位 if self.return_to_init_pose: self.get_logger().info(return to official init joint pose) self.send_angles_safe(self.init_angles) self.get_logger().info(demo finished) def main(argsNone): rclpy.init(argsargs) node LineArcExecuteDemo() rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ __main__: main()4.修改setup.py文件加入excute实机执行文件入口entry_points{ console_scripts: [ line_arc_marker_demo mycobot_interp_demo.line_arc_marker_demo:main, line_arc_execute_demo mycobot_interp_demo.line_arc_execute_demo:main, ],5.之后回到ROS2/myCobot280文件夹下编译colcon build6.source和运行命令source install/setup.bash ros2 run mycobot_interp_demo line_arc_execute_demo7.运行机械臂状态如图晃动原因是我的底座插口插的较少然后吸盘没固定由于文件大小限制gif只保留了直线插补和圆弧插补视频部分回到初始位置未保留。想要提高精度就增加插补过程中的插补点个数。