openarm/openarm_test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

把位置控制器换成关节控制器

Browse Source
This commit is contained in:
shenchenyang 2026-03-09 16:11:45 +08:00
parent 933d5ec745
commit 019807eb74
1 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/openarm_servo_control/openarm_servo_control/ik_controller.py
View File

@ -85,8 +85,8 @@ class ArucoIKController(Node):
self.control_period = 0.05 # 20 ms self.control_period = 0.05 # 20 ms
self.joint_limiter = JointLimiter( self.joint_limiter = JointLimiter(
dof=7, dof=7,
max_velocity=0.2, # 建议 0.1~0.3，降低以减小速度 max_velocity=0.01, # 建议 0.1~0.3，降低以减小速度
max_acceleration=0.5, # 建议 0.2~0.6，降低以减小加速度 max_acceleration=0.01, # 建议 0.2~0.6，降低以减小加速度
dt=self.control_period dt=self.control_period
) )
self.pose_queue = queue.Queue(maxsize=10) # 用于存储最近的目标位姿，避免处理过旧的指令 self.pose_queue = queue.Queue(maxsize=10) # 用于存储最近的目标位姿，避免处理过旧的指令
@ -310,17 +310,17 @@ class ArucoIKController(Node):
jt.joint_names = self.joint_names jt.joint_names = self.joint_names
# 计算受限后的关节目标 # 计算受限后的关节目标
q_sol = np.array(res.solution.joint_state.position[:len(self.joint_names)]) q_sol = np.array(res.solution.joint_state.position[:len(self.joint_names)])
q_limited = self.joint_limiter.limit(q_sol) # q_limited = self.joint_limiter.limit(q_sol)
# 计算速度（基于上一帧的 q），并限幅 # # 计算速度（基于上一帧的 q），并限幅
if self.prev_q is None: if self.prev_q is None:
vel = np.zeros_like(q_limited) vel = np.zeros_like(q_sol)
else: else:
vel = (q_limited - self.prev_q) / self.control_period vel = (q_sol - self.prev_q) / self.control_period
vel = np.clip(vel, -self.joint_limiter.max_velocity, self.joint_limiter.max_velocity) vel = np.clip(vel, -self.joint_limiter.max_velocity, self.joint_limiter.max_velocity)
# 计算加速度并限幅 # # 计算加速度并限幅
if self.prev_vel is None: if self.prev_vel is None:
acc = np.zeros_like(vel) acc = np.zeros_like(vel)
else: else:
@ -329,26 +329,26 @@ class ArucoIKController(Node):
acc = np.clip(acc, -self.joint_limiter.max_acceleration, self.joint_limiter.max_acceleration) acc = np.clip(acc, -self.joint_limiter.max_acceleration, self.joint_limiter.max_acceleration)
# 保存为下一次使用 # 保存为下一次使用
self.prev_q = q_limited.copy() self.prev_q = q_sol.copy()
self.prev_vel = vel.copy() self.prev_vel = vel.copy()
# 构造轨迹点：把 time_from_start 设大一些（例如 0.1s）让控制器有更长时间插值，减少瞬时加速度 # 构造轨迹点：把 time_from_start 设大一些（例如 0.1s）让控制器有更长时间插值，减少瞬时加速度
# point = JointTrajectoryPoint() point = JointTrajectoryPoint()
msg = Float64MultiArray() # msg = Float64MultiArray()
# 在 velocity-only 模式下，不设置 positions，只填 velocities（取决于控制器实现是否支持） # 在 velocity-only 模式下，不设置 positions，只填 velocities（取决于控制器实现是否支持）
# if not self.publish_velocity_only: # if not self.publish_velocity_only:
# point.positions = q_limited.tolist() # point.positions = q_limited.tolist()
point.positions = q_sol.tolist()
# point.velocities = vel.tolist() point.velocities = vel.tolist()
# point.accelerations = acc.tolist() point.accelerations = acc.tolist()
# point.time_from_start.sec = 0 # point.time_from_start.sec = 0
# point.time_from_start.nanosec = int(0.1 * 1e9) # 0.1s # point.time_from_start.nanosec = int(0.1 * 1e9) # 0.1s
msg.data = (q_sol).tolist() # msg.data = (q_sol).tolist()
# self.get_logger().info(f"msg.data: {msg.data}") # self.get_logger().info(f"msg.data: {msg.data}")
# self.get_logger().info(f"Publishing joint command: vel={np.round(vel,3).tolist()}, acc={np.round(acc,3).tolist()}, pos={np.round(q_limited,3).tolist()}") # self.get_logger().info(f"Publishing joint command: vel={np.round(vel,3).tolist()}, acc={np.round(acc,3).tolist()}, pos={np.round(q_limited,3).tolist()}")
self.float64MultiArrayPublisher_.publish(msg) # self.float64MultiArrayPublisher_.publish(msg)
# jt.points.append(point) jt.points.append(point)
# self.traj_pub.publish(jt) self.traj_pub.publish(jt)
def main(): def main():
rclpy.init() rclpy.init()