From 62193a5078086fe8e5b6ef4d302097cf699d9eca Mon Sep 17 00:00:00 2001 From: John Choi Date: Fri, 19 Apr 2024 18:09:51 -0400 Subject: [PATCH] adding Emily/Edward's BoschPendulum scripts --- examples/BoschPendulum/BoschPendulum.py | 5 +- examples/BoschPendulum/main.py | 133 ++++++++++++++++++++ examples/BoschPendulum/pendulum_physical.py | 43 +++++++ examples/BoschPendulum/test_send.py | 22 ++++ examples/BoschPendulum/utils.py | 28 +++++ 5 files changed, 229 insertions(+), 2 deletions(-) create mode 100644 examples/BoschPendulum/main.py create mode 100644 examples/BoschPendulum/pendulum_physical.py create mode 100644 examples/BoschPendulum/test_send.py create mode 100644 examples/BoschPendulum/utils.py diff --git a/examples/BoschPendulum/BoschPendulum.py b/examples/BoschPendulum/BoschPendulum.py index 129b326..c473db1 100644 --- a/examples/BoschPendulum/BoschPendulum.py +++ b/examples/BoschPendulum/BoschPendulum.py @@ -135,7 +135,7 @@ def _rotatePendulum(self, degrees): # if __name__=="__main__": def end_program_callback(scene: Scene): - printGreenB("Ending Bosch Pendulum Program. Goodbye :)") + printYellowB("Ending Bosch Pendulum Program. Goodbye :)") # command line scene start options scene = Scene(cli_args=True, end_program_callback=end_program_callback) @@ -152,13 +152,14 @@ def end_program_callback(scene: Scene): #Call this function to create a boschPendulum class instance boschPendulum = BoschPendulum(scene, app_position, app_rotation, app_scale) + #Loop interval update timers in Milliseconds: EXAMPLE_DOOR_INTERVAL = 5000 EXAMPLE_CHASSIS_INTERVAL = 50 EXAMPLE_PENDULUM_INTERVAL = 50 @scene.run_once def ProgramStart(): - print("Program start.") + print("Program started.") @scene.run_forever(interval_ms=EXAMPLE_DOOR_INTERVAL) def ExampleDoorUpdate(): diff --git a/examples/BoschPendulum/main.py b/examples/BoschPendulum/main.py new file mode 100644 index 0000000..69e9cc9 --- /dev/null +++ b/examples/BoschPendulum/main.py @@ -0,0 +1,133 @@ +from arena import * + +import numpy as np + +import time +from pendulum_physical import PendulumPhysical + +from utils import * + +import BoschPendulum + +scene = Scene(host="arenaxr.org", namespace = "johnchoi", scene="pendulum") + +grabbing = False + +grabber = None +child_pose_relative_to_parent = None + +orig_position = (0,1.5,-1) +orig_scale = (0.1,0.1,0.1) +grabbed_scale = (0.11,0.11,0.11) + +pendulum = PendulumPhysical() + +BPsimulation = BoschPendulum(scene, Position(0,0,0), Rotation(0,0,0), Scale(1,1,1)) + +def box_click(scene, evt, msg): + global chasis + global grabbing + global grabber + global orig_scale + global child_pose_relative_to_parent + + if evt.type == "mousedown": + clicker = scene.users[evt.data.source] + handRight = clicker.hands.get("handRight", None) + # handLeft = clicker.hands.get("handLeft", None) + + if not grabbing: + print("grabbed") + + if handRight is not None: + grabber = handRight + #print("Grabber: ",grabber) + + grabbing = True + hand_pose = pose_matrix(grabber.data.position, grabber.data.rotation) + print("hand pose ",chasis.data.position, chasis.data.rotation) + child_pose = pose_matrix(chasis.data.position, chasis.data.rotation) + print("child pose") + child_pose_relative_to_parent = get_relative_pose_to_parent(hand_pose, child_pose) + print("End grabbing") + + elif evt.type == "mouseup": + if grabbing: + print("released") + grabbing = False + chasis.update_attributes(scale=orig_scale) + scene.update_object(chasis) + +chasis = Box( + object_id="chasis", + position=orig_position, + scale=orig_scale, + rotation=(1,0,0,0), + color=(50,60,200), + parent=None, + clickable=True, + evt_handler=box_click +) + +arm = Box( + object_id="arm", + position=(0,0,0), + scale=(0.5,7,0.5), + rotation=(1,0,0,0), + color=(50,100,100), + patent=None, + clickable=True, + parent=chasis, + evt_handler=box_click +) + +@scene.run_forever(interval_ms=10) +def move_box(): + global pendulum + global chasis + global grabber + global grabbed_scale + global child_pose_relative_to_parent + + if grabber is not None and child_pose_relative_to_parent is not None and grabbing: + hand_pose = pose_matrix(grabber.data.position, grabber.data.rotation) + new_pose = get_world_pose_when_parented(hand_pose, child_pose_relative_to_parent) + + new_position = (new_pose[0,3], new_pose[1,3], new_pose[2,3]) + new_rotation = Utils.matrix3_to_quat(new_pose[:3,:3]) + new_rotation = (new_rotation[3], new_rotation[0], new_rotation[1], new_rotation[2]) + print("New pos ",new_position[0]) # Virtual position + pendulum.set_position(new_position[0]) + + chasis.update_attributes(position=new_position, scale=grabbed_scale)#, rotation=new_rotation) + scene.update_object(chasis) + print("Finished move box update") + +@scene.run_forever(interval_ms=100) +def update_pendulum(): + global pendulum + global arm + + theta_rad = pendulum.get_rotation() + if theta_rad is not None: + theta_deg = np.degrees(theta_rad) + arm.update_attributes(rotation=(0,0,theta_deg)) + scene.update_object(chasis) + +@scene.run_once +def make_objects(): + global pendulum + pendulum.set_position(0) + scene.add_object(chasis) + scene.add_object(arm) + +# @scene.run_async +# async def update_pendulum(): +# while True: +# pendulum.set_position(.07) +# await scene.sleep(1000) +# pendulum.set_position(-.07) +# await scene.sleep(1000) + +if __name__ == "__main__": + scene.run_tasks() diff --git a/examples/BoschPendulum/pendulum_physical.py b/examples/BoschPendulum/pendulum_physical.py new file mode 100644 index 0000000..3fb3bfc --- /dev/null +++ b/examples/BoschPendulum/pendulum_physical.py @@ -0,0 +1,43 @@ +import paho.mqtt.client as mqtt +import time +import struct + +class PendulumPhysical: + def __init__(self, broker_address="localhost", broker_port=1884): + self.min_position = -0.15 + self.max_position = 0.15 + + self.position = None + self.theta = None + + self.client = mqtt.Client("arena") + self.client.on_connect = self._on_connect + self.client.on_message = self._on_message + self.client.connect(broker_address, broker_port) + self.client.loop_start() + + def _on_connect(self, client, userdata, flags, rc): + if rc == 0: + print("[PendulumPhysical] Connected to *local* MQTT broker") + self.client.subscribe("silverline/pendulum/pos_act") + self.client.subscribe("silverline/pendulum/theta") + else: + print("Failed to connect, return code: ", rc) + + def _on_message(self, client, userdata, msg): + if msg.topic == "silverline/pendulum/pos_act": + self.position = struct.unpack('d', msg.payload)[0] + # print("Position: ", self.position) + elif msg.topic == "silverline/pendulum/theta": + self.theta = struct.unpack('d', msg.payload)[0] + + def _publish(self, topic, payload): + self.client.publish(topic, payload) + + def set_position(self, position): + position = max(self.min_position, min(self.max_position, position)) + print("Pos: ", position) + self._publish("silverline/pendulum/pos_set", struct.pack('d', position)) + + def get_rotation(self): + return self.theta # in radians diff --git a/examples/BoschPendulum/test_send.py b/examples/BoschPendulum/test_send.py new file mode 100644 index 0000000..60667bf --- /dev/null +++ b/examples/BoschPendulum/test_send.py @@ -0,0 +1,22 @@ +import struct +import paho.mqtt.client as mqtt + +broker_address = "localhost" +broker_port = 1883 +topic = "silverline/pendulum/pos_act" + +def publish_double(topic, value): + client = mqtt.Client() + + client.connect(broker_address, broker_port) + + binary_data = struct.pack('d', value) + + client.publish(topic, binary_data) + + client.disconnect() + +if __name__ == "__main__": + value_to_publish = 0.09 + + publish_double(topic, value_to_publish) diff --git a/examples/BoschPendulum/utils.py b/examples/BoschPendulum/utils.py new file mode 100644 index 0000000..6d173f3 --- /dev/null +++ b/examples/BoschPendulum/utils.py @@ -0,0 +1,28 @@ +import numpy as np +from arena.utils import Utils + +def pose_matrix(position, rotation): + position = np.array((position.x, position.y, position.z)) + rotation = np.array((rotation.x, rotation.y, rotation.z, rotation.w)) + scale = np.array((1, 1, 1)) + + rotation_matrix = np.eye(4) + rotation_matrix[:3,:3] = Utils.quat_to_matrix3(rotation) + + scale_matrix = np.diag([scale[0], scale[1], scale[2], 1]) + + translation_matrix = np.eye(4) + translation_matrix[:3, 3] = [position[0], position[1], position[2]] + + pose_matrix = translation_matrix @ rotation_matrix @ scale_matrix + + return pose_matrix + +def get_relative_pose_to_parent(parent_pose, child_pose_world): + parent_pose_inverse = np.linalg.inv(parent_pose) + relative_pose = parent_pose_inverse @ child_pose_world + return relative_pose + +def get_world_pose_when_parented(parent_pose, child_pose_relative_to_parent): + world_pose = parent_pose @ child_pose_relative_to_parent + return world_pose