diff --git a/experiments/experiments/lift_experiment.yaml b/experiments/experiments/lift_experiment.yaml new file mode 100644 index 0000000..92f211d --- /dev/null +++ b/experiments/experiments/lift_experiment.yaml @@ -0,0 +1,365 @@ +# lift_experiment.yaml -- e-MDB policy-learning experiment for emdb_simulator's +# KitchenLift task (RoboCasa/robosuite UR5e), driven through mujoco_emdb_sim's +# sim_bridge. +# +# Modeled on the working ur5_grasp_experiment.yaml: same autonomous-learning + +# SAC machinery (WorldModelLearned + novelty exploration + effectance + +# model-creation + QUtilityModel, with PolicyLearned as the Policy connector so +# discovered goals are learned by SAC). Adapted for the lift task: +# - perceptions: obj (x,y,z) + obj_grasped, via mujoco_emdb_sim.perception +# - actuation: 7-DOF EE deltas [dx,dy,dz,droll,dpitch,dyaw,grasp] +# - control services hosted by sim_bridge (-> scene_loader /step_action,/reset) +# - mission reward: /emdb/simulator/sensor/progress (sparse lift success) +# +# NOTE: the mdb perception set exposes the OBJECT but not the arm/EE pose, so the +# policy can't observe arm->object distance directly -- learning to reach may be +# limited until an EE perception is added on the sim side. Bounds are untuned. +# +# scene_loader now streams /emdb/simulator/sensor/* continuously in rl mode via +# a heartbeat timer (no more one-shot-per-step perceptions), so no relay node +# is needed between the sim and the main loop. +# +# RUN (two shells, shared ROS_DOMAIN_ID=0, both sourcing this same workspace's +# install/setup.bash -- sim and architecture packages live side by side here): +# A) sim: ros2 launch emdb_simulator emdb_simulator.launch.py \ +# teleop:=false perception_mode:=mdb +# B) arch: ros2 launch experiments lift_launch.py + +Experiment: + name: main_loop + class_name: cognitive_processes.main_loop.MainLoopLight + new_executor: True + threads: 4 + parameters: + iterations: 2000 + trials: 5 + softmax_selection: False + softmax_temperature: 0.3 + kill_on_finish: True + +Control: + id: ltm_emdb_simulator + control_topic: /main_loop/control + control_msg: cognitive_processes_interfaces.msg.ControlMsg + episodes_topic: /main_loop/episodes + episodes_msg: core_interfaces.msg.Container + executed_action_service: /emdb/simulator/executed_action + executed_action_msg: cognitive_node_interfaces.srv.Action + world_reset_service: /emdb/simulator/world_reset + world_reset_msg: cognitive_processes_interfaces.srv.WorldReset + +LTM: + Globals: + actuation_config: + arm: ["dx", "dy", "dz", "droll", "dpitch", "dyaw", "grasp"] + + # Absolute paths so the logs land in /home/fabian/Documents/TFM/ros_ws/results (bind-mounted to the + # host ./results) no matter which directory the launch is started from. + # Each run appends a _N suffix (goodness_0.txt, goodness_1.txt, ...). + Files: + - + id: goodness + class: core.file.FileGoodness + file: /home/fabian/Documents/TFM/ros_ws/results/goodness.txt + - + id: pnodes_content + class: core.file.FilePNodesContent + file: /home/fabian/Documents/TFM/ros_ws/results/pnodes_content.txt + parameters: + save_interval: 0 + - + id: trials + class: core.file.FileTrialsSuccess + file: /home/fabian/Documents/TFM/ros_ws/results/trials.txt + - + id: dataset + class: core.file.FileEpisodesDataset + file: /home/fabian/Documents/TFM/ros_ws/results/dataset.csv + - + id: world_model_success + class: core.file.FileWorldModelSuccess + file: /home/fabian/Documents/TFM/ros_ws/results/world_model_success.txt + - + id: save_models + class: core.file.FileSaveModels + file: /home/fabian/Documents/TFM/ros_ws/results/models_save + parameters: + save_interval: 0 + + Connectors: + - + data: Space + default_class: cognitive_nodes.space.ANNSpace + - + data: Perception + default_class: cognitive_nodes.perception.Perception + - + data: PNode + default_class: cognitive_nodes.pnode.PNode + parameters: + space_class: cognitive_nodes.space.ANNSpace + history_size: 500 + - + data: CNode + default_class: cognitive_nodes.cnode.CNode + - + data: Goal + default_class: cognitive_nodes.goal.GoalMotiven + parameters: + space_class: cognitive_nodes.space.ANNSpace + history_size: 300 + min_confidence: 0.94 + ltm_id: ltm_0 + - + data: WorldModel + default_class: cognitive_nodes.world_model.WorldModelLearned + parameters: + episodes_msg: core_interfaces.msg.Container + episodes_topic: /main_loop/episodes + prediction_srv_type: cognitive_node_interfaces.srv.Predict + main_size: 2000 + train_sample: 200 + train_split: 0.8 + validation_split: 0.1 + secondary_size: 50 + retrain: True + - + data: UtilityModel + default_class: cognitive_nodes.utility_model.QUtilityModel + parameters: + max_iterations: 50 + candidate_actions: 100 + ltm_id: ltm_0 + candidate_generation: "latin" + softmax_selection: True + softmax_temperature: 0.01 + trace_length: 20 + min_traces: 20 + max_traces: 200 + train_traces: 10 + train_every: 5 + replace_every: 5 + discount_factor: 0.9 + reward_factor: 10.0 + max_antitraces: 5 + evaluation_method: "exponential" + epochs: 100 + learning_rate: 0.001 + output_activation: "linear" + hidden_layers: [256, 128] + - + data: Policy + default_class: cognitive_nodes.policy.PolicyLearned + parameters: + obs_dim: 4 # obj (x,y,z = 3) + obj_grasped (1) + buffer_size: 100000 + train_every: 5 + gradient_steps: 100 + batch_size: 100 + min_traces: 20 + max_steps: 50 + ltm_id: ltm_0 + learning_rate: 0.001 + + Nodes: + Perception: + - + name: obj + class_name: mujoco_emdb_sim.perception.EmdbSimulatorPerception + parameters: + default_msg: emdb_interfaces.msg.ObjectStateArray + default_topic: /emdb/simulator/sensor/obj + normalize_data: + x_min: -2.0 + x_max: 2.0 + y_min: -2.0 + y_max: 2.0 + z_min: 0.0 + z_max: 2.0 + - + name: obj_grasped + class_name: mujoco_emdb_sim.perception.EmdbSimulatorPerception + parameters: + default_msg: std_msgs.msg.Bool + default_topic: /emdb/simulator/sensor/obj/grasped + + RobotPurpose: + - + name: lift_object_need + class_name: cognitive_nodes.robot_purpose.RobotPurpose + parameters: + weight: 1.0 + drive_id: 'lift_object_drive' + purpose_type: 'Mission' + terminal: True + - + name: novelty_need + class_name: cognitive_nodes.robot_purpose.RobotPurpose + parameters: + weight: 0.1 + drive_id: 'novelty_drive' + purpose_type: 'Need' + - + name: effectance_need + class_name: cognitive_nodes.robot_purpose.RobotPurpose + parameters: + weight: 0.25 + drive_id: 'effectance_drive' + purpose_type: 'Need' + - + name: model_creation_need + class_name: cognitive_nodes.robot_purpose.RobotPurpose + parameters: + weight: 1.0 + drive_id: 'model_creation_drive' + purpose_type: 'Need' + + Drive: + - + # Sparse lift-success signal (float(_check_success())), exposed as + # a perception per e-MDB's "reward is just another perception". + name: lift_object_drive + class_name: cognitive_nodes.drive.DriveExponential + parameters: + input_topic: /emdb/simulator/sensor/progress + input_msg: std_msgs.msg.Float32 + min_eval: 0.8 + neighbors: [{"name": "lift_object_need", "node_type": "RobotPurpose"}] + - + name: novelty_drive + class_name: cognitive_nodes.novelty.DriveNovelty + parameters: + neighbors: [{"name": "novelty_need", "node_type": "RobotPurpose"}] + - + name: effectance_drive + class_name: cognitive_nodes.effectance.DriveEffectanceInternal + parameters: + ltm_id: ltm_0 + min_confidence: 0.84 + limit_depth: False + neighbors: [{"name": "effectance_need", "node_type": "RobotPurpose"}] + - + name: model_creation_drive + class_name: cognitive_nodes.model_creation.ModelCreationDrive + parameters: + neighbors: [{"name": "model_creation_need", "node_type": "RobotPurpose"}] + LTM_id: ltm_0 + max_iterations: 20 + episodes_topic: /main_loop/episodes + episodes_msg: core_interfaces.msg.Container + model_creation_policy: model_creation_policy + + Goal: + - + name: novelty_goal + class_name: dummy_nodes.dummy_goal.GoalDummy + parameters: + neighbors: [{"name": "novelty_drive", "node_type": "Drive"}] + - + name: effectance_goal + class_name: dummy_nodes.dummy_goal.GoalDummy + parameters: + neighbors: [{"name": "effectance_drive", "node_type": "Drive"}] + - + name: model_creation_goal + class_name: dummy_nodes.dummy_goal.GoalDummy + parameters: + neighbors: [{"name": "model_creation_drive", "node_type": "Drive"}] + + PNode: + - + name: novelty_pnode + class_name: dummy_nodes.dummy_pnodes.ActivatedDummyPNode + parameters: + space_class: cognitive_nodes.space.ActivatedDummySpace + - + name: effectance_pnode + class_name: dummy_nodes.dummy_pnodes.ActivatedDummyPNode + parameters: + space_class: cognitive_nodes.space.ActivatedDummySpace + - + name: model_creation_pnode + class_name: dummy_nodes.dummy_pnodes.ActivatedDummyPNode + parameters: + space_class: cognitive_nodes.space.ActivatedDummySpace + + CNode: + - + name: novelty_cnode + class_name: cognitive_nodes.cnode.CNode + parameters: + neighbors: [{"name": "novelty_goal", "node_type": "Goal"}, {"name": "novelty_pnode", "node_type": "PNode"}] + - + name: model_creation_cnode + class_name: cognitive_nodes.cnode.CNode + parameters: + neighbors: [{"name": "model_creation_goal", "node_type": "Goal"}, {"name": "model_creation_pnode", "node_type": "PNode"}] + - + name: effectance_cnode + class_name: cognitive_nodes.cnode.CNode + parameters: + neighbors: [{"name": "effectance_goal", "node_type": "Goal"}, {"name": "effectance_pnode", "node_type": "PNode"}] + + Policy: + - + name: model_creation_policy + class_name: cognitive_nodes.model_creation.ModelCreationPolicy + parameters: + neighbors: [{"name": "model_creation_cnode", "node_type": "CNode"}] + LTM_id: ltm_0 + max_iterations: 20 + episodes_topic: /main_loop/episodes + episodes_msg: core_interfaces.msg.Container + - + name: effectance_policy + class_name: cognitive_nodes.effectance.PolicyEffectanceInternal + parameters: + goal_class: cognitive_nodes.effectance.GoalActivatePNode + neighbors: [{"name": "effectance_cnode", "node_type": "CNode"}] + confidence: 0.5 + threshold_delta: 0.5 + limit_depth: False + ltm_id: ltm_0 + + UtilityModel: + - + name: novelty_exploration + class_name: cognitive_nodes.utility_model.NoveltyUtilityModel + new_executor: True + threads: 2 + parameters: + trace_length: 20 + max_iterations: 50 + candidate_actions: 100 + ltm_id: ltm_0 + neighbors: [{"name": "novelty_cnode", "node_type": "CNode"}] + softmax_selection: False + softmax_temperature: 0.1 + +# Read by sim_bridge (config_file param): the actuator bounds it uses to +# un-normalize the Container action into scene_loader/StepAction fields. Keep +# these consistent with LTM.Globals.actuation_config above. +EmdbSimulator: + Actuation: + arm: + dx: + type: float + bounds: [-0.05, 0.05] + dy: + type: float + bounds: [-0.05, 0.05] + dz: + type: float + bounds: [-0.05, 0.05] + droll: + type: float + bounds: [-0.5, 0.5] + dpitch: + type: float + bounds: [-0.5, 0.5] + dyaw: + type: float + bounds: [-0.5, 0.5] + grasp: + type: float + bounds: [-1.0, 1.0] diff --git a/experiments/experiments/ur5_grasp_experiment.yaml b/experiments/experiments/ur5_grasp_experiment.yaml new file mode 100644 index 0000000..74fffba --- /dev/null +++ b/experiments/experiments/ur5_grasp_experiment.yaml @@ -0,0 +1,375 @@ +# Adapted from the 2D Baxter experiment for the UR5e MuJoCo simulator. +# Key changes vs the 2D version: +# - Single arm (all right_arm entries removed) +# - Units are METERS and DEGREES -> new actuation bounds and normalize ranges +# - Policy obs_dim: 4 (ball_angle 1 + dist_left_arm_ball 3) +Experiment: + name: main_loop + class_name: cognitive_processes.main_loop.MainLoopLight + new_executor: True + threads: 4 + parameters: + iterations: 2000 + trials: 5 + softmax_selection: False + softmax_temperature: 0.3 + kill_on_finish: True +Control: + id: ltm_simulator + control_topic: /main_loop/control + control_msg: cognitive_processes_interfaces.msg.ControlMsg + episodes_topic: /main_loop/episodes + episodes_msg: core_interfaces.msg.Container + executed_action_service: /mdb/baxter/executed_action + executed_action_msg: cognitive_node_interfaces.srv.Action + world_reset_service: /mdb/baxter/world_reset + world_reset_msg: cognitive_processes_interfaces.srv.WorldReset +LTM: + Globals: + actuation_config: + left_arm: ["dist", "angle", "z"] + + Files: + - + id: goodness + class: core.file.FileGoodness + file: goodness.txt + - + id: pnodes_content + class: core.file.FilePNodesContent + file: pnodes_content.txt + parameters: + save_interval: 0 + - + id: trials + class: core.file.FileTrialsSuccess + file: trials.txt + - + id: dataset + class: core.file.FileEpisodesDataset + file: dataset.csv + - + id: world_model_success + class: core.file.FileWorldModelSuccess + file: world_model_success.txt + - + id: save_models + class: core.file.FileSaveModels + file: models_save + parameters: + save_interval: 0 + + Connectors: + - + data: Space + default_class: cognitive_nodes.space.ANNSpace + - + data: Perception + default_class: cognitive_nodes.perception.Perception + - + data: PNode + default_class: cognitive_nodes.pnode.PNode + parameters: + space_class: cognitive_nodes.space.ANNSpace + history_size: 500 + - + data: CNode + default_class: cognitive_nodes.cnode.CNode + - + data: Goal + default_class: cognitive_nodes.goal.GoalMotiven + parameters: + space_class: cognitive_nodes.space.ANNSpace + history_size: 300 + min_confidence: 0.94 + ltm_id: ltm_0 + - + data: WorldModel + default_class: cognitive_nodes.world_model.WorldModelLearned + parameters: + episodes_msg: core_interfaces.msg.Container + episodes_topic: /main_loop/episodes + prediction_srv_type: cognitive_node_interfaces.srv.Predict + main_size: 2000 + train_sample: 200 + train_split: 0.8 + validation_split: 0.1 + secondary_size: 50 + retrain: True + - + data: UtilityModel + default_class: cognitive_nodes.utility_model.QUtilityModel + parameters: + max_iterations: 50 + candidate_actions: 100 + ltm_id: ltm_0 + candidate_generation: "latin" + softmax_selection: True + softmax_temperature: 0.01 + trace_length: 20 + min_traces: 20 + max_traces: 200 + train_traces: 10 + train_every: 5 + replace_every: 5 + discount_factor: 0.9 + reward_factor: 10.0 + max_antitraces: 5 + evaluation_method: "exponential" + epochs: 100 + learning_rate: 0.001 + output_activation: "linear" + hidden_layers: [256, 128] + - + data: Policy + default_class: cognitive_nodes.policy.PolicyLearned + parameters: + obs_dim: 5 # ball_angle (1) + dist_left_arm_ball (3) + arm_height (1) + buffer_size: 100000 + train_every: 5 + gradient_steps: 100 + batch_size: 100 + min_traces: 20 + max_steps: 50 + ltm_id: ltm_0 + learning_rate: 0.001 + Nodes: + Perception: + - + name: ball_angle + class_name: sim_2d_emdb.perception.Sim2DDistancesPerception + parameters: + default_msg: std_msgs.msg.Float32 + default_topic: /mdb/baxter/sensor/ball_angle + normalize_data: + min_value: -0.8 # meters (was -1300 in 2D units) + max_value: 0.8 + - + name: dist_left_arm_ball + class_name: sim_2d_emdb.perception.Sim2DDistancesPerception + parameters: + default_msg: simulators_interfaces.msg.ObjectListMsg + default_topic: /mdb/baxter/sensor/dist_left_arm_ball + normalize_data: + distance_min: 0 + distance_max: 1.5 # meters (was 3000) + angle_min: -180 + angle_max: 180 + - + name: arm_height + class_name: sim_2d_emdb.perception.Sim2DDistancesPerception + parameters: + default_msg: std_msgs.msg.Float32 + default_topic: /mdb/baxter/sensor/arm_height + normalize_data: + min_value: 0.0 # meters (EE height above floor) + max_value: 0.5 + + RobotPurpose: + - + name: grasped_ball_need + class_name: cognitive_nodes.robot_purpose.RobotPurpose + parameters: + weight: 1.0 + drive_id: 'grasped_ball_drive' + purpose_type: 'Mission' + terminal: True + - + name: novelty_need + class_name: cognitive_nodes.robot_purpose.RobotPurpose + parameters: + weight: 0.1 + drive_id: 'novelty_drive' + purpose_type: 'Need' + - + name: effectance_need + class_name: cognitive_nodes.robot_purpose.RobotPurpose + parameters: + weight: 0.25 + drive_id: 'effectance_drive' + purpose_type: 'Need' + - + name: model_creation_need + class_name: cognitive_nodes.robot_purpose.RobotPurpose + parameters: + weight: 1.0 + drive_id: 'model_creation_drive' + purpose_type: 'Need' + + Drive: + - + name: grasped_ball_drive + class_name: cognitive_nodes.drive.DriveExponential + parameters: + input_topic: /mdb/baxter/sensor/grasped_ball + input_msg: std_msgs.msg.Float32 + min_eval: 0.8 + neighbors: [{"name": "grasped_ball_need", "node_type": "RobotPurpose"}] + - + name: novelty_drive + class_name: cognitive_nodes.novelty.DriveNovelty + parameters: + neighbors: [{"name": "novelty_need", "node_type": "RobotPurpose"}] + - + name: effectance_drive + class_name: cognitive_nodes.effectance.DriveEffectanceInternal + parameters: + ltm_id: ltm_0 + min_confidence: 0.84 + limit_depth: False + neighbors: [{"name": "effectance_need", "node_type": "RobotPurpose"}] + - + name: model_creation_drive + class_name: cognitive_nodes.model_creation.ModelCreationDrive + parameters: + neighbors: [{"name": "model_creation_need", "node_type": "RobotPurpose"}] + LTM_id: ltm_0 + max_iterations: 20 + episodes_topic: /main_loop/episodes + episodes_msg: core_interfaces.msg.Container + model_creation_policy: model_creation_policy + + Goal: + - + name: novelty_goal + class_name: dummy_nodes.dummy_goal.GoalDummy + parameters: + neighbors: [{"name": "novelty_drive", "node_type": "Drive"}] + - + name: effectance_goal + class_name: dummy_nodes.dummy_goal.GoalDummy + parameters: + neighbors: [{"name": "effectance_drive", "node_type": "Drive"}] + - + name: model_creation_goal + class_name: dummy_nodes.dummy_goal.GoalDummy + parameters: + neighbors: [{"name": "model_creation_drive", "node_type": "Drive"}] + PNode: + - + name: novelty_pnode + class_name: dummy_nodes.dummy_pnodes.ActivatedDummyPNode + parameters: + space_class: cognitive_nodes.space.ActivatedDummySpace + - + name: effectance_pnode + class_name: dummy_nodes.dummy_pnodes.ActivatedDummyPNode + parameters: + space_class: cognitive_nodes.space.ActivatedDummySpace + - + name: model_creation_pnode + class_name: dummy_nodes.dummy_pnodes.ActivatedDummyPNode + parameters: + space_class: cognitive_nodes.space.ActivatedDummySpace + CNode: + - + name: novelty_cnode + class_name: cognitive_nodes.cnode.CNode + parameters: + neighbors: [{"name": "novelty_goal", "node_type": "Goal"}, {"name": "novelty_pnode", "node_type": "PNode"}] + - + name: model_creation_cnode + class_name: cognitive_nodes.cnode.CNode + parameters: + neighbors: [{"name": "model_creation_goal", "node_type": "Goal"}, {"name": "model_creation_pnode", "node_type": "PNode"}] + - + name: effectance_cnode + class_name: cognitive_nodes.cnode.CNode + parameters: + neighbors: [{"name": "effectance_goal", "node_type": "Goal"}, {"name": "effectance_pnode", "node_type": "PNode"}] + + Policy: + - + name: model_creation_policy + class_name: cognitive_nodes.model_creation.ModelCreationPolicy + parameters: + neighbors: [{"name": "model_creation_cnode", "node_type": "CNode"}] + LTM_id: ltm_0 + max_iterations: 20 + episodes_topic: /main_loop/episodes + episodes_msg: core_interfaces.msg.Container + - + name: effectance_policy + class_name: cognitive_nodes.effectance.PolicyEffectanceInternal + parameters: + goal_class: cognitive_nodes.effectance.GoalActivatePNode + neighbors: [{"name": "effectance_cnode", "node_type": "CNode"}] + confidence: 0.5 + threshold_delta: 0.5 + limit_depth: False + ltm_id: ltm_0 + + UtilityModel: + - + name: novelty_exploration + class_name: cognitive_nodes.utility_model.NoveltyUtilityModel + new_executor: True + threads: 2 + parameters: + trace_length: 20 + max_iterations: 50 + candidate_actions: 100 + ltm_id: ltm_0 + neighbors: [{"name": "novelty_cnode", "node_type": "CNode"}] + softmax_selection: False + softmax_temperature: 0.1 + +# Section name kept as SimulatedBaxter so the loader works unchanged +# (sim_ur5_mujoco.py also accepts a "SimulatedUR5" key if you rename it) +SimulatedBaxter: + Actuation: + left_arm: + dist: + type: float + bounds: [0, 0.25] # meters to advance along the heading + angle: + type: float + bounds: [-120, 120] # degrees, heading change (as in sim2D) + z: + type: float + bounds: [-0.15, 0.15] # meters, height change relative to current EE z + Perceptions: + - + name: ball + perception_topic: /mdb/baxter/sensor/ball + perception_msg: simulators_interfaces.msg.Object2DListMsg + - + name: ball_in_left_hand + perception_topic: /mdb/baxter/sensor/ball_in_left_hand + perception_msg: std_msgs.msg.Bool + - + name: box + perception_topic: /mdb/baxter/sensor/box + perception_msg: simulators_interfaces.msg.Object2DListMsg + - + name: left_arm + perception_topic: /mdb/baxter/sensor/left_arm + perception_msg: simulators_interfaces.msg.Object2DListMsg + - + name: ball_in_box + perception_topic: /mdb/baxter/sensor/ball_in_box + perception_msg: std_msgs.msg.Float32 + - + name: grasped_ball + perception_topic: /mdb/baxter/sensor/grasped_ball + perception_msg: std_msgs.msg.Float32 + - + name: dist_left_arm_ball + perception_topic: /mdb/baxter/sensor/dist_left_arm_ball + perception_msg: simulators_interfaces.msg.ObjectListMsg + - + name: dist_ball_box + perception_topic: /mdb/baxter/sensor/dist_ball_box + perception_msg: simulators_interfaces.msg.ObjectListMsg + - + name: box_angle + perception_topic: /mdb/baxter/sensor/box_angle + perception_msg: std_msgs.msg.Float32 + - + name: ball_angle + perception_topic: /mdb/baxter/sensor/ball_angle + perception_msg: std_msgs.msg.Float32 + - + name: arm_height + perception_topic: /mdb/baxter/sensor/arm_height + perception_msg: std_msgs.msg.Float32 diff --git a/experiments/launch/lift_launch.py b/experiments/launch/lift_launch.py new file mode 100644 index 0000000..fad131a --- /dev/null +++ b/experiments/launch/lift_launch.py @@ -0,0 +1,118 @@ +from launch import LaunchDescription, LaunchContext +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.event_handlers import OnProcessExit +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + OpaqueFunction, + RegisterEventHandler, + Shutdown, +) +from launch.substitutions import ( + LaunchConfiguration, + FindExecutable, + PathJoinSubstitution, +) + + +def launch_setup(context: LaunchContext, *args, **kwargs): + + logger = LaunchConfiguration("log_level") + random_seed = LaunchConfiguration("random_seed") + experiment_file = LaunchConfiguration("experiment_file") + experiment_package = LaunchConfiguration("experiment_package") + config_package = LaunchConfiguration("config_package") + config_file = LaunchConfiguration("config_file") + + core_node = Node( + package="core", + executable="commander", + output="screen", + arguments=["--ros-args", "--log-level", logger], + parameters=[{"random_seed": random_seed}], + ) + + ltm_node = Node( + package="core", + executable="ltm", + output="screen", + arguments=["0", "--ros-args", "--log-level", logger], + ) + + # The bridge plays the "simulator" role for the architecture: it hosts the + # executed_action / world_reset services and triggers commander/load_experiment. + # The actual physics lives in emdb_simulator's scene_loader, launched + # separately in the TFM venv (see lift_experiment.yaml header). + bridge_node = Node( + package="mujoco_emdb_sim", + executable="sim_bridge", + output="screen", + arguments=["--ros-args", "--log-level", logger], + parameters=[ + { + "random_seed": random_seed, + "config_file": PathJoinSubstitution( + [FindPackageShare(experiment_package), "experiments", experiment_file] + ), + } + ], + ) + + config_service_call = ExecuteProcess( + cmd=[ + [ + FindExecutable(name="ros2"), + " ", + "service call", + " ", + "commander/load_config", + " ", + "core_interfaces/srv/LoadConfig", + " ", + '"{file:', + " ", + PathJoinSubstitution( + [FindPackageShare(config_package), "config", config_file] + ), + '}"', + ] + ], + shell=True, + ) + + shutdown_on_exit = RegisterEventHandler( + OnProcessExit( + target_action=core_node, + on_exit=[Shutdown()], + ) + ) + + return [config_service_call, core_node, ltm_node, bridge_node, shutdown_on_exit] + + +def generate_launch_description(): + + declared_arguments = [ + DeclareLaunchArgument( + "log_level", default_value=["info"], description="Logging level"), + DeclareLaunchArgument( + "random_seed", default_value="0", + description="The seed to the random numbers generator"), + DeclareLaunchArgument( + "experiment_file", default_value="lift_experiment.yaml", + description="The file that loads the experiment config"), + DeclareLaunchArgument( + "config_file", default_value="commander_threaded.yaml", + description="The file that loads the commander config"), + DeclareLaunchArgument( + "config_package", default_value="core", + description="Package where the config file is located"), + DeclareLaunchArgument( + "experiment_package", default_value="experiments", + description="Package where the experiment file is located"), + ] + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/experiments/launch/ur5_grasp_launch.py b/experiments/launch/ur5_grasp_launch.py new file mode 100644 index 0000000..54f2bd6 --- /dev/null +++ b/experiments/launch/ur5_grasp_launch.py @@ -0,0 +1,180 @@ +from launch import LaunchDescription, LaunchContext +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.event_handlers import OnProcessExit +from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction, RegisterEventHandler, Shutdown +from launch.substitutions import ( + LaunchConfiguration, + FindExecutable, + PathJoinSubstitution, + Command, +) + + +def launch_setup(context: LaunchContext, *args, **kwargs): + + logger = LaunchConfiguration("log_level") + random_seed = LaunchConfiguration("random_seed") + visualize = LaunchConfiguration("visualize") + experiment_file = LaunchConfiguration("experiment_file") + experiment_package = LaunchConfiguration("experiment_package") + config_package = LaunchConfiguration("config_package") + config_file = LaunchConfiguration("config_file") + realtime = LaunchConfiguration("realtime") + sim_speed = LaunchConfiguration("sim_speed") + spawn_box = LaunchConfiguration("spawn_box") + + core_node = Node( + package="core", + executable="commander", + output="screen", + arguments=["--ros-args", "--log-level", logger], + parameters=[{"random_seed": random_seed}], + ) + + ltm_node = Node( + package="core", + executable="ltm", + output="screen", + arguments=["0", "--ros-args", "--log-level", logger], + ) + + simulator_node = Node( + package="ur5_mujoco", + executable="sim_ur5", + output="screen", + arguments=["--ros-args", "--log-level", logger], + parameters=[ + { + "random_seed": random_seed, + "config_file": PathJoinSubstitution( + [FindPackageShare(experiment_package), "experiments", experiment_file] + ), + "visualize": visualize, + "realtime": realtime, + "sim_speed": sim_speed, + "spawn_box": spawn_box, + } + ], + ) + + config_service_call = ExecuteProcess( + cmd=[ + [ + FindExecutable(name="ros2"), + " ", + "service call", + " ", + "commander/load_config", + " ", + "core_interfaces/srv/LoadConfig", + " ", + '"{file:', + " ", + PathJoinSubstitution( + [FindPackageShare(config_package), "config", config_file] + ), + '}"', + ] + ], + shell=True, + ) + + shutdown_on_exit = RegisterEventHandler( + OnProcessExit( + target_action=core_node, # Nodo que supervisar + on_exit=[Shutdown()], # Acción: Cerrar todos los nodos + ) + ) + + nodes_to_start = [config_service_call, core_node, ltm_node, simulator_node, shutdown_on_exit] + + return nodes_to_start + + +def generate_launch_description(): + + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "log_level", + default_value=["info"], + description="Logging level", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "random_seed", + default_value="0", + description="The seed to the random numbers generator", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "experiment_file", + default_value="ur5_grasp_experiment.yaml", + description="The file that loads the experiment config", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "config_file", + default_value="commander_threaded.yaml", + description="The file that loads the commander config", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "config_package", + default_value="core", + description="Package where the config file is located", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "experiment_package", + default_value="experiments", + description="Package where the experiment file is located", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "visualize", + default_value="True", + description="Whether to visualize the simulation or not", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "realtime", + default_value="False", + description="Pace the simulation to wall clock for smooth viewing", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "sim_speed", + default_value="2.0", + description="Playback speed when realtime is on (2.0 = 2x)", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "spawn_box", + default_value="True", + description="Whether the target box is present. False parks it " + "off-scene (invisible, inert) for a ball-only setup.", + ) + ) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) \ No newline at end of file diff --git a/mujoco_emdb_sim/mujoco_emdb_sim/__init__.py b/mujoco_emdb_sim/mujoco_emdb_sim/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/mujoco_emdb_sim/mujoco_emdb_sim/perception.py b/mujoco_emdb_sim/mujoco_emdb_sim/perception.py new file mode 100644 index 0000000..7cca4fa --- /dev/null +++ b/mujoco_emdb_sim/mujoco_emdb_sim/perception.py @@ -0,0 +1,69 @@ +""" +Perception subclass for emdb_simulator's perception_mode:=mdb topics. + +The generic cognitive_nodes.perception.Perception is abstract +(process_and_send_reading raises), so even the scalar Bool/Float32 sensors need +a concrete subclass. This one handles both shapes emitted on +/emdb/simulator/sensor/*: + + * emdb_interfaces/ObjectStateArray (e.g. .../obj): flattens the first object's + world position into normalized [x, y, z]. + * std_msgs/Bool, std_msgs/Float32 (.../obj/grasped, .../progress): passed + through as a single 'data' feature (already in [0, 1]). +""" + +import numpy as np + +from core.container import Container +from cognitive_nodes.perception import Perception + + +class EmdbSimulatorPerception(Perception): + """Flatten emdb_simulator mdb sensor messages into e-MDB perception values.""" + + # Pass everything to the base by KEYWORD. The Perception base has a `node_type` + # parameter between `class_name` and `default_msg`, so a positional super().__init__ + # would misalign the args (default_msg landing in the wrong slot). Keyword args are + # robust to that; node_type defaults to "Perception" in the base, which is exactly + # the LTM category this node should register under. + def __init__(self, name="perception", + class_name="cognitive_nodes.perception.Perception", + default_msg=None, default_topic=None, normalize_data=None, + **params): + super().__init__(name, class_name=class_name, + default_msg=default_msg, default_topic=default_topic, + normalize_data=normalize_data, **params) + + def _norm(self, value, key_min, key_max): + nv = self.normalize_values + if nv and key_min in nv and key_max in nv: + lo, hi = nv[key_min], nv[key_max] + if hi != lo: + return (value - lo) / (hi - lo) + return value + + def process_and_send_reading(self): + reading = self.reading + + if hasattr(reading, "objects"): # emdb_interfaces/ObjectStateArray + if len(reading.objects) == 0: + self.get_logger().warning("Received empty ObjectStateArray.") + return + pos = reading.objects[0].pose.position + x = self._norm(float(pos.x), "x_min", "x_max") + y = self._norm(float(pos.y), "y_min", "y_max") + z = self._norm(float(pos.z), "z_min", "z_max") + labels = ["x", "y", "z"] + data = np.array([x, y, z]) + else: # std_msgs/Bool or /Float32 + labels = ["data"] + data = np.array([float(reading.data)]) + + if self.container is None: + self.container = Container(self.name, max_size=1, + container_type="perception", labels=labels) + self.container.push(data, labels, + timestamps=self.get_clock().now().nanoseconds) + self.get_logger().debug( + f"Publishing normalized {self.name} = {self.container}") + self.perception_publisher.publish(self.container.to_msg()) diff --git a/mujoco_emdb_sim/mujoco_emdb_sim/sim_bridge.py b/mujoco_emdb_sim/mujoco_emdb_sim/sim_bridge.py new file mode 100644 index 0000000..36242da --- /dev/null +++ b/mujoco_emdb_sim/mujoco_emdb_sim/sim_bridge.py @@ -0,0 +1,231 @@ +""" +sim_bridge -- adapter between the e-MDB cognitive architecture and +emdb_simulator's RoboCasa/robosuite MuJoCo scenes. + +The cognitive architecture speaks its own control protocol: + * executed_action_service (cognitive_node_interfaces/Action) + * world_reset_service (cognitive_processes_interfaces/WorldReset) + * control_topic (cognitive_processes_interfaces/ControlMsg) + +emdb_simulator's scene_loader speaks its own RL protocol: + * /step_action (emdb_interfaces/StepAction) -- one EE-delta physics step + * /reset_episode (emdb_interfaces/ResetEpisode) -- restart the episode + +This node hosts the e-MDB services and forwards each request to scene_loader. +It is task-agnostic: the action layout/bounds come from the experiment yaml's +EmdbSimulator.Actuation block, so the same bridge serves any emdb_simulator +scene. It does NOT publish perceptions: scene_loader already publishes them in +perception_mode:=mdb on /emdb/simulator/sensor/*, and the experiment's +Perception nodes subscribe there directly. + +Runtime note: this node imports emdb_interfaces (from the TFM workspace), so +the shell that launches it must source BOTH this workspace's install and +/root/TFM/ros_packages/install (see the launch instructions). +""" + +import os + +import yaml +import yamlloader + +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rcl_interfaces.msg import ParameterDescriptor + +from core.service_client import ServiceClient +from core_interfaces.srv import LoadConfig +from core.container import Container +from core.utils import class_from_classname + +# Friend's sim interface. Imported at runtime; requires the TFM ros_packages +# install to be sourced on top of this workspace. +from emdb_interfaces.srv import StepAction, ResetEpisode + + +class SimBridge(Node): + """Translate e-MDB action/reset calls into scene_loader step/reset calls.""" + + def __init__(self): + super().__init__("sim_bridge") + + self.random_seed = ( + self.declare_parameter("random_seed", value=0) + .get_parameter_value().integer_value + ) + self.config_file = ( + self.declare_parameter( + "config_file", + descriptor=ParameterDescriptor(dynamic_typing=True), + ).get_parameter_value().string_value + ) + self.standalone = ( + self.declare_parameter("standalone", value=False) + .get_parameter_value().bool_value + ) + # scene_loader's native RL services (override if you remap them). + self.step_service = ( + self.declare_parameter("step_service", value="/step_action") + .get_parameter_value().string_value + ) + self.reset_service = ( + self.declare_parameter("reset_service", value="/reset_episode") + .get_parameter_value().string_value + ) + + self.cbgroup_server = MutuallyExclusiveCallbackGroup() + + self.actuation_config = None + # Actuator label used in the experiment yaml's actuation_config + # (feature labels arrive as "arm:dx", "arm:dy", ...). + self.arm_name = "arm" + + # Sync clients to scene_loader, created lazily on first use so the bridge + # can start before the sim finishes building its (heavy) scene. + self._step_client = None + self._reset_client = None + # True once the world_reset SERVICE is hosted -> control-topic "reset_world" + # commands are then ignored (the service is authoritative). This also avoids + # two callbacks driving the shared reset client concurrently. + self.service_world_reset = False + + # ------------------------------------------------ scene_loader clients + def _step(self, **fields): + if self._step_client is None: + self.get_logger().info( + f"Connecting to sim step service {self.step_service}...") + self._step_client = ServiceClient(StepAction, self.step_service) + return self._step_client.send_request(**fields) + + def _reset(self, **fields): + if self._reset_client is None: + self.get_logger().info( + f"Connecting to sim reset service {self.reset_service}...") + self._reset_client = ServiceClient(ResetEpisode, self.reset_service) + return self._reset_client.send_request(**fields) + + # ------------------------------------------------ action decoding + def denormalize_actuation(self, action: Container, actuation_config): + """Un-normalize [0,1] Container features to engineering units via the + yaml `bounds`. Same convention as Sim2DSimple / sim_ur5.""" + action_dims = action.feature_labels + data = action.read() + for dim in action_dims: + actuator, param = dim.split(":", 1) + if actuation_config[actuator][param]["type"] == "float": + bounds = actuation_config[actuator][param]["bounds"] + value = data.sel(features=dim).values + data.loc[{"features": dim}] = ( + bounds[0] + (value * (bounds[1] - bounds[0]))) + return data + + def _feature(self, vec, features, name, default=0.0): + key = f"{self.arm_name}:{name}" + if key in features: + return float(vec.sel(features=key).values) + return default + + # ------------------------------------------------ e-MDB service callbacks + def executed_action_callback(self, request, response): + action = Container.from_msg(request.action) + vec = self.denormalize_actuation(action, self.actuation_config) + features = list(vec.coords["features"].values) + + # scene_loader's StepAction: 6 EE deltas + int grasp. base_* / next_* are + # left at 0 (fixed single UR5 arm, no mobile-base motion in this task). + # grasp: float in [-1, 1] rounded to scene_loader's int32 (per the yaml). + result = self._step( + dx=self._feature(vec, features, "dx"), + dy=self._feature(vec, features, "dy"), + dz=self._feature(vec, features, "dz"), + droll=self._feature(vec, features, "droll"), + dpitch=self._feature(vec, features, "dpitch"), + dyaw=self._feature(vec, features, "dyaw"), + base_dx=0.0, base_dy=0.0, base_dyaw=0.0, + grasp=int(round(self._feature(vec, features, "grasp"))), + next_arm=0, next_robot=0, + ) + response.success = bool(result.success) if result is not None else False + return response + + def world_reset_callback(self, request, response): + # -1 / -1 keeps the current layout/style; the episode just restarts. + result = self._reset(layout_id=-1, style_id=-1) + response.success = bool(result.success) if result is not None else False + return response + + def control_callback(self, data): + command = getattr(data, "command", "") + if command == "reset_world" and not self.service_world_reset: + self._reset(layout_id=-1, style_id=-1) + elif command == "end": + self.get_logger().info("Ending bridge as requested by LTM...") + rclpy.shutdown() + + # ------------------------------------------------ yaml config + def load_configuration(self): + if not self.config_file or not os.path.isfile(self.config_file): + self.get_logger().error( + f"Config file '{self.config_file}' not found!") + rclpy.shutdown() + return + config = yaml.load( + open(self.config_file, "r", encoding="utf-8"), + Loader=yamlloader.ordereddict.CLoader, + ) + self.actuation_config = config["EmdbSimulator"]["Actuation"] + self.setup_control_channel(config["Control"]) + + if not self.standalone: + self.load_experiment_file_in_commander() + else: + self.get_logger().info( + "STANDALONE mode: not contacting the commander") + + def setup_control_channel(self, simulation): + self.ident = simulation["id"] + message = class_from_classname(simulation["control_msg"]) + self.create_subscription( + message, simulation["control_topic"], self.control_callback, 0) + + service_action = simulation.get("executed_action_service") + service_world_reset = simulation.get("world_reset_service") + if simulation.get("executed_policy_topic"): + raise RuntimeError( + "Topic-triggered policies not supported; use " + "executed_action_service") + if service_action: + msg_srv = class_from_classname(simulation["executed_action_msg"]) + self.create_service( + msg_srv, service_action, self.executed_action_callback, + callback_group=self.cbgroup_server) + if service_world_reset: + self.service_world_reset = True + msg_reset = class_from_classname(simulation["world_reset_msg"]) + self.create_service( + msg_reset, service_world_reset, self.world_reset_callback, + callback_group=self.cbgroup_server) + + def load_experiment_file_in_commander(self): + self.load_client = ServiceClient(LoadConfig, "commander/load_experiment") + return self.load_client.send_request(file=self.config_file) + + +def main(args=None): + rclpy.init(args=args) + bridge = SimBridge() + bridge.load_configuration() + # Multi-threaded so world_reset / control can be serviced while an + # executed_action call is blocked waiting on the sim's step. + executor = MultiThreadedExecutor(num_threads=4) + try: + rclpy.spin(bridge, executor=executor) + except KeyboardInterrupt: + print("Keyboard Interrupt Detected: Shutting down sim bridge...") + finally: + bridge.destroy_node() + + +if __name__ == "__main__": + main() diff --git a/mujoco_emdb_sim/package.xml b/mujoco_emdb_sim/package.xml new file mode 100644 index 0000000..bfbcf72 --- /dev/null +++ b/mujoco_emdb_sim/package.xml @@ -0,0 +1,28 @@ + + + + mujoco_emdb_sim + 0.0.0 + Bridge + perception adapters connecting emdb_simulator + (RoboCasa/robosuite MuJoCo scenes) to the e-MDB architecture. + jummo + Apache-2.0 + + rclpy + core + core_interfaces + cognitive_nodes + cognitive_node_interfaces + cognitive_processes_interfaces + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/mujoco_emdb_sim/resource/mujoco_emdb_sim b/mujoco_emdb_sim/resource/mujoco_emdb_sim new file mode 100644 index 0000000..e69de29 diff --git a/mujoco_emdb_sim/setup.cfg b/mujoco_emdb_sim/setup.cfg new file mode 100644 index 0000000..29c1721 --- /dev/null +++ b/mujoco_emdb_sim/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/mujoco_emdb_sim +[install] +install_scripts=$base/lib/mujoco_emdb_sim diff --git a/mujoco_emdb_sim/setup.py b/mujoco_emdb_sim/setup.py new file mode 100644 index 0000000..b4166eb --- /dev/null +++ b/mujoco_emdb_sim/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = 'mujoco_emdb_sim' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='jummo', + maintainer_email='jpmoro0307@gmail.com', + description='Bridge + perception adapters connecting emdb_simulator ' + '(RoboCasa/robosuite MuJoCo scenes) to the e-MDB architecture.', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'sim_bridge = mujoco_emdb_sim.sim_bridge:main', + ], + }, +)