Merge pull request #14 from olincollege/logging

Added Python Logging

common/BUILD

@@ -4,6 +4,7 @@ py_library(
     srcs = [
         "__init__.py",
         "utils.py",
+        "logging.py",
     ],
     visibility = ["//visibility:public"],
 )

common/__init__.py

@@ -1 +1,2 @@
 from common.utils import *
+from common.logging import *

common/logging.py

@@ -0,0 +1,8 @@
+import logging
+from pydrake.common import configure_logging
+
+
+def init_logging():
+    configure_logging()
+    logging.getLogger().setLevel(logging.DEBUG)
+    logging.getLogger("drake").setLevel(logging.INFO)

common/utils.py

@@ -1,7 +1,69 @@
-from pydrake.multibody.parsing import Parser
+from pydrake.all import *
 
 
 def ConfigureParser(parser: Parser):
     """Add the models/package.xml index to the given Parser."""
     package_xml = "models/package.xml"
     parser.package_map().AddPackageXml(filename=package_xml)
+
+
+class PoseTransform(LeafSystem):
+    def __init__(
+        self,
+        X_BA: RigidTransform = RigidTransform(),
+    ):
+        LeafSystem.__init__(self)
+        self.DeclareAbstractInputPort("pose", AbstractValue.Make(RigidTransform()))
+        self.DeclareAbstractOutputPort(
+            "pose",
+            lambda: AbstractValue.Make(RigidTransform()),
+            self._CalcOutput,
+        )
+        self.X_BA = X_BA
+
+    def _CalcOutput(self, context, output):
+        pose = self.EvalAbstractInput(context, 0).get_value()
+        pose = pose @ self.X_BA
+        output.get_mutable_value().set(pose.rotation(), pose.translation())
+
+
+# Credit to https://github.com/RussTedrake/manipulation
+def AddMeshcatTriad(
+    meshcat: Meshcat,
+    path: str,
+    length: float = 0.25,
+    radius: float = 0.01,
+    opacity: float = 1.0,
+    X_PT: RigidTransform = RigidTransform(),
+):
+    """Adds an X-Y-Z triad to the meshcat scene.
+
+    Args:
+        meshcat: A Meshcat instance.
+        path: The Meshcat path on which to attach the triad. Using relative paths will attach the triad to the path's coordinate system.
+        length: The length of the axes in meters.
+        radius: The radius of the axes in meters.
+        opacity: The opacity of the axes in [0, 1].
+        X_PT: The pose of the triad relative to the path.
+    """
+    meshcat.SetTransform(path, X_PT)
+    # x-axis
+    X_TG = RigidTransform(RotationMatrix.MakeYRotation(np.pi / 2), [length / 2.0, 0, 0])
+    meshcat.SetTransform(path + "/x-axis", X_TG)
+    meshcat.SetObject(
+        path + "/x-axis", Cylinder(radius, length), Rgba(1, 0, 0, opacity)
+    )
+
+    # y-axis
+    X_TG = RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2), [0, length / 2.0, 0])
+    meshcat.SetTransform(path + "/y-axis", X_TG)
+    meshcat.SetObject(
+        path + "/y-axis", Cylinder(radius, length), Rgba(0, 1, 0, opacity)
+    )
+
+    # z-axis
+    X_TG = RigidTransform([0, 0, length / 2.0])
+    meshcat.SetTransform(path + "/z-axis", X_TG)
+    meshcat.SetObject(
+        path + "/z-axis", Cylinder(radius, length), Rgba(0, 0, 1, opacity)
+    )

kinova_gen3/gen3_interface.py

@@ -27,7 +27,8 @@
     Gen3NamedPosition,
     kGen3NamedPositions,
 )
-import logging
+
+from common import *
 
 
 class Gen3InterfaceConfig:
@@ -114,13 +115,13 @@ def __init__(self, ip_address, port, hand_model_name, sim_plant: MultibodyPlant)
         self.base_cyclic = BaseCyclicClient(self.router)
 
         if self.base.GetArmState().active_state != Base_pb2.ARMSTATE_SERVOING_READY:
-            print(self.base.GetArmState())
+            logging.error(self.base.GetArmState())
             raise RuntimeError(
                 "Arm not in ready state. Clear any faults before trying again."
             )
 
     def CleanUp(self):
-        print("\nClosing Hardware Connection...")
+        logging.info("\nClosing Hardware Connection...")
 
         if self.session_manager is not None:
             router_options = RouterClientSendOptions()
@@ -129,7 +130,7 @@ def CleanUp(self):
             self.session_manager.CloseSession()
 
         self.transport.disconnect()
-        print("Hardware Connection Closed.")
+        logging.info("Hardware Connection Closed.")
 
     def GetFeedback(self, current_time):
         """
@@ -151,8 +152,6 @@ def check_for_end_or_abort(self, e):
         """
 
         def check(notification, e=e):
-            # print("EVENT : " + \
-            #       Base_pb2.ActionEvent.Name(notification.action_event))
             if (
                 notification.action_event == Base_pb2.ACTION_END
                 or notification.action_event == Base_pb2.ACTION_ABORT

main/BUILD

@@ -4,7 +4,10 @@ py_binary(
     name = "teleop_demo",
     srcs = ["teleop_demo.py"],
     imports = [".."],
-    deps = ["//station"],
+    deps = [
+        "//station",
+        "//common",
+    ],
 )
 
 py_binary(
@@ -18,7 +21,11 @@ py_binary(
     name = "perception_demo",
     srcs = ["perception_demo.py"],
     imports = [".."],
-    deps = ["//station:station","//planning:planning", "//utils:utils"],
+    deps = [
+        "//planning",
+        "//station",
+        "//common",
+    ],
 )
 
 py_binary(

main/hardware_demo.py

@@ -16,6 +16,7 @@
 )
 
 from kinova_gen3 import Gen3ControlMode
+from common.logging import *
 
 
 def run(*, scenario: Scenario, graphviz=None, teleop=None):
@@ -93,12 +94,13 @@ def run(*, scenario: Scenario, graphviz=None, teleop=None):
     try:
         simulator.AdvanceTo(scenario.simulation_duration)
     except KeyboardInterrupt:
-        print(simulator.get_actual_realtime_rate())
+        logging.info(simulator.get_actual_realtime_rate())
         if hardware_station.HasSubsystemNamed("gen3_interface"):
             hardware_station.GetSubsystemByName("gen3_interface").CleanUp()
 
 
 def main():
+    init_logging()
     parser = argparse.ArgumentParser(
         description="Run teleop demo for real hardware station"
     )
@@ -129,6 +131,7 @@ def main():
         filename=args.scenarios_yaml,
         scenario_name=args.scenario_name,
     )
+    logging.info(f"Running Hardware Demo")
     run(scenario=scenario, graphviz=args.graph_viz, teleop=args.teleop)
 
 

main/perception_demo.py

@@ -16,9 +16,11 @@
 from perception import MakePointCloudGenerator, ImageSegmenter
 import cv2
 
-from piplup.utils import PoseTransform
+from common.utils import PoseTransform
 from planning import SuctionGraspSelector
 
+from common.logging import *
+
 
 def run(*, scenario: Scenario, visualize=False):
     meshcat: Meshcat = StartMeshcat()
@@ -175,20 +177,23 @@ def run(*, scenario: Scenario, visualize=False):
                     break
     except KeyboardInterrupt:
         cv2.destroyAllWindows()
-        print(simulator.get_actual_realtime_rate())
+        logging.info(simulator.get_actual_realtime_rate())
         if hardware_station.HasSubsystemNamed("gen3_interface"):
             hardware_station.GetSubsystemByName("gen3_interface").CleanUp()
 
 
 def main():
+    init_logging()
     parser = argparse.ArgumentParser()
     parser.add_argument("--sim", action="store_true")
     parser.add_argument("-v", action="store_true")
     args = parser.parse_args()
+    scenario_name = "Simulated" if args.sim else "Hardware"
     scenario = load_scenario(
         filename="models/perception_scenarios.yaml",
-        scenario_name="Simulated" if args.sim else "Hardware",
+        scenario_name=scenario_name,
     )
+    logging.info(f"Running {scenario_name} Perception Demo")
     run(scenario=scenario, visualize=args.v)
 
 

main/porosity_demo.py

@@ -12,6 +12,8 @@
 
 from planning import PorosityPlanner
 
+from common.logging import *
+
 
 def run(*, scenario: Scenario):
     meshcat: Meshcat = StartMeshcat()
@@ -62,16 +64,18 @@ def run(*, scenario: Scenario):
     try:
         simulator.AdvanceTo(scenario.simulation_duration)
     except KeyboardInterrupt:
-        print(simulator.get_actual_realtime_rate())
+        logging.info(simulator.get_actual_realtime_rate())
         hardware_station.GetSubsystemByName("gen3_interface").CleanUp()
         hardware_station.GetSubsystemByName("epick").release()
 
 
 def main():
+    init_logging()
     scenario = load_scenario(
         filename="models/porosity_scenario.yaml",
         scenario_name="PorosityDemo",
     )
+    logging.info(f"Running Porosity Demo")
     run(scenario=scenario)
 
 

main/teleop_demo.py

@@ -17,10 +17,13 @@
 from graphviz import Source
 from kinova_gen3 import Gen3ControlMode
 
+from common.logging import *
+
 
 def run(*, scenario: Scenario, graphviz=None):
     meshcat: Meshcat = StartMeshcat()
     builder = DiagramBuilder()
+
     hardware_station: Diagram = builder.AddNamedSystem(
         "hardware_station", MakeHardwareStation(scenario, meshcat)
     )
@@ -92,6 +95,8 @@ def run(*, scenario: Scenario, graphviz=None):
 
 
 def main():
+    init_logging()
+
     parser = argparse.ArgumentParser(
         description="Run teleop demo for simulated hardware station"
     )
@@ -111,6 +116,7 @@ def main():
         filename="models/teleop_scenarios.yaml",
         scenario_name=args.scenario_name,
     )
+    logging.info(f"Running Teleop Demo for Scenario: {args.scenario_name}")
     run(scenario=scenario, graphviz=args.graph_viz)
 
 

planning/BUILD

@@ -3,9 +3,9 @@ py_library(
     srcs = [
         "__init__.py",
         "porosity_planner.py",
-        "suction_grasp.py"
+        "suction_grasp.py",
     ],
     imports = [".."],
     visibility = ["//visibility:public"],
-    deps = ["//utils",],
-)
+    deps = ["//common"],
+)

planning/porosity_planner.py

@@ -5,6 +5,8 @@
 from kinova_gen3 import Gen3ControlMode, Gen3NamedPosition, kGen3NamedPositions
 import time
 
+from common.logging import *
+
 
 class PorosityPlannerState(Enum):
     INIT = auto()
@@ -61,7 +63,7 @@ def change_command_mode(self, state: State, new_mode: Gen3ControlMode):
 
     def Update(self, context: Context, state: State):
         planner_state = context.get_abstract_state(self.planner_state_idx_).get_value()
-        # print(planner_state)
+        logging.debug(f"Current State: {planner_state}")
 
         match planner_state:
             case PorosityPlannerState.INIT:
@@ -118,7 +120,9 @@ def Update(self, context: Context, state: State):
             case PorosityPlannerState.MEASURE_SUCTION:
                 obj_det_status = self.get_input_port(self.obj_det_idx_).Eval(context)
                 pressure = self.get_input_port(self.pressure_idx_).Eval(context)
-                print(f"Pressure: {pressure}kPa \t Object Detection: {obj_det_status} ")
+                logging.info(
+                    f"Pressure: {pressure}kPa \t Object Detection: {obj_det_status} "
+                )
                 self.change_command_mode(state, Gen3ControlMode.kTwist)
                 # TODO this should be in the normal direction
                 state.get_mutable_discrete_state(self.command_idx_).set_value(
@@ -148,10 +152,8 @@ def Update(self, context: Context, state: State):
                 )
 
             case _:
-                print("Invalid Planner State")
+                logging.error("Invalid Planner State")
 
     def CalcGen3Command(self, context: Context, output: BasicVector):
         cmd = context.get_discrete_state(self.command_idx_)
-        print(self.command_idx_)
-
         output.SetFromVector(cmd)

planning/suction_grasp.py

@@ -2,7 +2,7 @@
 from pydrake.all import *
 from pydrake.geometry import Meshcat, StartMeshcat
 
-from piplup.utils import AddMeshcatTriad
+from common.utils import AddMeshcatTriad
 
 
 class SuctionGraspSelector(LeafSystem):

robotiq_2f_85/robotiq_2f_85_sim_driver.py

@@ -3,11 +3,14 @@
 from pydrake.all import *
 from .robotiq_2f_85_constants import *
 import numpy as np
+from common.logging import *
 
 
+# Adapted from https://github.com/vincekurtz/kinova_drake
 class Sim2f85Driver(LeafSystem):
     def __init__(self, plant: MultibodyPlant):
         LeafSystem.__init__(self)
+        logging.debug("Creating Simulated Robotiq 2f85 Gripper Driver")
 
         self.plant = plant
         self.context = self.plant.CreateDefaultContext()