refactor pbik from ik reference planner

examples/tutorials/05_additional_features_and_components.py

@@ -25,7 +25,7 @@
 ** 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. 
+and can be used for visualising/debugging etc.
 
 
 6. There are two debuggers used in this example: the `PlotjugglerLoopDebugger`
@@ -52,7 +52,7 @@
 
 from pyrcf.components.robot_interfaces.simulation import PybulletRobot
 from pyrcf.control_loop import MinimalCtrlLoop
-from pyrcf.components.local_planners import IKReferenceInterpolator
+from pyrcf.components.local_planners import PybulletIKReferenceInterpolator
 from pyrcf.components.global_planners.ui_reference_generators import (
     PybulletGUIGlobalPlannerInterface,
 )
@@ -97,16 +97,13 @@
     # 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,
-        ),
+    local_planner = PybulletIKReferenceInterpolator(
+        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,
     )

pyproject.toml

@@ -4,6 +4,13 @@ version = "0.0.6"
 authors = [{ name = "Saif Sidhik", email = "[email protected]" }]
 description = "A Python Robot Control Framework for quickly prototyping control algorithms for different robot embodiments."
 readme = "README.md"
+keywords = [
+  "python robot control framework",
+  "robotics",
+  "control framework",
+  "legged robot",
+  "motion planner",
+]
 requires-python = ">= 3.10"
 dependencies = [
   "pybullet>=3.2.6,<4",
@@ -15,7 +22,8 @@ dependencies = [
   "numpy>=1.26",
   "inputs>=0.5",
   "numpy-quaternion>=2023.0.4",
-  "zmq>=0.0.0", "tk",
+  "zmq>=0.0.0",
+  "tk",
 ]
 
 [build-system]

pyrcf/components/controllers/README.md

@@ -25,7 +25,7 @@ Joint PD controller for joint position and velocity tracking.
 
 - Compatible LocalPlanner Classes:
   - `JointReferenceInterpolator`
-  - `IKReferenceInterpolator`
+  - `PybulletIKReferenceInterpolator`
   - `BlindForwardingPlanner` (if applicable)
 
 ### `GravityCompensatedPDController`
@@ -50,5 +50,5 @@ Joint PD controller for joint position and velocity tracking with active gravity
 
 - Compatible LocalPlanner Classes:
   - `JointReferenceInterpolator`
-  - `IKReferenceInterpolator`
+  - `PybulletIKReferenceInterpolator`
   - `BlindForwardingPlanner` (if applicable)

pyrcf/components/local_planners/__init__.py

@@ -4,4 +4,4 @@
 from .local_planner_base import LocalPlannerBase, DummyLocalPlanner
 from .blind_forwarding_planner import BlindForwardingPlanner
 from .joint_reference_interpolator import JointReferenceInterpolator
-from .ik_reference_interpolator import IKReferenceInterpolator
+from .pb_ik_reference_interpolator import PybulletIKReferenceInterpolator

pyrcf/components/local_planners/ik_reference_interpolator.py → pyrcf/components/local_planners/pb_ik_reference_interpolator.py

@@ -16,24 +16,41 @@
 )
 
 
-class IKReferenceInterpolator(LocalPlannerBase):
-    """Solves IK for given end-effector reference and interpolates joint references."""
+class PybulletIKReferenceInterpolator(LocalPlannerBase):
+    """Solves IK using PybulletIKInterface for given end-effector reference and interpolates joint references."""
 
     def __init__(
         self,
-        pybullet_ik_interface: PybulletIKInterface,
+        urdf_path: str,
+        floating_base: bool = False,
+        starting_base_position: Vector3D = np.zeros(3),
+        starting_base_orientation: QuatType = np.array([0, 0, 0, 1]),
+        starting_joint_positions: List[float] = None,
+        joint_names_order: List[str] = None,
         filter_gain=0.05,
         blind_mode: bool = True,
         ee_names: List[str] = None,
         max_constraint_force: float = 10000,
         solver_erp: float = None,
+        **pb_ik_kwargs,
     ):
         """A "local planner" that simply solves IK for given end-effector reference
-        and interpolates joint references to achieve the target EE pose.
+        and interpolates joint references to achieve the target EE pose. Uses
+        PybulletIKInterface from pybullet_robot package to solve IK.
 
         Args:
-            pybullet_ik_interface (PybulletIKInterface): Pre-defined PybulletIKInterface
-                object that can be used for solving IK for this robot.
+            urdf_path (str): Path to urdf of the robot.
+            floating_base (bool, optional): Whether the robot has fixed base or floating base.
+                Defaults to False.
+            starting_base_position (Vector3D, optional): Starting base position of the robot.
+                Defaults to np.zeros(3).
+            starting_base_orientation (QuatType, optional): Starting base orientation of the robot.
+                Defaults to np.array([0, 0, 0, 1]).
+            starting_joint_positions (List[float], optional): Starting joint positions of the
+                robot. Defaults to None.
+            joint_names_order (List[str], optional): Order of joint names to be used. This will be
+                the order of the output of the IK as well. Defaults to None (use default order from
+                BulletRobot instance when loading this urdf).
             filter_gain (float, optional): Smoothing gain for second-order filter
                 interpolator. Defaults to 0.05.
             blind_mode (bool, optional): If set to False, will not use joint state
@@ -46,8 +63,18 @@ def __init__(
                 to pull the link. Defaults to 10000 (setting `None` will use pybullet default).
             solver_erp (float, optional): Error reduction parameter for this constraint. Defaults
                 to None (use pybullet default).
+            **pb_ik_kwargs (optional): Additional keywork arguments to be used while creating the
+                PybulletIKInterface object.
         """
-        self._pb_ik = pybullet_ik_interface
+        self._pb_ik = PybulletIKInterface(
+            urdf_path=urdf_path,
+            floating_base=floating_base,
+            starting_base_position=starting_base_position,
+            starting_base_orientation=starting_base_orientation,
+            starting_joint_positions=starting_joint_positions,
+            joint_names_order=joint_names_order,
+            **pb_ik_kwargs,
+        )
         self._alpha = filter_gain
         self._blind_mode = blind_mode
         self._max_force = max_constraint_force

test_scripts/ik_interpolator_position_controller_manipulator.py

@@ -9,9 +9,9 @@
 2. This demo uses a pybullet GUI interface to set end-effector targets. This
 is implemented as a `GlobalPlanner` (`UIBase`) called `PybulletGUIGlobalPlannerInterface`.
 
-3. The local planner is `IKReferenceInterpolator` which uses a second order filter
+3. The local planner is `PybulletIKReferenceInterpolator` 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`.
+the GUI sliders.
 
 4. The controller is a joint position (and velocity) tracking controller.
 
@@ -22,11 +22,9 @@
 that shows the actual output from the planner (useful for controller tuning/debugging).
 """
 
-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.local_planners import PybulletIKReferenceInterpolator
 from pyrcf.components.global_planners.ui_reference_generators import (
     PybulletGUIGlobalPlannerInterface,
 )
@@ -68,16 +66,13 @@
 
     # create a local planner that interpolates joint positions (using second-order
     # filter) to reach the end-effector targets from global planner (GUI sliders)
-    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,
-        ),
+    local_planner = PybulletIKReferenceInterpolator(
+        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,
     )