From a7b8a0ab6e1749ae74370bd0628d007c429b5a0b Mon Sep 17 00:00:00 2001 From: Carl Date: Wed, 9 Nov 2022 11:43:07 +0100 Subject: [PATCH] Update godot integration structure Use Simulator global singleton instead of signals Make command code run inside the commands Update structure, remove custom SimulationNode, add config Get actors from gltf Fix callback and make default obs from step Change actor setup Cleanup and switch to rl manager --- examples/under_the_hood/godot_example.py | 9 ++- integrations/Godot/README.md | 17 ++++ .../Godot/simulate_godot/Scenes/scene.tscn | 4 +- .../simulate_godot/Simulate/Bridge/client.gd | 14 ++-- .../simulate_godot/Simulate/Bridge/command.gd | 14 +--- .../simulate_godot/Simulate/Commands/close.gd | 10 +-- .../Simulate/Commands/initialize.gd | 12 +-- .../simulate_godot/Simulate/Commands/reset.gd | 9 +-- .../simulate_godot/Simulate/Commands/step.gd | 34 +++++++- .../Simulate/GLTF/hf_actuator.gd | 20 ++--- .../Simulate/GLTF/hf_articulation_body.gd | 4 +- .../Simulate/GLTF/hf_collider.gd | 4 +- .../Simulate/GLTF/hf_extensions.gd | 41 ++++------ .../Simulate/GLTF/hf_physic_material.gd | 4 +- .../Simulate/GLTF/hf_raycast_sensor.gd | 3 +- .../Simulate/GLTF/hf_reward_function.gd | 4 +- .../Simulate/GLTF/hf_rigid_body.gd | 11 ++- .../Simulate/GLTF/hf_state_sensor.gd | 3 +- .../RLActors/Sensors/camera_sensor.gd | 11 +++ .../RLActors/Sensors/raycast_sensor.gd | 9 +++ .../Simulate/RLActors/Sensors/sensor.gd | 2 + .../Simulate/RLActors/Sensors/state_sensor.gd | 11 +++ .../Simulate/RLActors/actions.gd | 33 ++++++++ .../simulate_godot/Simulate/RLActors/actor.gd | 81 +++++++++++++++++++ .../simulate_godot/Simulate/RLActors/map.gd | 20 +++++ .../{RLAgents => RLActors}/reward_function.gd | 11 +-- .../simulate_godot/Simulate/RLAgents/agent.gd | 59 -------------- .../Simulate/RLAgents/agent_manager.gd | 25 ------ .../Simulate/RLAgents/rl_action.gd | 33 -------- .../Godot/simulate_godot/Simulate/config.gd | 30 +++++++ .../simulate_godot/Simulate/rl_manager.gd | 13 +++ .../Simulate/simulation_node.gd | 48 ----------- .../simulate_godot/Simulate/simulator.gd | 32 +++++--- .../Godot/simulate_godot/project.godot | 39 +++++---- src/simulate/engine/godot_engine.py | 8 ++ 35 files changed, 393 insertions(+), 289 deletions(-) create mode 100644 integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/camera_sensor.gd create mode 100644 integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/raycast_sensor.gd create mode 100644 integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/sensor.gd create mode 100644 integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/state_sensor.gd create mode 100644 integrations/Godot/simulate_godot/Simulate/RLActors/actions.gd create mode 100644 integrations/Godot/simulate_godot/Simulate/RLActors/actor.gd create mode 100644 integrations/Godot/simulate_godot/Simulate/RLActors/map.gd rename integrations/Godot/simulate_godot/Simulate/{RLAgents => RLActors}/reward_function.gd (91%) delete mode 100644 integrations/Godot/simulate_godot/Simulate/RLAgents/agent.gd delete mode 100644 integrations/Godot/simulate_godot/Simulate/RLAgents/agent_manager.gd delete mode 100644 integrations/Godot/simulate_godot/Simulate/RLAgents/rl_action.gd create mode 100644 integrations/Godot/simulate_godot/Simulate/config.gd create mode 100644 integrations/Godot/simulate_godot/Simulate/rl_manager.gd delete mode 100644 integrations/Godot/simulate_godot/Simulate/simulation_node.gd diff --git a/examples/under_the_hood/godot_example.py b/examples/under_the_hood/godot_example.py index 84a37b30..2e3bba90 100644 --- a/examples/under_the_hood/godot_example.py +++ b/examples/under_the_hood/godot_example.py @@ -139,11 +139,12 @@ def make_scene(build_exe): # act! for i in range(100): action = [env.action_space.sample()] + print(f"action: {action}") obs, reward, done, info = env.step(action=action) - axim1.set_data(obs["CameraSensor"].reshape(3, camera_height, camera_width).transpose(1, 2, 0)) - fig1.canvas.flush_events() - axim2.set_data(obs["SecurityCamera"].reshape(3, 256, 256).transpose(1, 2, 0)) - fig2.canvas.flush_events() + # axim1.set_data(obs["CameraSensor"].reshape(3, camera_height, camera_width).transpose(1, 2, 0)) + # fig1.canvas.flush_events() + # axim2.set_data(obs["SecurityCamera"].reshape(3, 256, 256).transpose(1, 2, 0)) + # fig2.canvas.flush_events() plt.pause(0.1) diff --git a/integrations/Godot/README.md b/integrations/Godot/README.md index c518340a..7c8920f1 100644 --- a/integrations/Godot/README.md +++ b/integrations/Godot/README.md @@ -25,3 +25,20 @@ scene.render() - Run the python script. It should print "Waiting for connection...", meaning that it has spawned a websocket server, and is waiting for a connection from the Godot client. - Press Play (F5) in Godot. It should connect to the Python client, then display a basic Sphere. The python script should finish execution. +### Add glTF extensions + +The integration uses the `GLTFDocument` API to load and save glTF files. You can find the main glTF loading logic in the `simulate_godot/Simulate/Commands/initialize.gd` script. +To register new extensions, we use the `GLTFDocumentExtension` API. Currently, we register the `HFExtension` class to load every extension. + +For every node found with the label `extensions`, the `_import_node` method from the registered `HFExtension` class will be called with a few arguments: +- `state`: the current `GLTFState` object which contains basically all the data and nodes. +- `gltf_node`: -- not sure what this one is for -- +- `json`: json object containing all the default attributes of the node (name, id, translation, rotation, etc.) +- `node`: a temporary node in the tree that we can replace with a more adapted class. + +All extensions are mapped in the `_import_node` method. To add a new extension, you need to add a mapping following the same pattern as you can find in the file. +You can then create a new script extending a Node type that makes the most sense for the glTF extension, then import all the fields of the extensions as attributes of this node. +To see how you can access the custom attributes through `GLTFState` object, you can check how the other extension nodes are written. + +Now, when the glTF parser will find a node with the extension you defined, it'll create a node of the types you defined and fill its attributes with all the values you mapped and that are found in the glTF file. +Once all this data is imported in the scene, you can use it in any way you want. \ No newline at end of file diff --git a/integrations/Godot/simulate_godot/Scenes/scene.tscn b/integrations/Godot/simulate_godot/Scenes/scene.tscn index 7fcae24e..f1d1597c 100644 --- a/integrations/Godot/simulate_godot/Scenes/scene.tscn +++ b/integrations/Godot/simulate_godot/Scenes/scene.tscn @@ -6,6 +6,6 @@ [node name="BaseWorld" type="Node3D" parent="."] -[node name="Camera" type="Camera3D" parent="BaseWorld"] -transform = Transform3D(0.707107, -0.5, 0.5, 0, 0.707107, 0.707107, -0.707107, -0.5, 0.5, 8, 8, 8) +[node name="Camera3D" type="Camera3D" parent="BaseWorld"] +transform = Transform3D(0.651331, 0, -0.758794, 0, 1, 0, 0.758794, 0, 0.651331, -5.6946, 6.34299, 6.48804) script = ExtResource("2_cqhjv") diff --git a/integrations/Godot/simulate_godot/Simulate/Bridge/client.gd b/integrations/Godot/simulate_godot/Simulate/Bridge/client.gd index f82c7c8e..b3d579eb 100644 --- a/integrations/Godot/simulate_godot/Simulate/Bridge/client.gd +++ b/integrations/Godot/simulate_godot/Simulate/Bridge/client.gd @@ -18,15 +18,17 @@ var _warmed_up: bool = false func _ready() -> void: _status = _stream.get_status() + func _physics_process(_delta) -> void: # this is called at a fixed rate update_status() - print("------------physic step--------------") +# print("------------physic step--------------") if _status == _stream.STATUS_CONNECTED: get_tree().paused = true read() + func update_status() -> void: # Get the current stream status _stream.poll() @@ -46,6 +48,7 @@ func update_status() -> void: print("Error with socket stream.") emit_signal("error") + func read() -> void: # Get data from TCP stream, only reads 1 command at a time while true: @@ -77,6 +80,7 @@ func read() -> void: get_tree().paused = false break + func connect_to_host(host: String, port: int) -> void: # Connect to the TCP server that was setup on the python-side API print("Connecting to %s:%d" % [host, port]) @@ -89,18 +93,16 @@ func connect_to_host(host: String, port: int) -> void: _stream.disconnect_from_host() emit_signal("error") + func send(out_data: PackedByteArray) -> bool: # Send back data to the python-side code, usually for callbacks and obs if _status != _stream.STATUS_CONNECTED: print("Error: Stream is not currently connected.") return false - var stream_error: int = _stream.put_data(PackedByteArray([len(out_data)])) - if stream_error != OK: - print("Error writing to stream: ", error) - return false + _stream.put_32(len(out_data)) - stream_error = _stream.put_data(out_data) + var stream_error = _stream.put_data(out_data) if stream_error != OK: print("Error writing to stream: ", error) return false diff --git a/integrations/Godot/simulate_godot/Simulate/Bridge/command.gd b/integrations/Godot/simulate_godot/Simulate/Bridge/command.gd index fe3be828..a1bcf964 100644 --- a/integrations/Godot/simulate_godot/Simulate/Bridge/command.gd +++ b/integrations/Godot/simulate_godot/Simulate/Bridge/command.gd @@ -2,20 +2,16 @@ class_name Command extends Node -signal callback - var content : Variant var _commands : Dictionary func load_commands() -> void: - var com_path : String = "res://Simulate/Commands" var directory = DirAccess.open(com_path) if directory: directory.list_dir_begin() - while true: var file = directory.get_next() if file == "": @@ -23,18 +19,12 @@ func load_commands() -> void: var command_name = file.split(".")[0] var command_script = load(com_path + "/" + file) _commands[command_name] = command_script.new() - _commands[command_name].connect("callback", _handle_callback) - add_child(_commands[command_name]) - directory.list_dir_end() + func execute(type: String) -> void: if type in _commands: _commands[type].execute(content) else: print("Unknown command.") - emit_signal("callback", PackedByteArray([97, 99, 107])) - - -func _handle_callback(callback_data: PackedByteArray) -> void: - emit_signal("callback", callback_data) + Simulator.send_callback("{}") diff --git a/integrations/Godot/simulate_godot/Simulate/Commands/close.gd b/integrations/Godot/simulate_godot/Simulate/Commands/close.gd index ec5a3142..b95fa524 100644 --- a/integrations/Godot/simulate_godot/Simulate/Commands/close.gd +++ b/integrations/Godot/simulate_godot/Simulate/Commands/close.gd @@ -2,11 +2,7 @@ extends Node # Close the simulation -signal callback - - func execute(_content: Variant) -> void: - get_tree().paused = false - get_tree().quit() - print("Close called!") - emit_signal("callback", PackedByteArray([97, 99, 107])) + Simulator.get_tree().paused = false +# Simulator.get_tree().quit() + Simulator.send_callback("{}") diff --git a/integrations/Godot/simulate_godot/Simulate/Commands/initialize.gd b/integrations/Godot/simulate_godot/Simulate/Commands/initialize.gd index 1acda15c..d87d93e2 100644 --- a/integrations/Godot/simulate_godot/Simulate/Commands/initialize.gd +++ b/integrations/Godot/simulate_godot/Simulate/Commands/initialize.gd @@ -2,9 +2,6 @@ extends Node # Build the scene and sets global simulation metadata -signal callback - - func execute(content: Variant) -> void: var content_bytes : PackedByteArray = Marshalls.base64_to_raw(content["b64bytes"]) @@ -12,9 +9,12 @@ func execute(content: Variant) -> void: var gltf_doc : GLTFDocument = GLTFDocument.new() gltf_doc.register_gltf_document_extension(HFExtensions.new(), false) - + gltf_doc.append_from_buffer(content_bytes, "", gltf_state) var gltf_scene = gltf_doc.generate_scene(gltf_state) - get_tree().current_scene.add_child(gltf_scene) + gltf_scene.name = "Scene" - emit_signal("callback", PackedByteArray([97, 99, 107])) + Config.parse(gltf_state.json["extensions"].get("HF_config", {})) + Simulator.add_child(gltf_scene) + Simulator.load_nodes() + Simulator.send_callback("{}") diff --git a/integrations/Godot/simulate_godot/Simulate/Commands/reset.gd b/integrations/Godot/simulate_godot/Simulate/Commands/reset.gd index 977ac6c9..d6766228 100644 --- a/integrations/Godot/simulate_godot/Simulate/Commands/reset.gd +++ b/integrations/Godot/simulate_godot/Simulate/Commands/reset.gd @@ -2,9 +2,6 @@ extends Node # Reset the simulation -signal callback - - -func execute(_content:Variant) -> void: - get_tree().paused = false - emit_signal("callback", PackedByteArray([97, 99, 107])) +func execute(_content: Variant) -> void: + Simulator.get_tree().paused = false + Simulator.send_callback("{}") diff --git a/integrations/Godot/simulate_godot/Simulate/Commands/step.gd b/integrations/Godot/simulate_godot/Simulate/Commands/step.gd index 3559d2a2..4c7c0436 100644 --- a/integrations/Godot/simulate_godot/Simulate/Commands/step.gd +++ b/integrations/Godot/simulate_godot/Simulate/Commands/step.gd @@ -2,9 +2,35 @@ extends Node # Step through the simulation (also steps the physics!) -signal callback +var frame_skip: int +var time_step: float +var return_frames: bool +var return_nodes: bool -func execute(_content: Variant) -> void: - get_tree().paused = false - emit_signal("callback", PackedByteArray([97, 99, 107])) +func execute(content: Variant) -> void: + if content.get("frame_skip", 1): + Simulator.get_tree().paused = false + if content.has("action"): + for actor in Simulator.actors: + actor.step(content.get("action")) + var event_data = { + "actor_sensor_buffers": { + "raycast_sensor": { + "type": "float", + "shape": [1, 1, 4], + "floatBuffer": [0.0, 0.0, 0.0, 0.0] + } + }, + "actor_reward_buffer": { + "type": "float", + "shape": 1, + "floatBuffer": [0.0] + }, + "actor_done_buffer": { + "type": "uint8", + "shape": 1, + "uintBuffer": [0] + } + } + Simulator.send_callback(str(event_data)) diff --git a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_actuator.gd b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_actuator.gd index 86049867..411b096d 100644 --- a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_actuator.gd +++ b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_actuator.gd @@ -1,5 +1,5 @@ -extends Node3D class_name HFActuator +extends Node3D var action_mapping: ActionMapping @@ -8,18 +8,18 @@ var n: int var dtype: String var low: Array var high: Array -var shape: Array +var shape: Array var actuator_tag: String +var is_actor: bool func import(state: GLTFState, json: Dictionary, extensions: Dictionary): - print("Importing an actuator.") var actuator: Dictionary = state.json["extensions"]["HF_actuators"]["objects"][extensions["HF_actuators"]["object_id"]] action_space = ActionSpace.new() name = extensions["HF_actuators"]["name"] - var mesh = MeshInstance3D.new() - mesh.mesh = state.meshes[json["mesh"]].mesh.get_mesh() - add_child(mesh) + + if json.has("extras"): + is_actor = json["extras"].get("is_actor", false) position = Vector3( json["translation"][0], @@ -88,12 +88,12 @@ func import(state: GLTFState, json: Dictionary, extensions: Dictionary): class ActionMapping: var action: String - var amplitude: float - var offset: float + var amplitude: float = 1.0 + var offset: float = 0.0 var axis: Vector3 var position: Vector3 - var use_local_coordinates: bool - var is_impulse: bool + var use_local_coordinates: bool = true + var is_impulse: bool = false var max_velocity_threshold: float diff --git a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_articulation_body.gd b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_articulation_body.gd index c83b697d..6fa29cb4 100644 --- a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_articulation_body.gd +++ b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_articulation_body.gd @@ -1,5 +1,5 @@ -extends Generic6DOFJoint3D class_name HFArticulationBody +extends Generic6DOFJoint3D var joint_type: String = "" @@ -20,10 +20,10 @@ var upper_limit: float = 0.0 var lower_limit: float = 0.0 var mass: float = 1.0 var center_of_mass: Vector3 = Vector3.ZERO +var actuator: HFActuator func import(state: GLTFState, json: Dictionary, extensions: Dictionary): - print("Importing an articulation body.") var articulation_body: Dictionary = state.json["extensions"]["HF_articulation_bodies"]["objects"][extensions["HF_articulation_bodies"]["object_id"]] name = extensions["HF_articulation_bodies"]["name"] diff --git a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_collider.gd b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_collider.gd index f40a1fa7..ea7a5bca 100644 --- a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_collider.gd +++ b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_collider.gd @@ -1,5 +1,5 @@ -extends StaticBody3D class_name HFCollider +extends StaticBody3D var type: GLTFEnums.ColliderType = GLTFEnums.ColliderType.box @@ -8,10 +8,10 @@ var offset: Vector3 = Vector3.ZERO var intangible: bool = false var convex: bool = false var physic_material: HFPhysicMaterial +var actuator: HFActuator func import(state: GLTFState, json: Dictionary, extensions: Dictionary): - print("Importing a collider.") var collider: Dictionary = state.json["extensions"]["HF_colliders"]["objects"][extensions["HF_colliders"]["object_id"]] name = extensions["HF_colliders"]["name"] diff --git a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_extensions.gd b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_extensions.gd index e73940ca..8fa52f8c 100644 --- a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_extensions.gd +++ b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_extensions.gd @@ -1,5 +1,5 @@ -extends GLTFDocumentExtension class_name HFExtensions +extends GLTFDocumentExtension func _import_node(state: GLTFState, _gltf_node: GLTFNode, json: Dictionary, node: Node): @@ -7,33 +7,26 @@ func _import_node(state: GLTFState, _gltf_node: GLTFNode, json: Dictionary, node if not json.has("extensions"): return OK - if extensions.has("HF_actuators"): - var actuator = HFActuator.new() - actuator.import(state, json, extensions) - node.replace_by(actuator) + var new_node + if extensions.has("HF_rigid_bodies"): + new_node = HFRigidBody.new() if extensions.has("HF_articulation_bodies"): - var articulation_body = HFArticulationBody.new() - articulation_body.import(state, json, extensions) - node.replace_by(articulation_body) + new_node = HFArticulationBody.new() if extensions.has("HF_colliders"): - var collider = HFCollider.new() - collider.import(state, json, extensions) - node.replace_by(collider) + new_node = HFCollider.new() if extensions.has("HF_raycast_sensors"): - var raycast_sensor = HFRaycastSensor.new() - raycast_sensor.import(state, json, extensions) - node.replace_by(raycast_sensor) + new_node = HFRaycastSensor.new() if extensions.has("HF_reward_functions"): - var reward_function = HFRewardFunction.new() - reward_function.import(state, json, extensions) - node.replace_by(reward_function) - if extensions.has("HF_rigid_bodies"): - var rigid_body = HFRigidBody.new() - rigid_body.import(state, json, extensions) - node.replace_by(rigid_body) + new_node = HFRewardFunction.new() if extensions.has("HF_state_sensors"): - var state_sensor = HFStateSensor.new() - state_sensor.import(state, json, extensions) - node.replace_by(state_sensor) + new_node = HFStateSensor.new() + if new_node: + new_node.import(state, json, extensions) + if extensions.has("HF_actuators") and "actuator" in new_node: + new_node.actuator = HFActuator.new() + new_node.actuator.import(state, json, extensions) + node.replace_by(new_node) + else: + print("Extension not implemented.") return OK diff --git a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_physic_material.gd b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_physic_material.gd index c483a6ff..93470289 100644 --- a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_physic_material.gd +++ b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_physic_material.gd @@ -1,15 +1,15 @@ -extends PhysicsMaterial class_name HFPhysicMaterial +extends PhysicsMaterial var material_name: String = "" var dynamic_friction: float = 0.6 var friction_combine: GLTFEnums.PhysicMaterialCombine var bounce_combine: GLTFEnums.PhysicMaterialCombine +var actuator: HFActuator func import(state: GLTFState, extensions: Dictionary, id: int): - print("Importing a physic material.") var physic_material: Dictionary = state.json["extensions"]["HF_physic_materials"]["objects"][id] material_name = extensions["HF_physic_materials"]["name"] friction = 0.6 diff --git a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_raycast_sensor.gd b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_raycast_sensor.gd index 8e7ceb8f..304fa857 100644 --- a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_raycast_sensor.gd +++ b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_raycast_sensor.gd @@ -1,5 +1,5 @@ -extends Node3D class_name HFRaycastSensor +extends Node3D var n_horizontal_rays: int @@ -11,7 +11,6 @@ var sensor_tag: String func import(state: GLTFState, json: Dictionary, extensions: Dictionary): - print("Importing a raycast sensor.") var raycast_sensor: Dictionary = state.json["extensions"]["HF_raycast_sensors"]["objects"][extensions["HF_raycast_sensors"]["object_id"]] name = extensions["HF_raycast_sensors"]["name"] diff --git a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_reward_function.gd b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_reward_function.gd index b7c651fe..91cd3723 100644 --- a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_reward_function.gd +++ b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_reward_function.gd @@ -1,5 +1,5 @@ -extends Node3D class_name HFRewardFunction +extends Node3D var type: String @@ -12,10 +12,10 @@ var threshold: float var is_terminal: bool var is_collectable: bool var trigger_once: bool +var actuator: HFActuator func import(state: GLTFState, json: Dictionary, extensions: Dictionary): - print("Importing a reward function.") var reward_function: Dictionary = state.json["extensions"]["HF_reward_functions"]["objects"][extensions["HF_reward_functions"]["object_id"]] name = extensions["HF_reward_functions"]["name"] diff --git a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_rigid_body.gd b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_rigid_body.gd index 23827449..69f876c9 100644 --- a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_rigid_body.gd +++ b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_rigid_body.gd @@ -1,14 +1,19 @@ -extends RigidBody3D class_name HFRigidBody +extends RigidBody3D var constraints: Array +var actuator: HFActuator func import(state: GLTFState, json: Dictionary, extensions: Dictionary): - print("Importing a rigid body.") var rigid_body: Dictionary = state.json["extensions"]["HF_rigid_bodies"]["objects"][extensions["HF_rigid_bodies"]["object_id"]] - name = extensions["HF_rigid_bodies"]["name"] + name = json["name"] + + var mesh = MeshInstance3D.new() + mesh.mesh = state.meshes[json["mesh"]].mesh.get_mesh() + mesh.name = json["name"] + "_mesh" + add_child(mesh) position = Vector3( json["translation"][0], diff --git a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_state_sensor.gd b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_state_sensor.gd index 01f2629b..2d19ec0f 100644 --- a/integrations/Godot/simulate_godot/Simulate/GLTF/hf_state_sensor.gd +++ b/integrations/Godot/simulate_godot/Simulate/GLTF/hf_state_sensor.gd @@ -1,5 +1,5 @@ -extends Node3D class_name HFStateSensor +extends Node3D var reference_entity: String @@ -9,7 +9,6 @@ var sensor_tag: String func import(state: GLTFState, json: Dictionary, extensions: Dictionary): - print("Importing a state sensor.") var state_sensor: Dictionary = state.json["extensions"]["HF_state_sensors"]["objects"][extensions["HF_state_sensors"]["object_id"]] name = extensions["HF_state_sensors"]["name"] diff --git a/integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/camera_sensor.gd b/integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/camera_sensor.gd new file mode 100644 index 00000000..0e9db831 --- /dev/null +++ b/integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/camera_sensor.gd @@ -0,0 +1,11 @@ +extends Camera3D + + +# Called when the node enters the scene tree for the first time. +func _ready(): + pass # Replace with function body. + + +# Called every frame. 'delta' is the elapsed time since the previous frame. +func _process(delta): + pass diff --git a/integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/raycast_sensor.gd b/integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/raycast_sensor.gd new file mode 100644 index 00000000..dbce4dbd --- /dev/null +++ b/integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/raycast_sensor.gd @@ -0,0 +1,9 @@ +extends Sensor + + +func _ready(): + pass # Replace with function body. + + +func _process(delta): + pass diff --git a/integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/sensor.gd b/integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/sensor.gd new file mode 100644 index 00000000..dd7c60c0 --- /dev/null +++ b/integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/sensor.gd @@ -0,0 +1,2 @@ +class_name Sensor +extends Node3D diff --git a/integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/state_sensor.gd b/integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/state_sensor.gd new file mode 100644 index 00000000..d2497676 --- /dev/null +++ b/integrations/Godot/simulate_godot/Simulate/RLActors/Sensors/state_sensor.gd @@ -0,0 +1,11 @@ +extends Sensor + + +# Called when the node enters the scene tree for the first time. +func _ready(): + pass # Replace with function body. + + +# Called every frame. 'delta' is the elapsed time since the previous frame. +func _process(delta): + pass diff --git a/integrations/Godot/simulate_godot/Simulate/RLActors/actions.gd b/integrations/Godot/simulate_godot/Simulate/RLActors/actions.gd new file mode 100644 index 00000000..89a858a0 --- /dev/null +++ b/integrations/Godot/simulate_godot/Simulate/RLActors/actions.gd @@ -0,0 +1,33 @@ +class_name Actions +extends Node + + +func execute_action(actor, action) -> void: + pass + +func add_force(actor, value, mapping) -> void: + pass + +func add_relative_force(actor, value, mapping) -> void: + pass + +func add_torque(actor, value, mapping) -> void: + pass + +func add_relative_torque(actor, value, mapping) -> void: + pass + +func add_force_at_position(actor, value, mapping) -> void: + pass + +func move_position(actor, value, mapping) -> void: + pass + +func move_relative_position(actor, value, mapping) -> void: + pass + +func move_rotation(actor, value, mapping) -> void: + pass + +func move_relative_rotation(actor, value, mapping) -> void: + pass diff --git a/integrations/Godot/simulate_godot/Simulate/RLActors/actor.gd b/integrations/Godot/simulate_godot/Simulate/RLActors/actor.gd new file mode 100644 index 00000000..287c419a --- /dev/null +++ b/integrations/Godot/simulate_godot/Simulate/RLActors/actor.gd @@ -0,0 +1,81 @@ +class_name Actor +extends Node3D + + +var node: Node3D +var sensors: Array +var observations: Dictionary +var actuators: Dictionary +var reward_functions: Array +var accum_reward: float +var current_action: Dictionary + + +func _init(actor_node: Node): + node = actor_node + initialize() + + +func initialize() -> void: + for child in Simulator.get_all_children(node): + if child is HFActuator: + actuators[child.actuator_tag] = child + if child is Camera3D or child is HFStateSensor or child is HFRaycastSensor: + sensors.append(child) + if child is HFRewardFunction: + reward_functions.append(child) + + +func step(action: Dictionary) -> void: + current_action = action + var action_value = current_action.get("actuator") + + +func enable_sensors(): + for sensor in sensors: + sensor.enable() + + +func disable_sensors(): + for sensor in sensors: + sensor.disable() + + +func get_sensor_observations() -> Dictionary: + return {} + + +func update_reward() -> void: + accum_reward += calculate_reward() + + +func reset() -> void: + accum_reward = 0.0 + for reward_function in reward_functions: + reward_function.reset() + + +func calculate_reward() -> float: + var reward = 0.0 + for reward_function in reward_functions: + reward += reward_function.calculate_reward() + return reward + + +func get_reward() -> float: + return accum_reward + + +func zero_reward() -> void: + accum_reward = 0.0 + + +func is_done() -> bool: + var done = false + return done + + +class Data: + var done: bool + var reward: float + var frames: Dictionary diff --git a/integrations/Godot/simulate_godot/Simulate/RLActors/map.gd b/integrations/Godot/simulate_godot/Simulate/RLActors/map.gd new file mode 100644 index 00000000..5f1f40ae --- /dev/null +++ b/integrations/Godot/simulate_godot/Simulate/RLActors/map.gd @@ -0,0 +1,20 @@ +class_name Map +extends Node3D + + +var bounds +var active: bool +var root: Node +var actors: Dictionary + + +func _init(): + pass + + +func init_map_sensors(): + pass + + +func reset(): + pass diff --git a/integrations/Godot/simulate_godot/Simulate/RLAgents/reward_function.gd b/integrations/Godot/simulate_godot/Simulate/RLActors/reward_function.gd similarity index 91% rename from integrations/Godot/simulate_godot/Simulate/RLAgents/reward_function.gd rename to integrations/Godot/simulate_godot/Simulate/RLActors/reward_function.gd index 5540abbf..de4faa22 100644 --- a/integrations/Godot/simulate_godot/Simulate/RLAgents/reward_function.gd +++ b/integrations/Godot/simulate_godot/Simulate/RLActors/reward_function.gd @@ -2,8 +2,8 @@ class_name RewardFunction extends Node -var entity_a: SimulationNode -var entity_b: SimulationNode +var entity_a: Node3D +var entity_b: Node3D var reward_scalar: float = 1.0 var distance_metric: DistanceMetric @@ -11,26 +11,27 @@ var distance_metric: DistanceMetric func reset() -> void: pass + func calculate_reward() -> float: return 0.0 class DistanceMetric: - func Calculate(e1: SimulationNode, e2: SimulationNode) -> float: + func Calculate(e1: Node3D, e2: Node3D) -> float: return 0.0 class EuclideanDistance: extends DistanceMetric - func Calculate(e1: SimulationNode, e2: SimulationNode) -> float: + func Calculate(e1: Node3D, e2: Node3D) -> float: return e1.position.distance_to(e2.position) class CosineDistance: extends DistanceMetric - func Calculate(e1: SimulationNode, e2: SimulationNode) -> float: + func Calculate(e1: Node3D, e2: Node3D) -> float: return e1.position.distance_to(e2.position) diff --git a/integrations/Godot/simulate_godot/Simulate/RLAgents/agent.gd b/integrations/Godot/simulate_godot/Simulate/RLAgents/agent.gd deleted file mode 100644 index 0a7bbc23..00000000 --- a/integrations/Godot/simulate_godot/Simulate/RLAgents/agent.gd +++ /dev/null @@ -1,59 +0,0 @@ -class_name Agent -extends Node - - -var node: SimulationNode -var action_space -var observations -var reward_functions -var accum_reward: float -var current_action - - -func _init(node: SimulationNode): - self.node = node - -func initialize() -> void: - pass - -func handle_intermediate_frame() -> void: - pass - -func step(action) -> void: - pass - -func get_event_data() -> Data: - return Data.new() - -func agent_update() -> void: - pass - -func get_camera_observations() -> Dictionary: - return {} - -func update_reward() -> void: - pass - -func reset() -> void: - pass - -func calculate_reward() -> float: - return 0.0 - -func get_reward() -> float: - return 0.0 - -func zero_reward() -> float: - return 0.0 - -func is_done() -> bool: - return false - -func try_get_reward_function(reward, reward_function) -> bool: - return true - - -class Data: - var done: bool - var reward: float - var frames: Dictionary diff --git a/integrations/Godot/simulate_godot/Simulate/RLAgents/agent_manager.gd b/integrations/Godot/simulate_godot/Simulate/RLAgents/agent_manager.gd deleted file mode 100644 index 9b571ec3..00000000 --- a/integrations/Godot/simulate_godot/Simulate/RLAgents/agent_manager.gd +++ /dev/null @@ -1,25 +0,0 @@ -class_name AgentManager -extends Node - - -var instance: AgentManager -var agents: Dictionary - - -func _init(): - pass - -func on_scene_initialized(kwargs: Dictionary) -> void: - pass - -func on_before_step(event_data) -> void: - pass - -func on_step(event_data) -> void: - pass - -func on_reset() -> void: - pass - -func on_before_scene_unloaded() -> void: - pass diff --git a/integrations/Godot/simulate_godot/Simulate/RLAgents/rl_action.gd b/integrations/Godot/simulate_godot/Simulate/RLAgents/rl_action.gd deleted file mode 100644 index b28e5b34..00000000 --- a/integrations/Godot/simulate_godot/Simulate/RLAgents/rl_action.gd +++ /dev/null @@ -1,33 +0,0 @@ -class_name RLAction -extends Node - - -func execute_action(agent, action) -> void: - pass - -func add_force(agent, value, mapping) -> void: - pass - -func add_relative_force(agent, value, mapping) -> void: - pass - -func add_torque(agent, value, mapping) -> void: - pass - -func add_relative_torque(agent, value, mapping) -> void: - pass - -func add_force_at_position(agent, value, mapping) -> void: - pass - -func move_position(agent, value, mapping) -> void: - pass - -func move_relative_position(agent, value, mapping) -> void: - pass - -func move_rotation(agent, value, mapping) -> void: - pass - -func move_relative_rotation(agent, value, mapping) -> void: - pass diff --git a/integrations/Godot/simulate_godot/Simulate/config.gd b/integrations/Godot/simulate_godot/Simulate/config.gd new file mode 100644 index 00000000..801c405a --- /dev/null +++ b/integrations/Godot/simulate_godot/Simulate/config.gd @@ -0,0 +1,30 @@ +extends Node + + +var time_step: float = 0.02 +var frame_skip: int = 1 +var return_nodes: bool = true +var return_frames: bool = true +var node_filter: Array +var camera_filter: Array +var ambient_color: Color = Color.GRAY +var gravity: Vector3 = Vector3.DOWN * 9.81 + + +func parse(config: Dictionary): + time_step = config.get("time_step", 0.02) + frame_skip = config.get("frame_skip", 1) + return_nodes = config.get("return_nodes", true) + return_frames = config.get("return_frames", true) + node_filter = config.get("node_filter", []) + camera_filter = config.get("camera_filter", []) + var color_array = config.get("ambient_color") + if color_array: + ambient_color = Color(color_array[0], color_array[1], color_array[2]) + else: + ambient_color = Color.GRAY + var gravity_array = config.get("gravity") + if gravity_array: + gravity = Vector3(gravity_array[0], gravity[1], gravity[2]) + else: + gravity = Vector3.DOWN * 9.81 diff --git a/integrations/Godot/simulate_godot/Simulate/rl_manager.gd b/integrations/Godot/simulate_godot/Simulate/rl_manager.gd new file mode 100644 index 00000000..a527bef8 --- /dev/null +++ b/integrations/Godot/simulate_godot/Simulate/rl_manager.gd @@ -0,0 +1,13 @@ +class_name RLManager +extends Node + + +var root +var actors + + +func load_nodes(): + root = Simulator.get_node("Scene/scene_00") + for child in Simulator.get_all_children(root): + if "is_actor" in child and child.is_actor: + actors.append(Actor.new(child)) diff --git a/integrations/Godot/simulate_godot/Simulate/simulation_node.gd b/integrations/Godot/simulate_godot/Simulate/simulation_node.gd deleted file mode 100644 index 0a25bcc8..00000000 --- a/integrations/Godot/simulate_godot/Simulate/simulation_node.gd +++ /dev/null @@ -1,48 +0,0 @@ -class_name SimulationNode -extends Node3D - - -var camera_data -var light_data -var collider_data -var rigidbody_data -var joint_data -var agent_data - -var camera -var light -var collider -var rigidbody -var joint -var data - - -func initialize() -> void: - pass - -func reset_state() -> void: - pass - -func initialize_camera() -> void: - pass - -func initiliaze_light() -> void: - pass - -func initialize_collider() -> void: - pass - -func initialize_rigidbody() -> void: - pass - -func initialize_joint() -> void: - pass - -func get_data(): - pass - - -class Data: - var name: String - var position: Vector3 - var rotation: Quaternion diff --git a/integrations/Godot/simulate_godot/Simulate/simulator.gd b/integrations/Godot/simulate_godot/Simulate/simulator.gd index 463effaa..a54b3e82 100644 --- a/integrations/Godot/simulate_godot/Simulate/simulator.gd +++ b/integrations/Godot/simulate_godot/Simulate/simulator.gd @@ -1,4 +1,4 @@ -extends Node +extends Node3D # Core of the integration and base of the scene # Handles data received from the client and call commands @@ -10,7 +10,8 @@ const RECONNECT_TIMEOUT: float = 3.0 var _client : Client = Client.new() var _command : Command = Command.new() -var agent +var rl_manager: RLManager +var map_pool: Array func _ready() -> void: @@ -19,22 +20,31 @@ func _ready() -> void: _client.connect("disconnected", _handle_client_disconnected) _client.connect("error", _handle_client_error) _client.connect("data", _handle_client_data) - _command.connect("callback", _handle_callback) - + + _client.name = "Client" add_child(_client) - add_child(_command) _command.load_commands() _client.connect_to_host(HOST, PORT) + +func get_all_children(node: Node, arr: Array = []): + arr.push_back(node) + for child in node.get_children(): + arr = get_all_children(child,arr) + return arr + + func _connect_after_timeout(timeout: float) -> void: # Retry connection after given timeout await get_tree().create_timer(timeout).timeout _client.connect_to_host(HOST, PORT) + func _handle_client_connected() -> void: print("Client connected to server.") + func _handle_client_data(data: PackedByteArray) -> void: # Parse data and sends content for command execution var str_data : String = data.get_string_from_utf8() @@ -52,17 +62,21 @@ func _handle_client_data(data: PackedByteArray) -> void: else: print("Error parsing data.") + func _handle_client_disconnected() -> void: # Reconnect client if it gets disconnected print("Client disconnected from server.") _connect_after_timeout(RECONNECT_TIMEOUT) + func _handle_client_error() -> void: # Reconnect client if there is an error print("Client error.") _connect_after_timeout(RECONNECT_TIMEOUT) -func _handle_callback(callback_data: PackedByteArray) -> void: - # Send callback to python-side API - print("Sending callback.") - _client.send(callback_data) + +func send_callback(callback: String) -> void: + var callback_bytes = Marshalls.base64_to_raw(Marshalls.utf8_to_base64(callback)) + print("Callback length: ", len(callback_bytes)) + _client.send(callback_bytes) + print("Callback sent.") diff --git a/integrations/Godot/simulate_godot/project.godot b/integrations/Godot/simulate_godot/project.godot index 7f762ae6..4d84f083 100644 --- a/integrations/Godot/simulate_godot/project.godot +++ b/integrations/Godot/simulate_godot/project.godot @@ -10,14 +10,14 @@ config_version=5 _global_script_classes=[{ "base": "Node", -"class": &"Agent", +"class": &"Actions", "language": &"GDScript", -"path": "res://Simulate/RLAgents/agent.gd" +"path": "res://Simulate/RLActors/actions.gd" }, { -"base": "Node", -"class": &"AgentManager", +"base": "Node3D", +"class": &"Actor", "language": &"GDScript", -"path": "res://Simulate/RLAgents/agent_manager.gd" +"path": "res://Simulate/RLActors/actor.gd" }, { "base": "Node", "class": &"Client", @@ -79,10 +79,15 @@ _global_script_classes=[{ "language": &"GDScript", "path": "res://Simulate/GLTF/hf_state_sensor.gd" }, { +"base": "Node3D", +"class": &"Map", +"language": &"GDScript", +"path": "res://Simulate/RLActors/map.gd" +}, { "base": "Node", -"class": &"RLAction", +"class": &"RLManager", "language": &"GDScript", -"path": "res://Simulate/RLAgents/rl_action.gd" +"path": "res://Simulate/rl_manager.gd" }, { "base": "Node", "class": &"RenderCamera", @@ -92,16 +97,16 @@ _global_script_classes=[{ "base": "Node", "class": &"RewardFunction", "language": &"GDScript", -"path": "res://Simulate/RLAgents/reward_function.gd" +"path": "res://Simulate/RLActors/reward_function.gd" }, { "base": "Node3D", -"class": &"SimulationNode", +"class": &"Sensor", "language": &"GDScript", -"path": "res://Simulate/simulation_node.gd" +"path": "res://Simulate/RLActors/Sensors/sensor.gd" }] _global_script_class_icons={ -"Agent": "", -"AgentManager": "", +"Actions": "", +"Actor": "", "Client": "", "Command": "", "GLTFEnums": "", @@ -114,10 +119,11 @@ _global_script_class_icons={ "HFRewardFunction": "", "HFRigidBody": "", "HFStateSensor": "", -"RLAction": "", +"Map": "", +"RLManager": "", "RenderCamera": "", "RewardFunction": "", -"SimulationNode": "" +"Sensor": "" } [application] @@ -130,3 +136,8 @@ config/icon="res://icon.png" [autoload] Simulator="*res://Simulate/simulator.gd" +Config="*res://Simulate/config.gd" + +[rendering] + +renderer/rendering_method="gl_compatibility" diff --git a/src/simulate/engine/godot_engine.py b/src/simulate/engine/godot_engine.py index 6dd6fdc6..da89c8fb 100644 --- a/src/simulate/engine/godot_engine.py +++ b/src/simulate/engine/godot_engine.py @@ -201,6 +201,14 @@ def step(self, action: Optional[Dict] = None, **kwargs: Any) -> Union[Dict, str] kwargs.update({"action": action}) return self.run_command("step", **kwargs) + def step_send_async(self, **kwargs: Any): + """Send the Step command asynchronously.""" + self.run_command_async("step", **kwargs) + + def step_recv_async(self) -> str: + """Receive the response from the Step command asynchronously.""" + return self.get_response_async() + def reset(self) -> Union[Dict, str]: """ Reset the environment.