diff --git a/src/embodied_robot/robot_walk/move_straight.py b/src/embodied_robot/robot_walk/move_straight.py index bba3b0c182..17f7e1a5c9 100644 --- a/src/embodied_robot/robot_walk/move_straight.py +++ b/src/embodied_robot/robot_walk/move_straight.py @@ -3,7 +3,7 @@ """ DeepMind Humanoid Robot Simulation Dynamic Obstacle Avoidance + Moving Target Tracking -适配修改后的官方Humanoid模型 +稳定版:解决机器人一动就倒地问题 UTF-8 encoded, GitHub compatible """ @@ -17,7 +17,6 @@ import sys # ====================== Global Configuration ====================== -# Disable all log output (cross-version compatible) os.environ['MUJOCO_QUIET'] = '1' os.environ['MUJOCO_GL'] = 'egl' os.environ['PYTHONIOENCODING'] = 'utf-8' @@ -27,9 +26,9 @@ random.seed(42) -class DynamicPatrolController: +class StablePatrolController: def __init__(self, model_path): - """Initialize robot controller with dynamic target and obstacle support""" + """Initialize robot controller with enhanced balance control""" # Load model and data self.model = mujoco.MjModel.from_xml_path(model_path) self.data = mujoco.MjData(self.model) @@ -38,32 +37,33 @@ def __init__(self, model_path): # Simulation settings self.sim_start_time = 0.0 self.last_print_time = 0.0 - self.target_switch_cooldown = 2.0 + self.target_switch_cooldown = 3.0 # 延长冷却时间 self.last_target_switch_time = 0.0 + self.stabilization_phase = 2.0 # 初始稳定阶段(前2秒不移动) - # Patrol target configuration (dynamic updatable) + # Patrol target configuration self.patrol_points = [ - {"name": "patrol_target_1", "pos": np.array([0.0, 0.0]), "label": "Start Point", "update_interval": 8.0}, - {"name": "patrol_target_2", "pos": np.array([4.0, -2.0]), "label": "Patrol Point 1 (SW)", - "update_interval": 10.0}, - {"name": "patrol_target_3", "pos": np.array([8.0, 2.0]), "label": "Patrol Point 2 (NE)", + {"name": "patrol_target_1", "pos": np.array([0.0, 0.0]), "label": "Start Point", "update_interval": 10.0}, + {"name": "patrol_target_2", "pos": np.array([2.0, -1.0]), "label": "Patrol Point 1 (SW)", "update_interval": 12.0}, - {"name": "patrol_target_4", "pos": np.array([10.0, -1.0]), "label": "Patrol Point 3 (NW)", - "update_interval": 9.0}, - {"name": "patrol_target_5", "pos": np.array([12.0, 0.0]), "label": "Final Point", "update_interval": 11.0} + {"name": "patrol_target_3", "pos": np.array([4.0, 1.0]), "label": "Patrol Point 2 (NE)", + "update_interval": 14.0}, + {"name": "patrol_target_4", "pos": np.array([6.0, -0.5]), "label": "Patrol Point 3 (NW)", + "update_interval": 11.0}, + {"name": "patrol_target_5", "pos": np.array([8.0, 0.0]), "label": "Final Point", "update_interval": 13.0} ] self.current_target_idx = 0 self.patrol_cycles = 0 self.patrol_completed = False - self.target_reached_threshold = 0.8 + self.target_reached_threshold = 1.0 # 增大到达阈值,减少频繁转向 # Dynamic target control parameters self.patrol_motor_ids = {} self.patrol_joint_ids = {} self.patrol_body_ids = {} self.last_target_update = {i: 0.0 for i in range(len(self.patrol_points))} - self.target_movement_range = {"x": [-2.0, 14.0], "y": [-5.0, 5.0]} - self.target_move_speed = 0.5 + self.target_movement_range = {"x": [-1.0, 9.0], "y": [-3.0, 3.0]} + self.target_move_speed = 0.2 # 降低目标移动速度 # Obstacle detection & avoidance parameters self.valid_wall_names = ["wall1", "wall2", "wall3", "wall4"] @@ -71,9 +71,9 @@ def __init__(self, model_path): self.wall_names = [] self.wall_types = {} self.wall_pos_history = {} - self.obstacle_distance_threshold = 2.0 - self.obstacle_avoidance_duration = 5.0 - self.return_to_path_duration = 4.0 + self.obstacle_distance_threshold = 2.5 # 增大避障阈值,提前反应 + self.obstacle_avoidance_duration = 6.0 # 延长避障时间 + self.return_to_path_duration = 5.0 # 延长返回路径时间 self.wall_priority = {"dynamic1": 4, "dynamic2": 3, "dynamic3": 2, "fixed": 1} # Avoidance state variables @@ -85,71 +85,51 @@ def __init__(self, model_path): self.closest_wall_info = {"name": "", "distance": float('inf'), "type": ""} self.turn_dir_label = "" - # Robot control parameters (适配官方Humanoid参数) - self.gait_period = 2.0 # 调整步态周期适配官方模型 - self.swing_gain = 0.7 # 降低摆动增益 - self.stance_gain = 0.7 # 降低支撑增益 - self.forward_speed = 0.25 # 降低前进速度 - self.heading_kp = 80.0 # 降低航向比例增益 - self.balance_kp = 100.0 # 降低平衡比例增益 - self.balance_kd = 15.0 # 降低平衡微分增益 - self.torso_pitch_target = 0.0 + # Robot control parameters (稳定性优化) + self.gait_period = 3.0 # 延长步态周期,更慢更稳 + self.swing_gain = 0.3 # 大幅降低摆动增益 + self.stance_gain = 0.4 # 降低支撑增益 + self.forward_speed = 0.1 # 极低的前进速度,保证稳定 + self.heading_kp = 40.0 # 大幅降低航向比例增益 + self.balance_kp = 60.0 # 降低平衡比例增益 + self.balance_kd = 30.0 # 提高阻尼系数,增强稳定性 + self.torso_pitch_target = 0.05 # 轻微前倾,利于平衡 self.torso_roll_target = 0.0 + self.max_joint_velocity = 0.5 # 限制关节最大速度 - # 适配官方模型的关节名称映射 - self.joint_name_mapping = { - "abdomen_x": "abdomen_x", - "abdomen_y": "abdomen_y", - "abdomen_z": "abdomen_z", - "hip_x_right": "hip_x_right", - "hip_y_right": "hip_y_right", - "hip_z_right": "hip_z_right", - "knee_right": "knee_right", - "ankle_x_right": "ankle_x_right", - "ankle_y_right": "ankle_y_right", - "hip_x_left": "hip_x_left", - "hip_y_left": "hip_y_left", - "hip_z_left": "hip_z_left", - "knee_left": "knee_left", - "ankle_x_left": "ankle_x_left", - "ankle_y_left": "ankle_y_left", - "shoulder1_right": "shoulder1_right", - "shoulder2_right": "shoulder2_right", - "elbow_right": "elbow_right", - "shoulder1_left": "shoulder1_left", - "shoulder2_left": "shoulder2_left", - "elbow_left": "elbow_left" - } + # 平衡辅助参数 + self.center_of_mass_target = np.array([0.0, 0.0, 0.8]) # 重心目标位置 + self.com_kp = 15.0 # 重心比例增益 + self.foot_height_target = 0.1 # 抬脚高度目标 + self.foot_height_kp = 20.0 # Initialize component IDs self._init_component_ids() # Initialize obstacle position history self._init_obstacle_history() + # 初始稳定姿态 + self._set_initial_pose() def _init_component_ids(self): - """Initialize all mujoco component IDs (joints, actuators, bodies)""" - # Torso ID (core robot body) - 适配官方模型的torso名称 + """Initialize all mujoco component IDs""" + # Torso ID (core robot body) self.torso_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "torso") if self.torso_id == -1: self.torso_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "waist_lower") # Dynamic patrol target IDs for idx, point in enumerate(self.patrol_points): - # Motor (actuator) IDs self.patrol_motor_ids[idx] = { "x": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"patrol{idx + 1}_motor_x"), "y": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"patrol{idx + 1}_motor_y") } - # Joint IDs (slide joints for movement) self.patrol_joint_ids[idx] = { "x": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, f"patrol{idx + 1}_slide_x"), "y": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, f"patrol{idx + 1}_slide_y") } - # Body IDs (target markers) self.patrol_body_ids[idx] = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, point["name"]) # Dynamic obstacle IDs - # Wall2 (sinusoidal motion) self.wall2_joint_ids = { "y": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "wall2_slide_y"), "z": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "wall2_slide_z") @@ -159,11 +139,10 @@ def _init_component_ids(self): "z": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall2_motor_z") } self.wall2_params = { - "y_amp": 2.5, "y_freq": 0.7, "y_phase": random.uniform(0, 2 * np.pi), - "z_amp": 0.4, "z_freq": 0.3, "z_phase": random.uniform(0, 2 * np.pi) + "y_amp": 1.5, "y_freq": 0.3, "y_phase": random.uniform(0, 2 * np.pi), # 降低障碍物运动幅度和频率 + "z_amp": 0.2, "z_freq": 0.2, "z_phase": random.uniform(0, 2 * np.pi) } - # Wall3 (random walk) self.wall3_joint_ids = { "x": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "wall3_slide_x"), "y": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "wall3_slide_y") @@ -173,14 +152,13 @@ def _init_component_ids(self): "y": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall3_motor_y") } self.wall3_params = { - "x_base": 6.0, "x_range": 1.0, "x_speed": 0.4, - "y_base": 0.0, "y_range": 2.0, "y_speed": 0.6, + "x_base": 4.0, "x_range": 0.5, "x_speed": 0.2, # 降低障碍物运动范围和速度 + "y_base": 0.0, "y_range": 1.0, "y_speed": 0.3, "x_dir": random.choice([-1, 1]), "y_dir": random.choice([-1, 1]), - "x_switch": random.uniform(2.0, 4.0), "y_switch": random.uniform(1.5, 3.5) + "x_switch": random.uniform(3.0, 5.0), "y_switch": random.uniform(2.0, 4.0) } self.wall3_last_switch = {"x": 0.0, "y": 0.0} - # Wall4 (circular motion) self.wall4_joint_ids = { "rot": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "wall4_rotate"), "rad": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "wall4_radial") @@ -190,9 +168,9 @@ def _init_component_ids(self): "rad": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall4_motor_rad") } self.wall4_params = { - "rot_speed": 0.5, "rot_dir": random.choice([-1, 1]), - "rad_amp": 0.8, "rad_freq": 0.2, "rad_phase": random.uniform(0, 2 * np.pi), - "rad_base": 1.2 + "rot_speed": 0.2, "rot_dir": random.choice([-1, 1]), # 降低旋转速度 + "rad_amp": 0.4, "rad_freq": 0.1, "rad_phase": random.uniform(0, 2 * np.pi), + "rad_base": 1.0 } # Wall IDs for obstacle detection @@ -210,16 +188,79 @@ def _init_component_ids(self): elif wall_name == "wall4": self.wall_types[wall_name] = "dynamic3" + # 关节名称映射 + self.joint_name_mapping = { + "abdomen_x": "abdomen_x", + "abdomen_y": "abdomen_y", + "abdomen_z": "abdomen_z", + "hip_x_right": "hip_x_right", + "hip_y_right": "hip_y_right", + "hip_z_right": "hip_z_right", + "knee_right": "knee_right", + "ankle_x_right": "ankle_x_right", + "ankle_y_right": "ankle_y_right", + "hip_x_left": "hip_x_left", + "hip_y_left": "hip_y_left", + "hip_z_left": "hip_z_left", + "knee_left": "knee_left", + "ankle_x_left": "ankle_x_left", + "ankle_y_left": "ankle_y_left", + "shoulder1_right": "shoulder1_right", + "shoulder2_right": "shoulder2_right", + "elbow_right": "elbow_right", + "shoulder1_left": "shoulder1_left", + "shoulder2_left": "shoulder2_left", + "elbow_left": "elbow_left" + } + def _init_obstacle_history(self): """Initialize obstacle position history for prediction""" for wall_name in self.wall_names: self.wall_pos_history[wall_name] = deque(maxlen=10) + def _set_initial_pose(self): + """设置初始稳定姿态,防止启动时倒地""" + # 重置所有控制指令 + self.data.ctrl[:] = 0.0 + + # 设置初始关节角度(站立姿态) + joint_positions = { + "abdomen_x": 0.0, + "abdomen_y": 0.0, + "abdomen_z": 0.0, + "hip_x_right": 0.0, + "hip_y_right": -0.2, + "hip_z_right": 0.0, + "knee_right": 0.2, + "ankle_x_right": 0.0, + "ankle_y_right": 0.1, + "hip_x_left": 0.0, + "hip_y_left": -0.2, + "hip_z_left": 0.0, + "knee_left": 0.2, + "ankle_x_left": 0.0, + "ankle_y_left": 0.1, + "shoulder1_right": 0.0, + "shoulder2_right": 0.0, + "elbow_right": -0.1, + "shoulder1_left": 0.0, + "shoulder2_left": 0.0, + "elbow_left": -0.1 + } + + for joint_name, target_pos in joint_positions.items(): + joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name) + if joint_id != -1: + self.data.qpos[joint_id] = target_pos + def _update_dynamic_targets(self, elapsed_time): - """Update dynamic patrol target positions in real-time""" + """Update dynamic patrol target positions (降低更新频率和速度)""" + if elapsed_time < self.stabilization_phase: + return # 初始稳定阶段不更新目标 + current_target = self.patrol_points[self.current_target_idx] - # 1. Real-time sync with target body positions (from XML) + # 1. Real-time sync with target body positions for idx in self.patrol_body_ids: if self.patrol_body_ids[idx] != -1: target_body_pos = self.data.xpos[self.patrol_body_ids[idx]] @@ -230,28 +271,27 @@ def _update_dynamic_targets(self, elapsed_time): if (elapsed_time - self.last_target_update[idx] > point["update_interval"] and not self.avoid_obstacle and not self.return_to_path): - # Generate new random position (within safe range) + # Generate new random position (缩小范围) new_x = random.uniform(self.target_movement_range["x"][0], self.target_movement_range["x"][1]) new_y = random.uniform(self.target_movement_range["y"][0], self.target_movement_range["y"][1]) # Avoid obstacle areas if self.torso_id != -1: torso_pos = self.data.xpos[self.torso_id][:2] - while np.linalg.norm(np.array([new_x, new_y]) - torso_pos) < 3.0: + while np.linalg.norm(np.array([new_x, new_y]) - torso_pos) < 4.0: # 增大安全距离 new_x = random.uniform(self.target_movement_range["x"][0], self.target_movement_range["x"][1]) new_y = random.uniform(self.target_movement_range["y"][0], self.target_movement_range["y"][1]) - # Control target movement via actuators + # Control target movement (降低速度) if (self.patrol_motor_ids[idx]["x"] != -1 and self.patrol_motor_ids[idx]["y"] != -1 and self.patrol_joint_ids[idx]["x"] != -1 and self.patrol_joint_ids[idx]["y"] != -1): - # Smooth movement control current_x = self.data.qpos[self.patrol_joint_ids[idx]["x"]] current_y = self.data.qpos[self.patrol_joint_ids[idx]["y"]] - self.data.ctrl[self.patrol_motor_ids[idx]["x"]] = (new_x - current_x) * self.target_move_speed - self.data.ctrl[self.patrol_motor_ids[idx]["y"]] = (new_y - current_y) * self.target_move_speed + self.data.ctrl[self.patrol_motor_ids[idx]["x"]] = (new_x - current_x) * self.target_move_speed * 0.5 + self.data.ctrl[self.patrol_motor_ids[idx]["y"]] = (new_y - current_y) * self.target_move_speed * 0.5 # Update target position and timestamp self.patrol_points[idx]["pos"] = np.array([new_x, new_y]) @@ -261,7 +301,10 @@ def _update_dynamic_targets(self, elapsed_time): print(f"\n🔄 Target updated: {point['label']} moved to ({new_x:.2f}, {new_y:.2f})") def _control_dynamic_obstacles(self, elapsed_time): - """Control dynamic obstacle movements""" + """Control dynamic obstacle movements (降低运动速度和幅度)""" + if elapsed_time < self.stabilization_phase: + return # 初始稳定阶段不移动障碍物 + # Wall2: Sinusoidal Y+Z motion if all(id != -1 for id in self.wall2_motor_ids.values()): wall2_y_target = self.wall2_params["y_amp"] * np.sin( @@ -269,33 +312,33 @@ def _control_dynamic_obstacles(self, elapsed_time): wall2_z_target = self.wall2_params["z_amp"] * np.sin( self.wall2_params["z_freq"] * elapsed_time + self.wall2_params["z_phase"]) + 0.75 self.data.ctrl[self.wall2_motor_ids["y"]] = (wall2_y_target - self.data.qpos[ - self.wall2_joint_ids["y"]]) * 2.5 + self.wall2_joint_ids["y"]]) * 1.0 # 降低控制增益 self.data.ctrl[self.wall2_motor_ids["z"]] = (wall2_z_target - self.data.qpos[ - self.wall2_joint_ids["z"]]) * 1.8 + self.wall2_joint_ids["z"]]) * 0.8 # Wall3: Random walk X+Y if all(id != -1 for id in self.wall3_motor_ids.values()): if elapsed_time - self.wall3_last_switch["x"] > self.wall3_params["x_switch"]: self.wall3_params["x_dir"] *= -1 - self.wall3_params["x_switch"] = random.uniform(2.0, 4.0) + self.wall3_params["x_switch"] = random.uniform(3.0, 5.0) self.wall3_last_switch["x"] = elapsed_time if elapsed_time - self.wall3_last_switch["y"] > self.wall3_params["y_switch"]: self.wall3_params["y_dir"] *= -1 - self.wall3_params["y_switch"] = random.uniform(1.5, 3.5) + self.wall3_params["y_switch"] = random.uniform(2.0, 4.0) self.wall3_last_switch["y"] = elapsed_time wall3_x_target = self.wall3_params["x_base"] + self.wall3_params["x_dir"] * self.wall3_params["x_speed"] * ( - elapsed_time % 5) + elapsed_time % 8) # 延长周期 wall3_y_target = self.wall3_params["y_base"] + self.wall3_params["y_dir"] * self.wall3_params["y_speed"] * ( - elapsed_time % 4) - wall3_x_target = np.clip(wall3_x_target, 5.0, 7.0) - wall3_y_target = np.clip(wall3_y_target, -2.5, 2.5) + elapsed_time % 6) + wall3_x_target = np.clip(wall3_x_target, 3.5, 4.5) # 缩小范围 + wall3_y_target = np.clip(wall3_y_target, -1.0, 1.0) self.data.ctrl[self.wall3_motor_ids["x"]] = (wall3_x_target - self.data.qpos[ - self.wall3_joint_ids["x"]]) * 2.2 + self.wall3_joint_ids["x"]]) * 1.0 self.data.ctrl[self.wall3_motor_ids["y"]] = (wall3_y_target - self.data.qpos[ - self.wall3_joint_ids["y"]]) * 2.0 + self.wall3_joint_ids["y"]]) * 0.8 # Wall4: Circular motion if all(id != -1 for id in self.wall4_motor_ids.values()): @@ -304,19 +347,19 @@ def _control_dynamic_obstacles(self, elapsed_time): self.wall4_params["rad_freq"] * elapsed_time + self.wall4_params["rad_phase"]) self.data.ctrl[self.wall4_motor_ids["rot"]] = (wall4_rot_target - self.data.qpos[ - self.wall4_joint_ids["rot"]]) * 1.5 + self.wall4_joint_ids["rot"]]) * 0.8 self.data.ctrl[self.wall4_motor_ids["rad"]] = (wall4_rad_target - self.data.qpos[ - self.wall4_joint_ids["rad"]]) * 2.0 + self.wall4_joint_ids["rad"]]) * 0.8 def _detect_obstacles(self, elapsed_time): - """Detect obstacles and determine avoidance strategy""" - if not self.wall_ids or self.torso_id == -1 or self.patrol_completed: + """Detect obstacles and determine avoidance strategy (提前避障)""" + if elapsed_time < self.stabilization_phase or not self.wall_ids or self.torso_id == -1 or self.patrol_completed: return torso_pos = self.data.xpos[self.torso_id][:2] wall_distances = [] - # Calculate obstacle distances (with prediction for dynamic obstacles) + # Calculate obstacle distances (with prediction) for idx, wall_id in enumerate(self.wall_ids): wall_name = self.wall_names[idx] wall_type = self.wall_types.get(wall_name, "fixed") @@ -325,15 +368,15 @@ def _detect_obstacles(self, elapsed_time): self.wall_pos_history[wall_name].append(wall_pos) current_distance = np.linalg.norm(torso_pos - wall_pos) - # Predict future position for dynamic obstacles + # Predict future position predicted_distance = current_distance if len(self.wall_pos_history[wall_name]) > 5 and wall_type != "fixed": pos_history = np.array(self.wall_pos_history[wall_name]) velocity = (pos_history[-1] - pos_history[0]) / len(pos_history) * self.model.opt.timestep * 10 - future_pos = wall_pos + velocity * 0.8 + future_pos = wall_pos + velocity * 1.0 # 延长预测时间 predicted_distance = np.linalg.norm(torso_pos - future_pos) - # Weight distance by obstacle priority (dynamic > fixed) + # Weight distance weighted_distance = predicted_distance / self.wall_priority[wall_type] wall_distances.append({ "name": wall_name, @@ -345,7 +388,7 @@ def _detect_obstacles(self, elapsed_time): "weighted_dist": weighted_distance }) - # Find closest obstacle (priority-based) + # Find closest obstacle if wall_distances: wall_distances.sort(key=lambda x: x["weighted_dist"]) closest_wall = wall_distances[0] @@ -357,13 +400,13 @@ def _detect_obstacles(self, elapsed_time): "pos": closest_wall["pos"] } - # Trigger obstacle avoidance + # Trigger obstacle avoidance (更早触发) if (closest_wall["predicted_dist"] < self.obstacle_distance_threshold and not self.avoid_obstacle and not self.return_to_path): self.avoid_obstacle = True self.obstacle_avoidance_start = elapsed_time - # Determine turn direction (away from obstacle) + # Determine turn direction wall_relative = closest_wall["pos"] - torso_pos target_vector = self.patrol_points[self.current_target_idx]["pos"] - torso_pos cross_product = np.cross(np.append(wall_relative, 0), np.append(target_vector, 0))[2] @@ -374,7 +417,7 @@ def _detect_obstacles(self, elapsed_time): print( f"\n⚠️ Obstacle detected: {closest_wall['name']} (distance: {closest_wall['predicted_dist']:.2f}m) - Turning {self.turn_dir_label}") - # Complete avoidance: return to path + # Complete avoidance if self.avoid_obstacle and ( elapsed_time - self.obstacle_avoidance_start) > self.obstacle_avoidance_duration: self.avoid_obstacle = False @@ -389,12 +432,50 @@ def _detect_obstacles(self, elapsed_time): f"✅ Back to patrol path - tracking target: {self.patrol_points[self.current_target_idx]['label']}") def _get_joint_id(self, joint_name): - """获取关节ID(适配名称映射)""" + """获取关节ID""" mapped_name = self.joint_name_mapping.get(joint_name, joint_name) return mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, mapped_name) + def _get_joint_vel_id(self, joint_name): + """获取关节速度ID""" + mapped_name = self.joint_name_mapping.get(joint_name, joint_name) + return mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, mapped_name) + + def _limit_joint_velocity(self, joint_name, cmd): + """限制关节速度,防止过快运动""" + vel_id = self._get_joint_vel_id(joint_name) + if vel_id == -1: + return cmd + + current_vel = self.data.qvel[vel_id] + # 速度限制 + if abs(current_vel) > self.max_joint_velocity: + cmd = cmd * (self.max_joint_velocity / abs(current_vel)) * 0.5 + + return cmd + + def _compute_center_of_mass(self): + """计算机器人重心位置""" + if self.torso_id == -1: + return np.array([0.0, 0.0, 0.8]) + + # 简化版重心计算(基于躯干位置+腿部姿态) + torso_pos = self.data.xpos[self.torso_id] + + # 获取左右脚位置 + foot_right_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "foot_right") + foot_left_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "foot_left") + + foot_right_pos = self.data.xpos[foot_right_id] if foot_right_id != -1 else torso_pos + foot_left_pos = self.data.xpos[foot_left_id] if foot_left_id != -1 else torso_pos + + # 重心 = (躯干位置*0.7 + 双脚平均位置*0.3) + com = torso_pos * 0.7 + (foot_right_pos + foot_left_pos) * 0.15 + + return com + def _control_robot_gait(self, elapsed_time): - """Control robot gait and movement (适配官方Humanoid模型)""" + """Control robot gait with enhanced balance (核心稳定控制)""" if self.torso_id == -1 or self.patrol_completed: return @@ -428,94 +509,85 @@ def _control_robot_gait(self, elapsed_time): self.last_target_switch_time = time.time() print(f"🔄 Restarting patrol - tracking start point") - # Calculate heading error + # Calculate heading error (优化四元数计算) torso_quat = self.data.xquat[self.torso_id] - # 适配官方模型的四元数计算 - robot_yaw = np.arctan2(2 * (torso_quat[0] * torso_quat[1] + torso_quat[2] * torso_quat[3]), - 1 - 2 * (torso_quat[1]**2 + torso_quat[2]**2)) + # 正确的yaw计算 + siny_cosp = 2 * (torso_quat[3] * torso_quat[2] + torso_quat[0] * torso_quat[1]) + cosy_cosp = 1 - 2 * (torso_quat[1] ** 2 + torso_quat[2] ** 2) + robot_yaw = np.arctan2(siny_cosp, cosy_cosp) + target_yaw = np.arctan2(target_vector[1], target_vector[0]) yaw_error = target_yaw - robot_yaw - yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error)) + yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error)) # 归一化到[-pi, pi] - # Reset control commands (保留障碍物和巡逻点的控制) - robot_ctrl_size = self.model.nu - len(self.patrol_motor_ids) * 2 - len(self.wall2_motor_ids) - len(self.wall3_motor_ids) - len(self.wall4_motor_ids) - if robot_ctrl_size > 0: - self.data.ctrl[:robot_ctrl_size] = 0.0 - else: - self.data.ctrl[:] = 0.0 + # Reset control commands + self.data.ctrl[:self.model.nu] = 0.0 + + # -------------------------- 初始稳定阶段 -------------------------- + if elapsed_time < self.stabilization_phase: + # 仅保持站立平衡,不移动 + self._maintain_balance(elapsed_time) + return # -------------------------- Movement Control -------------------------- cycle = elapsed_time % self.gait_period phase = cycle / self.gait_period if self.return_to_path: - # Return to path mode + # Return to path mode (极慢的返回速度) return_phase = (elapsed_time - self.return_to_path_start) / self.return_to_path_duration - return_speed = 1.5 * np.cos(return_phase * np.pi) + return_speed = 0.8 * np.cos(return_phase * np.pi) # 降低返回速度 - # Heading control + # Heading control (极低增益) abdomen_z_id = self._get_joint_id("abdomen_z") hip_z_right_id = self._get_joint_id("hip_z_right") hip_z_left_id = self._get_joint_id("hip_z_left") if 0 <= abdomen_z_id < self.model.nu: - self.data.ctrl[abdomen_z_id] = self.heading_kp * yaw_error * return_speed * 0.08 + cmd = self.heading_kp * yaw_error * return_speed * 0.05 + self.data.ctrl[abdomen_z_id] = self._limit_joint_velocity("abdomen_z", cmd) if 0 <= hip_z_right_id < self.model.nu: - self.data.ctrl[hip_z_right_id] = -yaw_error * return_speed * 0.6 + cmd = -yaw_error * return_speed * 0.3 + self.data.ctrl[hip_z_right_id] = self._limit_joint_velocity("hip_z_right", cmd) if 0 <= hip_z_left_id < self.model.nu: - self.data.ctrl[hip_z_left_id] = yaw_error * return_speed * 0.6 + cmd = yaw_error * return_speed * 0.3 + self.data.ctrl[hip_z_left_id] = self._limit_joint_velocity("hip_z_left", cmd) - # Stabilize stance (适配官方模型的关节控制) - for side in ["right", "left"]: - hip_y_id = self._get_joint_id(f"hip_y_{side}") - knee_id = self._get_joint_id(f"knee_{side}") - ankle_y_id = self._get_joint_id(f"ankle_y_{side}") - - if 0 <= hip_y_id < self.model.nu: - self.data.ctrl[hip_y_id] = -0.6 # 降低力度适配官方模型 - if 0 <= knee_id < self.model.nu: - self.data.ctrl[knee_id] = 0.9 # 降低力度适配官方模型 - if 0 <= ankle_y_id < self.model.nu: - self.data.ctrl[ankle_y_id] = 0.3 # 降低力度适配官方模型 + # 增强平衡 + self._maintain_balance(elapsed_time) elif self.avoid_obstacle: - # Obstacle avoidance mode + # Obstacle avoidance mode (极慢的转向速度) avoid_phase = (elapsed_time - self.obstacle_avoidance_start) / self.obstacle_avoidance_duration - turn_speed = 1.4 * np.sin(avoid_phase * np.pi) # 降低转向速度 + turn_speed = 0.8 * np.sin(avoid_phase * np.pi) # 降低转向速度 - # Turn control + # Turn control (极低力度) hip_z_right_id = self._get_joint_id("hip_z_right") hip_z_left_id = self._get_joint_id("hip_z_left") abdomen_z_id = self._get_joint_id("abdomen_z") if 0 <= hip_z_right_id < self.model.nu: - self.data.ctrl[hip_z_right_id] = self.turn_direction * turn_speed * 1.0 # 降低力度 + cmd = self.turn_direction * turn_speed * 0.5 + self.data.ctrl[hip_z_right_id] = self._limit_joint_velocity("hip_z_right", cmd) if 0 <= hip_z_left_id < self.model.nu: - self.data.ctrl[hip_z_left_id] = -self.turn_direction * turn_speed * 1.0 # 降低力度 + cmd = -self.turn_direction * turn_speed * 0.5 + self.data.ctrl[hip_z_left_id] = self._limit_joint_velocity("hip_z_left", cmd) if 0 <= abdomen_z_id < self.model.nu: - self.data.ctrl[abdomen_z_id] = self.turn_direction * turn_speed * 1.5 # 降低力度 - - # Enhanced balance (适配官方模型) - for side in ["right", "left"]: - hip_y_id = self._get_joint_id(f"hip_y_{side}") - knee_id = self._get_joint_id(f"knee_{side}") - ankle_x_id = self._get_joint_id(f"ankle_x_{side}") + cmd = self.turn_direction * turn_speed * 0.8 + self.data.ctrl[abdomen_z_id] = self._limit_joint_velocity("abdomen_z", cmd) - if 0 <= hip_y_id < self.model.nu: - self.data.ctrl[hip_y_id] = -0.9 # 降低力度 - if 0 <= knee_id < self.model.nu: - self.data.ctrl[knee_id] = 1.1 # 降低力度 - if 0 <= ankle_x_id < self.model.nu: - self.data.ctrl[ankle_x_id] = 0.2 # 降低力度 + # 增强平衡 + self._maintain_balance(elapsed_time) else: - # Normal patrol mode - # Heading control + # Normal patrol mode (核心稳定控制) + # Heading control (极低增益) abdomen_z_id = self._get_joint_id("abdomen_z") if 0 <= abdomen_z_id < self.model.nu: - self.data.ctrl[abdomen_z_id] = self.heading_kp * yaw_error * 0.1 # 降低增益 + cmd = self.heading_kp * yaw_error * 0.05 + self.data.ctrl[abdomen_z_id] = self._limit_joint_velocity("abdomen_z", cmd) - # Leg gait control (适配官方模型的步态参数) + # Leg gait control (极慢的步态) for side, sign in [("right", 1), ("left", -1)]: swing_phase = (phase + 0.5 * sign) % 1.0 @@ -527,59 +599,106 @@ def _control_robot_gait(self, elapsed_time): ankle_y_id = self._get_joint_id(f"ankle_y_{side}") ankle_x_id = self._get_joint_id(f"ankle_x_{side}") - # Gait commands (适配官方模型调整增益) + # 极低的步态指令 if 0 <= hip_x_id < self.model.nu: - self.data.ctrl[hip_x_id] = self.swing_gain * np.sin(2 * np.pi * swing_phase) * self.forward_speed * 0.8 + cmd = self.swing_gain * np.sin(2 * np.pi * swing_phase) * self.forward_speed * 0.5 + self.data.ctrl[hip_x_id] = self._limit_joint_velocity(f"hip_x_{side}", cmd) if 0 <= hip_z_id < self.model.nu: - self.data.ctrl[hip_z_id] = self.stance_gain * np.cos( - 2 * np.pi * swing_phase) * 0.15 + yaw_error * 0.1 + cmd = self.stance_gain * np.cos(2 * np.pi * swing_phase) * 0.08 + yaw_error * 0.05 + self.data.ctrl[hip_z_id] = self._limit_joint_velocity(f"hip_z_{side}", cmd) if 0 <= hip_y_id < self.model.nu: - self.data.ctrl[hip_y_id] = -0.8 * np.sin(2 * np.pi * swing_phase) - 0.4 # 降低力度 + cmd = -0.4 * np.sin(2 * np.pi * swing_phase) - 0.2 # 大幅降低力度 + self.data.ctrl[hip_y_id] = self._limit_joint_velocity(f"hip_y_{side}", cmd) if 0 <= knee_id < self.model.nu: - self.data.ctrl[knee_id] = 1.1 * np.sin(2 * np.pi * swing_phase) + 0.7 # 降低力度 + cmd = 0.6 * np.sin(2 * np.pi * swing_phase) + 0.4 # 大幅降低力度 + self.data.ctrl[knee_id] = self._limit_joint_velocity(f"knee_{side}", cmd) if 0 <= ankle_y_id < self.model.nu: - self.data.ctrl[ankle_y_id] = 0.3 * np.cos(2 * np.pi * swing_phase) # 降低力度 + cmd = 0.15 * np.cos(2 * np.pi * swing_phase) # 大幅降低力度 + self.data.ctrl[ankle_y_id] = self._limit_joint_velocity(f"ankle_y_{side}", cmd) if 0 <= ankle_x_id < self.model.nu: - self.data.ctrl[ankle_x_id] = 0.15 * np.sin(2 * np.pi * swing_phase) # 降低力度 - - # Torso balance control (适配官方模型) - abdomen_x_id = self._get_joint_id("abdomen_x") - abdomen_y_id = self._get_joint_id("abdomen_y") - - # 适配官方模型的姿态计算 - pitch = 2 * (torso_quat[1] * torso_quat[3] - torso_quat[0] * torso_quat[2]) - roll = 2 * (torso_quat[0] * torso_quat[1] + torso_quat[2] * torso_quat[3]) - - # 获取关节速度ID - abdomen_x_joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "abdomen_x") - abdomen_y_joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "abdomen_y") + cmd = 0.08 * np.sin(2 * np.pi * swing_phase) # 大幅降低力度 + self.data.ctrl[ankle_x_id] = self._limit_joint_velocity(f"ankle_x_{side}", cmd) - if 0 <= abdomen_x_id < self.model.nu and abdomen_x_joint_id != -1: - self.data.ctrl[abdomen_x_id] = self.balance_kp * (self.torso_roll_target - roll) * 0.08 - self.balance_kd * \ - self.data.qvel[abdomen_x_joint_id] * 0.1 - if 0 <= abdomen_y_id < self.model.nu and abdomen_y_joint_id != -1: - self.data.ctrl[abdomen_y_id] = self.balance_kp * (self.torso_pitch_target - pitch) * 0.08 - self.balance_kd * \ - self.data.qvel[abdomen_y_joint_id] * 0.1 + # 核心平衡控制 + self._maintain_balance(elapsed_time) - # Arm swing for balance (适配官方模型降低力度) + # Arm swing for balance (极轻微的摆臂) for side, sign in [("right", 1), ("left", -1)]: shoulder1_id = self._get_joint_id(f"shoulder1_{side}") shoulder2_id = self._get_joint_id(f"shoulder2_{side}") elbow_id = self._get_joint_id(f"elbow_{side}") - shoulder1_cmd = 0.18 * np.sin(2 * np.pi * (phase + 0.5 * sign)) # 降低力度 - shoulder2_cmd = 0.14 * np.cos(2 * np.pi * (phase + 0.5 * sign)) # 降低力度 - elbow_cmd = -0.35 * np.sin(2 * np.pi * (phase + 0.5 * sign)) - 0.22 # 降低力度 + shoulder1_cmd = 0.08 * np.sin(2 * np.pi * (phase + 0.5 * sign)) # 大幅降低 + shoulder2_cmd = 0.06 * np.cos(2 * np.pi * (phase + 0.5 * sign)) # 大幅降低 + elbow_cmd = -0.15 * np.sin(2 * np.pi * (phase + 0.5 * sign)) - 0.1 # 大幅降低 if 0 <= shoulder1_id < self.model.nu: - self.data.ctrl[shoulder1_id] = shoulder1_cmd + self.data.ctrl[shoulder1_id] = self._limit_joint_velocity(f"shoulder1_{side}", shoulder1_cmd) if 0 <= shoulder2_id < self.model.nu: - self.data.ctrl[shoulder2_id] = shoulder2_cmd + self.data.ctrl[shoulder2_id] = self._limit_joint_velocity(f"shoulder2_{side}", shoulder2_cmd) if 0 <= elbow_id < self.model.nu: - self.data.ctrl[elbow_id] = elbow_cmd + self.data.ctrl[elbow_id] = self._limit_joint_velocity(f"elbow_{side}", elbow_cmd) + + def _maintain_balance(self, elapsed_time): + """增强型平衡控制(核心稳定函数)""" + # 1. 躯干姿态控制 (PD控制) + abdomen_x_id = self._get_joint_id("abdomen_x") + abdomen_y_id = self._get_joint_id("abdomen_y") + torso_quat = self.data.xquat[self.torso_id] + + # 计算roll/pitch + roll = 2 * (torso_quat[0] * torso_quat[1] + torso_quat[2] * torso_quat[3]) + pitch = 2 * (torso_quat[1] * torso_quat[3] - torso_quat[0] * torso_quat[2]) + + # 获取关节速度 + abdomen_x_vel_id = self._get_joint_vel_id("abdomen_x") + abdomen_y_vel_id = self._get_joint_vel_id("abdomen_y") + + # PD控制 + if 0 <= abdomen_x_id < self.model.nu and abdomen_x_vel_id != -1: + roll_error = self.torso_roll_target - roll + roll_vel = self.data.qvel[abdomen_x_vel_id] + cmd = self.balance_kp * roll_error * 0.05 - self.balance_kd * roll_vel * 0.1 + self.data.ctrl[abdomen_x_id] = np.clip(cmd, -0.5, 0.5) # 限制输出 + + if 0 <= abdomen_y_id < self.model.nu and abdomen_y_vel_id != -1: + pitch_error = self.torso_pitch_target - pitch + pitch_vel = self.data.qvel[abdomen_y_vel_id] + cmd = self.balance_kp * pitch_error * 0.05 - self.balance_kd * pitch_vel * 0.1 + self.data.ctrl[abdomen_y_id] = np.clip(cmd, -0.5, 0.5) # 限制输出 + + # 2. 重心控制 + com = self._compute_center_of_mass() + com_error = self.center_of_mass_target - com + + # 通过躯干微调调整重心 + if 0 <= abdomen_x_id < self.model.nu: + current_cmd = self.data.ctrl[abdomen_x_id] + self.data.ctrl[abdomen_x_id] = current_cmd + com_error[1] * self.com_kp * 0.01 + + if 0 <= abdomen_y_id < self.model.nu: + current_cmd = self.data.ctrl[abdomen_y_id] + self.data.ctrl[abdomen_y_id] = current_cmd + com_error[0] * self.com_kp * 0.01 + + # 3. 腿部稳定控制 + for side in ["right", "left"]: + hip_y_id = self._get_joint_id(f"hip_y_{side}") + knee_id = self._get_joint_id(f"knee_{side}") + ankle_y_id = self._get_joint_id(f"ankle_y_{side}") + + # 基础支撑力 + if 0 <= hip_y_id < self.model.nu: + current_cmd = self.data.ctrl[hip_y_id] + self.data.ctrl[hip_y_id] = current_cmd - 0.1 # 轻微弯曲,增强稳定 + if 0 <= knee_id < self.model.nu: + current_cmd = self.data.ctrl[knee_id] + self.data.ctrl[knee_id] = current_cmd + 0.1 # 轻微弯曲,增强稳定 + if 0 <= ankle_y_id < self.model.nu: + current_cmd = self.data.ctrl[ankle_y_id] + self.data.ctrl[ankle_y_id] = current_cmd + 0.05 # 轻微调整脚踝 def _print_status(self, elapsed_time): - """Print robot status (reduced frequency to avoid clutter)""" + """Print robot status""" if (elapsed_time - self.last_print_time) < 2.0 or self.torso_id == -1: return @@ -589,7 +708,10 @@ def _print_status(self, elapsed_time): distance_to_target = np.linalg.norm(current_target["pos"] - torso_pos[:2]) # Status text - if self.patrol_completed: + if elapsed_time < self.stabilization_phase: + status = "Stabilizing (initial phase)" + nav_info = "Building balance..." + elif self.patrol_completed: status = f"Patrol completed! Cycles: {self.patrol_cycles}" nav_info = "Waiting to restart" elif self.avoid_obstacle: @@ -604,18 +726,22 @@ def _print_status(self, elapsed_time): # Obstacle info obstacle_info = f"{self.closest_wall_info['name']}: {self.closest_wall_info['distance']:.2f}m" if \ - self.closest_wall_info["name"] else "None" + self.closest_wall_info["name"] else "None" + + # 重心信息 + com = self._compute_center_of_mass() + com_info = f"COM: z={com[2]:.2f}m" # Print status line print( - f"\r🕒 {elapsed_time:.1f}s | 📍 x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f} | 🗺️ {nav_info} | 🛡️ {obstacle_info} | 📊 {status}", + f"\r🕒 {elapsed_time:.1f}s | 📍 x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f} | {com_info} | 🗺️ {nav_info} | 🛡️ {obstacle_info} | 📊 {status}", end="" ) def run_simulation(self): """Main simulation loop""" - print("🤖 DeepMind Humanoid Simulation Started (适配官方模型)") - print("📌 Features: Dynamic Obstacle Avoidance + Moving Target Tracking") + print("🤖 DeepMind Humanoid Simulation Started (Stable Version)") + print("📌 Features: Enhanced Balance + Slow & Stable Movement") print("🔍 Press Ctrl+C to stop simulation\n") with viewer.launch_passive(self.model, self.data) as viewer_instance: @@ -626,17 +752,16 @@ def run_simulation(self): elapsed_time = time.time() - self.sim_start_time # Core control sequence - self._control_dynamic_obstacles(elapsed_time) # Step 1: Move obstacles - self._update_dynamic_targets(elapsed_time) # Step 2: Update targets - self._detect_obstacles(elapsed_time) # Step 3: Detect obstacles - self._control_robot_gait(elapsed_time) # Step 4: Control robot - self._print_status(elapsed_time) # Step 5: Print status + self._control_dynamic_obstacles(elapsed_time) + self._update_dynamic_targets(elapsed_time) + self._detect_obstacles(elapsed_time) + self._control_robot_gait(elapsed_time) + self._print_status(elapsed_time) # Step simulation mujoco.mj_step(self.model, self.data) viewer_instance.sync() - # 适配官方模型降低仿真步长延迟 - time.sleep(self.model.opt.timestep * 1.5) + time.sleep(self.model.opt.timestep * 3) # 更慢的仿真步长 except KeyboardInterrupt: print("\n\n🛑 Simulation interrupted by user") @@ -648,18 +773,20 @@ def run_simulation(self): if self.torso_id != -1: elapsed_time = time.time() - self.sim_start_time torso_pos = self.data.xpos[self.torso_id] + com = self._compute_center_of_mass() print(f"\n\n📋 Simulation Summary:") print(f" Total runtime: {elapsed_time:.1f} seconds") print(f" Patrol cycles completed: {self.patrol_cycles}") print(f" Final position: x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f}") + print(f" Final COM height: {com[2]:.2f}m (stable if > 0.7m)") print("🤖 Simulation ended successfully") if __name__ == "__main__": - # Set default model path (relative path for GitHub compatibility) + # Set default model path model_file = "Robot_move_straight.xml" - # Check command line argument for model path + # Check command line argument if len(sys.argv) > 1: model_file = sys.argv[1] @@ -671,10 +798,11 @@ def run_simulation(self): # Initialize and run controller try: - controller = DynamicPatrolController(model_file) + controller = StablePatrolController(model_file) controller.run_simulation() except Exception as e: print(f"\n❌ Failed to start simulation: {e}") import traceback + traceback.print_exc() sys.exit(1) \ No newline at end of file