diff --git a/common/BUILD b/common/BUILD index 761bc23..89fee11 100644 --- a/common/BUILD +++ b/common/BUILD @@ -4,6 +4,7 @@ py_library( srcs = [ "__init__.py", "utils.py", + "logging.py", ], visibility = ["//visibility:public"], ) diff --git a/common/__init__.py b/common/__init__.py index e12b409..49a7e62 100644 --- a/common/__init__.py +++ b/common/__init__.py @@ -1 +1,2 @@ from common.utils import * +from common.logging import * diff --git a/common/logging.py b/common/logging.py new file mode 100644 index 0000000..aa1e378 --- /dev/null +++ b/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) diff --git a/common/utils.py b/common/utils.py index 871b543..fdb3407 100644 --- a/common/utils.py +++ b/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) + ) diff --git a/kinova_gen3/gen3_interface.py b/kinova_gen3/gen3_interface.py index 39ccebc..dbd0f30 100644 --- a/kinova_gen3/gen3_interface.py +++ b/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 diff --git a/main/BUILD b/main/BUILD index 8c76555..49718f9 100644 --- a/main/BUILD +++ b/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( diff --git a/main/hardware_demo.py b/main/hardware_demo.py index acb888d..b09d80b 100644 --- a/main/hardware_demo.py +++ b/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) diff --git a/main/perception_demo.py b/main/perception_demo.py index 2dbbf27..51b0668 100644 --- a/main/perception_demo.py +++ b/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) diff --git a/main/porosity_demo.py b/main/porosity_demo.py index ff72ae8..5801316 100644 --- a/main/porosity_demo.py +++ b/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) diff --git a/main/teleop_demo.py b/main/teleop_demo.py index 575b79b..b46de44 100644 --- a/main/teleop_demo.py +++ b/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) diff --git a/planning/BUILD b/planning/BUILD index d2d3335..d2116a1 100644 --- a/planning/BUILD +++ b/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",], -) \ No newline at end of file + deps = ["//common"], +) diff --git a/planning/porosity_planner.py b/planning/porosity_planner.py index 04026ba..fa413c4 100644 --- a/planning/porosity_planner.py +++ b/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) diff --git a/planning/suction_grasp.py b/planning/suction_grasp.py index dc034ec..d39b79d 100644 --- a/planning/suction_grasp.py +++ b/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): diff --git a/robotiq_2f_85/robotiq_2f_85_sim_driver.py b/robotiq_2f_85/robotiq_2f_85_sim_driver.py index 1f2eda1..51cd297 100644 --- a/robotiq_2f_85/robotiq_2f_85_sim_driver.py +++ b/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() diff --git a/station/station.py b/station/station.py index c287b9e..356c90e 100644 --- a/station/station.py +++ b/station/station.py @@ -16,6 +16,8 @@ import numpy as np +from common.logging import * + def MakeHardwareStation( scenario: Scenario, @@ -23,10 +25,13 @@ def MakeHardwareStation( ) -> Diagram: builder = DiagramBuilder() + logging.info(f"Creating Hardware Station") if scenario.hardware_interface: scenario.plant_config.time_step = 0.0005 scenario.visualization.publish_period = 0.0005 + # Create the multibody plant and scene graph. + logging.debug(f"Adding Multibody Plant") sim_plant: MultibodyPlant sim_plant, scene_graph = AddMultibodyPlant( config=scenario.plant_config, builder=builder @@ -43,6 +48,7 @@ def MakeHardwareStation( sim_plant.Finalize() if scenario.hardware_interface: + logging.info(f"Hardware interface exists in scenario, creating interface") # Set the actuation of the sim plant to 0 # zero_actuation_sys = builder.AddSystem(ConstantVectorSource(np.zeros(9))) # builder.Connect( @@ -50,11 +56,10 @@ def MakeHardwareStation( # ) return MakeHardwareStationInterface(builder, scenario, meshcat, sim_plant) - # tf = sim_plant.CalcRelativeTransform(sim_plant.CreateDefaultContext(), sim_plant.world_frame(), sim_plant.GetFrameByName('image_frame', sim_plant.GetModelInstanceByName('camera4'))) - # print(list(np.concatenate([tf.translation(), RollPitchYaw(tf.rotation()).vector()]))) lcm_buses = None + logging.debug(f"Applying Driver Configs") # Add actuation inputs. ApplyDriverConfigs( driver_configs=scenario.model_drivers, @@ -84,6 +89,7 @@ def MakeHardwareStation( # Add scene cameras. for camera_name, camera in scenario.cameras.items(): + logging.debug(f"Adding Simulated Camera: {camera_name}") ApplyCameraConfig(config=camera, builder=builder, lcm_buses=lcm_buses) camera_subsystem: System = builder.GetSubsystemByName( f"rgbd_sensor_{camera_name}" @@ -102,6 +108,7 @@ def MakeHardwareStation( builder.ExportOutput(sim_plant.get_state_output_port(), "plant_continuous_state") builder.ExportOutput(sim_plant.get_body_poses_output_port(), "body_poses") + logging.info(f"Finished Creating Hardware Station") return builder.Build() @@ -149,7 +156,6 @@ def MakeHardwareStationInterface( interface_subsystem = builder.AddNamedSystem( "epick", EPickInterface(hardware_interface) ) - print(hardware_interface) else: raise RuntimeError( f"Invalid hardware interface type {hardware_interface} for model {model_name}" diff --git a/utils/BUILD b/utils/BUILD deleted file mode 100644 index c25e4f9..0000000 --- a/utils/BUILD +++ /dev/null @@ -1,18 +0,0 @@ -package( - default_visibility = ["//visibility:public"], -) - - -py_library( - name = "utils", - srcs = [ - "__init__.py", - "meshcat_utils.py", - "geometry_utils.py", - ], - data = [], - visibility = ["//visibility:public"], - deps = [ - "@drake//bindings/pydrake", - ], -) diff --git a/utils/__init__.py b/utils/__init__.py deleted file mode 100644 index 023bb39..0000000 --- a/utils/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -from piplup.utils.meshcat_utils import * -from piplup.utils.geometry_utils import * diff --git a/utils/geometry_utils.py b/utils/geometry_utils.py deleted file mode 100644 index 9401aed..0000000 --- a/utils/geometry_utils.py +++ /dev/null @@ -1,21 +0,0 @@ -from pydrake.all import * - - -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()) diff --git a/utils/meshcat_utils.py b/utils/meshcat_utils.py deleted file mode 100644 index 8960c61..0000000 --- a/utils/meshcat_utils.py +++ /dev/null @@ -1,45 +0,0 @@ -import numpy as np -from pydrake.all import * -from pydrake.geometry import Meshcat - - -# 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) - )