From 2511de08ba4bf1e2c6c91b16ac99df720b84c1a1 Mon Sep 17 00:00:00 2001 From: JustaGist Date: Fri, 9 Aug 2024 15:01:15 +0100 Subject: [PATCH] create additional demos --- .../05_additional_features_and_components.py | 151 ++++++++++++++ .../demo_bullet_robot_visualizer.py | 0 .../demo_plotjuggler_loop_debugger.py | 195 ++++++++++++++++++ 3 files changed, 346 insertions(+) create mode 100644 examples/tutorials/05_additional_features_and_components.py rename examples/{utils_examples => utils_demos}/demo_bullet_robot_visualizer.py (100%) create mode 100644 examples/utils_demos/demo_plotjuggler_loop_debugger.py diff --git a/examples/tutorials/05_additional_features_and_components.py b/examples/tutorials/05_additional_features_and_components.py new file mode 100644 index 0000000..5e29208 --- /dev/null +++ b/examples/tutorials/05_additional_features_and_components.py @@ -0,0 +1,151 @@ +"""Demo showing the use of some implmentations of pyrcf components: joint position controller, +end-effector reference interpolator (local planner), and pybullet GUI for setting target +end-effector poses (implemented as a GlobalPlanner). Also shows usage of "custom callbacks" +and "debugger" components in the control loop. + +1. This demo shows how to load any robot from the Awesome robots list +(https://github.com/robot-descriptions/robot_descriptions.py/tree/main?tab=readme-ov-file#descriptions) +as a PybulletRobot (RobotInterface derivative) instance. + +2. This demo uses a pybullet GUI interface to set end-effector targets. This +is implemented as a `GlobalPlanner` (`UIBase`) called `PybulletGUIGlobalPlannerInterface`. +Sliders and buttons in Pybullet GUI can be used to set end-effector pose target for the robot. + +3. The local planner is `IKReferenceInterpolator` which uses a second order filter +to smoothly reach the joint positions for reaching the end-effector target set using +the GUI sliders. IK is solved using the `PybulletIKInterface`. + +4. The controller is a joint position (and velocity) tracking controller. + +5. Also has an extended variant of this controller that adds gravity compensation torques to the +robot command, making the joint tracking much better. It compensates for the robot's gravity +torques by computing them using Pinocchio and sending the negative torques as additional control +commands. + +** Loop debuggers ** +Control Loop Debuggers are components that can intercept and read data passed by +the components in the control loop. These components read data (WILL/SHOULD NOT modify) +and can be used for visualising/debugging etc. + + +6. There are two debuggers used in this example: the `PlotjugglerLoopDebugger` +is an implementation of the CtrlLoopDebuggerBase class, and intercepts all data +to publish them so that the data can be visualised in plotjuggler. Use plotjuggler to +visualise data published by this debugger (ZMQ subscriber; msg protocol=json).There is +also a debug robot in the simulation (using `PybulletRobotPlanDebugger`, which shows a +shadow robot in sim) that shows the actual output from the planner (useful for controller +tuning/debugging). + +NOTE: More usage functionalities of the `PlotjugglerLoopDebugger` is demonstrated in the +example: `examples/demos/utils_demos/demo_plotjuggler_loop_debugger.py`. + +Running the demo: +Run the code. A pybullet window should open with a robot in the scene and sliders in the console. +Use sliders to choose end-effector target, and button to send the goal to the local planner. +By default, the controller loaded is the JointPDController without gravity compensation. +Try out the gravity compensated controller to see the difference in tracking with the same +PD gains. +Controllers can be switched by setting `USE_GRAVITY_COMP_CONTROLLER` to True or False. +""" + +from pybullet_robot import PybulletIKInterface + +from pyrcf.components.robot_interfaces.simulation import PybulletRobot +from pyrcf.control_loop import MinimalCtrlLoop +from pyrcf.components.local_planners import IKReferenceInterpolator +from pyrcf.components.global_planners.ui_reference_generators import ( + PybulletGUIGlobalPlannerInterface, +) +from pyrcf.components.controllers import JointPDController, GravityCompensatedPDController +from pyrcf.components.ctrl_loop_debuggers import PlotjugglerLoopDebugger, PybulletRobotPlanDebugger + +# pylint: disable=W0212 + +USE_GRAVITY_COMP_CONTROLLER: bool = True +"""Set this flag to True to use a PD controller with gravity compensation. +If set to False, will use a regular PD joint position tracking controller. +Try enabling and disabling to see difference in tracking.""" + +if __name__ == "__main__": + # load a manipulator robot from AwesomeRobotDescriptions (can load any robot + # from the awesome robot lists using the `fromAwesomeRobotDescriptions` class + # method. + # https://github.com/robot-descriptions/robot_descriptions.py/tree/main?tab=readme-ov-file#descriptions) + robot: PybulletRobot = PybulletRobot.fromAwesomeRobotDescriptions( + robot_description_name="iiwa14_description", + floating_base=False, + place_on_ground=False, + # if `ee_names` is not defined here, will have to be provided as argument to + # `PybulletGUIGlobalPlannerInterface` below. But setting it here is better as + # it also adds FK for this end-effector in the state estimates for this + # robot, which can be used as starting values for EE sliders + ee_names=["iiwa_link_ee"], + ) + + state = robot.read() + + # create a "global planner" to provide end-effector targets using pybullet sliders + global_planner = PybulletGUIGlobalPlannerInterface( + enable_joint_sliders=False, + cid=robot._sim_robot.cid, + enable_ee_sliders=True, + # ee_names=["iiwa_link_ee"], # not needed if defined in RobotInterface + ) + + # create a local planner that interpolates joint positions (using second-order + # filter) to reach the end-effector targets from global planner (GUI sliders). + # This planner uses the PybulletIKInterface to solve inverse kinematics for + # the target pose, and interpolates the joint targets from current to desired + # values using a second order filter. + local_planner = IKReferenceInterpolator( + # create `PybulletIKInterface` object for this robot for solving its IK. + pybullet_ik_interface=PybulletIKInterface( + urdf_path=robot._sim_robot.urdf_path, + floating_base=False, + starting_base_position=state.state_estimates.pose.position, + starting_base_orientation=state.state_estimates.pose.orientation, + starting_joint_positions=state.joint_states.joint_positions, + joint_names_order=state.joint_states.joint_names, + ), + filter_gain=0.03, + blind_mode=True, + ) + + if USE_GRAVITY_COMP_CONTROLLER: + # load a PD controller that adds gravity compensation torques to the + # control command. This is just an extension of the naive + # JointPositionVelocityController used below. + controller = GravityCompensatedPDController( + kp=[300, 300, 300, 300, 50, 50, 50], + kd=[10, 8, 5, 0.1, 0.1, 0.1, 0.1], + pinocchio_interface=robot.get_pinocchio_interface(), + ) + else: + # load position (and velocity) tracking controller without gravity compensation + controller = JointPDController( + kp=[300, 300, 300, 300, 50, 50, 50], kd=[10, 8, 5, 0.1, 0.1, 0.1, 0.1] + ) + + # create a control loop using these control loop components + control_loop: MinimalCtrlLoop = MinimalCtrlLoop.useWithDefaults( + robot_interface=robot, + controller=controller, + local_planner=local_planner, + global_planner=global_planner, + ) + + ctrl_loop_rate: float = 240 # 240hz control loop + + # loop debugger for publishing data to plotjuggler using zmq. + plj = PlotjugglerLoopDebugger(rate=60) + # enable ee pose comparison in the plotjuggler data stream + plj.data_streamer_config.publish_ee_pose_comparison = True + # this debugger can be used to visualise the planner output that the + # controller is trying to track + br_viz = PybulletRobotPlanDebugger(urdf_path=robot._sim_robot.urdf_path, rate=20) + + control_loop.run( + loop_rate=ctrl_loop_rate, + clock=robot.get_sim_clock(), + debuggers=[plj, br_viz], + ) diff --git a/examples/utils_examples/demo_bullet_robot_visualizer.py b/examples/utils_demos/demo_bullet_robot_visualizer.py similarity index 100% rename from examples/utils_examples/demo_bullet_robot_visualizer.py rename to examples/utils_demos/demo_bullet_robot_visualizer.py diff --git a/examples/utils_demos/demo_plotjuggler_loop_debugger.py b/examples/utils_demos/demo_plotjuggler_loop_debugger.py new file mode 100644 index 0000000..ffe5678 --- /dev/null +++ b/examples/utils_demos/demo_plotjuggler_loop_debugger.py @@ -0,0 +1,195 @@ +"""Demonstrating the functionalities of the `PlotjugglerLoopDebugger` which can +be used to inspect all data in a control loop, by visualising them in plotjuggler. + +Control Loop Debuggers are components that can intercept and read data passed by +the components in the control loop. These components read data (will not modify) +and can be used for visualising/debugging etc. For instance, the PlotjugglerLoopDebugger +is an implementation of the CtrlLoopDebuggerBase class, and intercepts all data +to publish them so that the data can be visualised in plotjuggler. + +NOTE: Plotjuggler should be installed separately. See official documentation here: +https://github.com/facontidavide/PlotJuggler?tab=readme-ov-file#installation. + +`PlotjugglerLoopDebugger` is an implementation of `CtrlLoopDebuggerBase` class +and therefore can be used with the control loop classes directly. + +Use plotjuggler to visualise data published by this debugger (ZMQ subscriber; +msg protocol=json). + +It can also publish custom data if required. + +Also, see `examples/controllers_demo/ik_interpolator_position_controller_manipulator.py` +for example on how to use multiple debuggers, and to see how to use a visual debugging +tool for debugging controllers/planners (`BulletRobotPlanDebugger`). +""" + +from pyrcf.components.robot_interfaces.simulation import PybulletRobot +from pyrcf.control_loop import MinimalCtrlLoop +from pyrcf.components.controllers import SegwayPIDBalanceController +from pyrcf.components.ctrl_loop_debuggers import PlotjugglerLoopDebugger, CtrlLoopDebuggerBase +from pyrcf.core.types import JointStates, EndEffectorStates, Pose3D +from pyrcf.core.types.debug_types import JointStatesCompareType, Pose3DCompareType +from pyrcf.utils.time_utils import PythonPerfClock + +if __name__ == "__main__": + robot = PybulletRobot.fromAwesomeRobotDescriptions( + robot_description_name="upkie_description", + floating_base=True, # this robot does not have its base fixed in the world + ee_names=["left_wheel_tire", "right_wheel_tire"], + ) + + controller = SegwayPIDBalanceController( + pinocchio_interface=robot.get_pinocchio_interface(), + gains=SegwayPIDBalanceController.Gains( + pitch_damping=10.0, + pitch_stiffness=50.0, + footprint_position_damping=5.0, + footprint_position_stiffness=2.0, + joint_kp=200.0, + joint_kd=2.0, + turning_gain_scale=1.0, + ), + max_integral_error_velocity=12, + joint_torque_limits=[-1.0, 1.0], + ) + + # Create a minimal control loop + control_loop: MinimalCtrlLoop = MinimalCtrlLoop.useWithDefaults( + robot_interface=robot, controller=controller + ) + + # Create a debugger that can be used in the control loop. + # The PlotjugglerLoopDebugger publishes data from all components in the + # control loop to port 9872 by default. Data from all components can then + # be visualised in plotjuggler (ZMQ subscriber; msg protocol=json). + # All debuggers can take as argument a `rate` and `clock` parameter which + # determines the frequency at which the debugger's main method is run + # in the control loop. By default, it uses the `PythonPerfClock()` (system) + # clock and runs at the same frequency as the control loop (`rate=None`). + plotjuggler_debugger: CtrlLoopDebuggerBase = PlotjugglerLoopDebugger( + rate=100, clock=PythonPerfClock() + ) + # NOTE: setting the rate to None will publish data at the rate of the control loop + + # Additional configuration parameters for this debugger can be modified as follows. + # # NOTE: all these values are the default, so you don't have to set them unless you + # # want to change them + plotjuggler_debugger.data_streamer_config.prefix_name = "ctrl_loop" + # The above is the prefix name under which all data will be grouped when viewing + # in plotjuggler. + plotjuggler_debugger.data_streamer_config.publish_port = 9872 # port to publish to + plotjuggler_debugger.data_streamer_config.publish_robot_states = True + plotjuggler_debugger.data_streamer_config.publish_global_plan = True + plotjuggler_debugger.data_streamer_config.publish_agent_outputs = True + plotjuggler_debugger.data_streamer_config.publish_robot_cmd = True + + # if you want to publish custom data in addition to the data from the + # control loop, you can also provide handles to functions that returns + # additional publishable data. E.g. + import time + import numpy as np + + start_time = time.time() + + # creating a dummy function for generating extra data to publish + # NOTE: The custom encoder defined in this library allows for + # a more sensible representation when publishing data using the + # datatypes defined in `pyrcf.core.types` + # module as well. + def _get_debug_data() -> dict: + t = time.time() - start_time + data = { + "useless_time_data": { + "t": t, + "cos": np.cos(t), + "sin": np.sin(t), + "floor": np.floor(np.cos(t)), + "ceil": np.ceil(np.cos(t)), + }, + "Demo joint states data": JointStates( + joint_names=["some", "random", "names"], + joint_positions=[0, 1, 2], + joint_efforts=[1, 4, 2], + ), + # Publishing Pose3D objects automatically encodes orientations as + # RPY angles in degrees in addition to quaternions as well for + # easier debugging + "Demo ee states data": EndEffectorStates( + ee_names=["ee_1", "ee_2"], + ee_poses=[ + Pose3D(), + Pose3D(position=np.ones(3) * t, orientation=np.array([0, 1, 0, 0])), + ], + ), + } + # Comparing lists, JointStates and Pose3D objects from different sources is made + # easier using the datatypes defined in `types.debug_types`. E.g. + new_js_obj = JointStates( + # use same joint names for all jointstates objects to make it possible + # to plot them side-by-side using the JointStatesCompareType encoding + joint_names=["some", "random", "names"], + joint_positions=[t, np.sin(t), np.cos(t)], + joint_efforts=[2 * t, np.sin(2 * t), np.cos(2 * t)], + ) + + # add a JointStatesCompareType datatype to make it easier to compare different JointStates + # objects in plotjugler. Just pass a list of JointStates objects that are to be compared. + data["compare_joint_states_data"] = JointStatesCompareType( + joint_states_list=[new_js_obj, data["Demo joint states data"]], + # optional names for the different sources + row_names=["source 1", "source 2"], + ) + + new_pose_obj = Pose3D(position=np.array([0, 1, 2]) * t, orientation=np.array([1, 0, 0, 0])) + + # add a Pose3DCompareType datatype to make it easier to compare different JointStates + # objects in plotjugler. Just pass a list of Pose3D objects that are to be compared. + data["compare_pose_obj"] = Pose3DCompareType( + pose_list=[new_pose_obj, *(data["Demo ee states data"].ee_poses)], + # optional names for the different sources + row_names=["pose obj 1", *(data["Demo ee states data"].ee_names)], + ) + + return data + + # add a handle to this external function to the control loop publisher + plotjuggler_debugger.add_debug_handle_to_publish(debug_publish_callables=[_get_debug_data]) + # NOTE: This could also have been done during construction of PlotjugglerDebugger: + # ``` + # plotjuggler_debugger = PlotjugglerLoopDebugger(debug_publish_callables=[_get_debug_data]) + # ``` + + # Tell the publisher to also stream the additional data we configured above. This + # is already set to True by default + plotjuggler_debugger.data_streamer_config.publish_debug_msg = True + + # In addition to publishing the raw data from the components, the debugger also + # can be used to compare the different JointStates and Pose3D objects from the + # outputs of the control loop components. + plotjuggler_debugger.data_streamer_config.publish_joint_states_comparison = True + # The above flag will make it easier to compare joint states data given from + # different sources (robot state, planner output, controller output etc.) + + plotjuggler_debugger.data_streamer_config.publish_ee_pose_comparison = False + # If the above value is set to True, will publish end-effector poses from + # different sources (robot state estimate, local planner references (if + # available)). By default this is set to False. See + # `examples/controllers_demo/ik_interpolator_position_controller_manipulator.py` + # for an example where this flag is set to True and can be used for comparing + # robot end-effector pose vs target end-effector pose. + + # The comparison flags above basically make use of the JointStatesComparisonType + # and Pose3DComparisonType objects respectively, similar to the example shown + # in the custom function `get_debug_data` defined above. + + ctrl_loop_rate: float = 200 # 200hz control loop + + # run the control loop allowing the debugger to intercept and read the data + control_loop.run(loop_rate=ctrl_loop_rate, debuggers=[plotjuggler_debugger]) + + # All data can now be visualised in plotjuggler (ZMQ subscriber; msg protocol=json). + + +# Also, see `examples/controllers_demo/ik_interpolator_position_controller_manipulator.py` +# for example on how to use multiple debuggers, and to see how to use a visual debugging +# tool for debugging controllers/planners (`BulletRobotPlanDebugger`).