Use gltf extension in Godot for Actors

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)
 

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. 

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")

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

integrations/Godot/simulate_godot/Simulate/Bridge/command.gd

@@ -2,39 +2,29 @@ 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 == "":
 				break
 			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("{}")

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("{}")

integrations/Godot/simulate_godot/Simulate/Commands/initialize.gd

@@ -2,19 +2,19 @@ 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"])
 
 	var gltf_state : GLTFState = GLTFState.new()
 	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("{}")

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("{}")

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))

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
 
 

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"]
 

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"]
 

integrations/Godot/simulate_godot/Simulate/GLTF/hf_extensions.gd

@@ -1,39 +1,32 @@
-extends GLTFDocumentExtension
 class_name HFExtensions
+extends GLTFDocumentExtension
 
 
 func _import_node(state: GLTFState, _gltf_node: GLTFNode, json: Dictionary, node: Node):
 	var extensions = json.get("extensions")
 	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