adding Emily/Edward's BoschPendulum scripts

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

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

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

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)

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