test migration to pixi; add incomplete demo

.gitattributes

@@ -0,0 +1,2 @@
+# GitHub syntax highlighting
+pixi.lock linguist-language=YAML linguist-generated=true

.github/dependabot.yml

@@ -0,0 +1,32 @@
+# To get started with Dependabot version updates, you'll need to specify which
+# package ecosystems to update and where the package manifests are located.
+# Please see the documentation for all configuration options:
+# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates 
+
+version: 2
+updates:
+  - package-ecosystem: "pip" # See documentation for possible values
+    directory: "/" # Location of package manifests
+    schedule:
+      interval: "monthly"
+    groups:
+      dev-dependencies:
+        #try to group all development dependencies updates into a single pr
+        patterns:          
+          - "black"
+          - "check-manifest"
+          - "pre-commit"
+          - "pylint"
+          - "pytest"
+          - "pytest-cov"          
+          - "hypothesis"
+          - "ruff"
+          - "coverage"
+      lib-dependencies:
+        #try to group all third party library updates into a single pr
+        patterns:
+          - "*"
+  - package-ecosystem: github-actions
+    directory: /
+    schedule:
+      interval: monthly

.github/workflows/publish.yml

@@ -0,0 +1,18 @@
+name: Auto-publish
+
+on: [push, workflow_dispatch]
+
+jobs:
+  # Auto-publish when version is increased
+  publish-job:
+    # Only publish on `test_pixi` branch
+    if: github.ref == 'refs/heads/test_pixi'
+    runs-on: ubuntu-latest
+    permissions:  # Don't forget permissions
+      contents: write
+    steps:
+    - uses: etils-actions/pypi-auto-publish@v1
+      with:
+        pypi-token: ${{ secrets.PYPI_API_TOKEN }}
+        gh-token: ${{ secrets.GITHUB_TOKEN }}
+        parse-changelog: true

.gitignore

@@ -26,6 +26,9 @@ share/python-wheels/
 *.egg
 MANIFEST
 
+# pixi environments
+.pixi
+
 # PyInstaller
 #  Usually these files are written by a python script from a template
 #  before PyInstaller builds the exe, so as to inject date/other infos into it.
@@ -94,13 +97,6 @@ ipython_config.py
 #   install all needed dependencies.
 #Pipfile.lock
 
-# poetry
-#   Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
-#   This is especially recommended for binary packages to ensure reproducibility, and is more
-#   commonly ignored for libraries.
-#   https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
-poetry.lock
-
 # pdm
 #   Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
 #pdm.lock

examples/demo_task_space_control.py

@@ -0,0 +1,53 @@
+import time
+import numpy as np
+from pybullet_robot.bullet_robot import BulletRobot
+from pybullet_robot.utils.urdf_utils import get_urdf_from_awesome_robot_descriptions
+
+from impedance_controllers import CartesianImpedanceController
+
+NEUTRAL_JOINT_POS = [
+    -0.017792060227770554,
+    -0.7601235411041661,
+    0.019782607023391807,
+    -2.342050140544315,
+    0.029840531355804868,
+    1.5411935298621688,
+    0.7534486589746342,
+    0.0,
+    0.0,
+]
+
+Kp = np.array([12000.0, 2000.0, 2000.0, 10, 10, 10])
+Kd = 2 * np.sqrt(Kp)
+Kd[3:] = np.asarray([0.00, 0.00, 0.00])
+NULLSPACE_Kp = np.array([1000] * 9)
+
+if __name__ == "__main__":
+
+    robot = BulletRobot(
+        urdf_path=get_urdf_from_awesome_robot_descriptions("panda_description"),
+        default_joint_positions=NEUTRAL_JOINT_POS,
+        enable_torque_mode=True,  # enable to be able to use torque control
+        ee_names=["panda_hand"],
+        run_async=False,  # set to False to enable stepping simulation manually
+    )
+
+    controller = CartesianImpedanceController(
+        robot=robot,
+        kp=Kp,
+        kd=Kd,
+        null_kp=NULLSPACE_Kp,
+        nullspace_pos_target=NEUTRAL_JOINT_POS,
+    )
+
+    robot.step()
+    curr_ee_pos, curr_ee_ori = robot.get_link_pose(link_id=robot.ee_ids[0])
+    controller.set_target(goal_pos=curr_ee_pos.copy(), goal_ori=curr_ee_ori.copy())
+
+    while True:
+
+        joint_cmds, _ = controller.compute_cmd()
+        robot.set_actuated_joint_commands(tau=joint_cmds)
+
+        robot.step()  # step simulation
+        time.sleep(0.01)

examples/impedance_controllers.py

@@ -0,0 +1,99 @@
+import numpy as np
+from pybullet_robot.bullet_robot import BulletRobot
+
+
+# pylint: disable=C0116
+def quat2rpy(quat: np.ndarray) -> tuple:
+    q1, q2, q3, q0 = quat
+    roll = np.arctan2(
+        2 * ((q2 * q3) + (q0 * q1)), q0**2 - q1**2 - q2**2 + q3**2
+    )  # radians
+    pitch = np.arcsin(2 * ((q1 * q3) - (q0 * q2)))
+    yaw = np.arctan2(2 * ((q1 * q2) + (q0 * q3)), q0**2 + q1**2 - q2**2 - q3**2)
+    return np.array((roll, pitch, yaw))
+
+
+def wrap_angle(angle: float | np.ndarray) -> float | np.ndarray:
+    return (angle + np.pi) % (2 * np.pi) - np.pi
+
+
+class CartesianImpedanceController:
+    """Simplified PD control for end-effector tracking."""
+
+    def __init__(
+        self,
+        robot: BulletRobot,
+        kp: np.array,
+        kd: np.ndarray,
+        null_kp: np.ndarray,
+        nullspace_pos_target: np.ndarray,
+    ):
+
+        self._robot = robot
+        self._kp = kp
+        self._kd = kd
+        self._null_kp = null_kp
+        self._nullspace_target = nullspace_pos_target
+        self._goal_pos: np.ndarray = None
+        self._goal_ori: np.ndarray = None
+
+    def set_target(self, goal_pos: np.ndarray, goal_ori: np.ndarray):
+        self._goal_pos = goal_pos
+        self._goal_ori = goal_ori
+
+    def compute_cmd(self):
+        curr_pos, curr_ori = self._robot.get_link_pose(
+            link_id=self._robot.get_link_id(link_name=self._robot.ee_names[0])
+        )
+        curr_joint_pos = self._robot.get_actuated_joint_positions()
+        delta_pos = self._goal_pos - curr_pos
+
+        delta_ori = wrap_angle(
+            wrap_angle(quat2rpy(self._goal_ori)) - wrap_angle(quat2rpy(curr_ori))
+        )
+
+        curr_vel, curr_omg = self._robot.get_link_velocity(
+            link_id=self._robot.get_link_id(link_name=self._robot.ee_names[0])
+        )
+        print(wrap_angle(quat2rpy(self._goal_ori)), wrap_angle(quat2rpy(curr_ori)))
+
+        cmd_force = np.zeros(6)
+        cmd_force[:3] = self._kp[:3] * delta_pos - self._kd[:3] * curr_vel
+        cmd_force[3:] = self._kp[3:] * delta_ori - self._kd[3:] * curr_omg
+
+        error = np.asarray([np.linalg.norm(delta_pos), np.linalg.norm(delta_ori)])
+
+        jac = self._robot.get_jacobian(ee_link_name=self._robot.ee_names[0])
+        null_space_filter = self._null_kp.dot(
+            np.eye(curr_joint_pos.size) - jac.T.dot(np.linalg.pinv(jac.T, rcond=1e-3))
+        )
+        tau = jac.T.dot(cmd_force.reshape([-1, 1])) + null_space_filter.dot(
+            (self._nullspace_target - curr_joint_pos)
+        )
+        # joint torques to be commanded
+        return tau.flatten(), error
+
+
+class JointImpedanceController:
+    """Simplified PD control for end-effector tracking."""
+
+    def __init__(self, robot: BulletRobot, kp: np.array, kd: np.ndarray):
+
+        self._robot = robot
+        self._kp = kp
+        self._kd = kd
+        self._goal_joint_pos: np.ndarray = None
+
+    def set_target(self, goal_joint_pos: np.ndarray):
+        self._goal_joint_pos = goal_joint_pos
+
+    def compute_cmd(self):
+        curr_joint_pos = self._robot.get_actuated_joint_positions()
+        curr_joint_vel = self._robot.get_actuated_joint_velocities()
+
+        delta_pos = self._goal_joint_pos - curr_joint_pos
+        # Desired joint effort commands computed using PD law
+        tau = self._kp * delta_pos - self._kd * curr_joint_vel
+
+        # joint torques to be commanded
+        return tau, np.linalg.norm(delta_pos)