added pico_client, QP solver error handling, and deprecated single arm controller class.

Author: zhigenzhao

scripts/ur5e_demo.py

@@ -37,8 +37,6 @@ def main():
         # visualize_placo=True,
     )
 
-    controller.initialize()
-    controller.xr_init()
     controller.run()
 
 

scripts/ur5e_dual_arm_demo.py

@@ -70,7 +70,7 @@ if [[ "$1" == "--conda" ]]; then
 
     echo -e "[INFO] Created conda environment named '$ENV_NAME'.\n"
     echo -e "\t\t1. To activate the environment, run:                conda activate $ENV_NAME"
-    echo -e "\t\t2. To install the package, run:                     bash setup.sh --install"
+    echo -e "\t\t2. To install the package, run:                     bash setup_conda.sh --install"
     echo -e "\t\t3. To deactivate the environment, run:              conda deactivate"
     echo -e "\n"
 
@@ -114,20 +114,21 @@ elif [[ "$1" == "--install" ]]; then
     cd RoboticsService-Python
     cp ../RoboticsService-PC/RoboticsService/PXREARobotSDK/build/libPXREARobotSDK.so lib/ || { echo "Failed to copy libPXREARobotSDK.so"; exit 1; }
     python setup.py install || { echo "Failed to install RoboticsService-Python"; exit 1; }
+    cd ..
+    rm -rf RoboticsService-PC
 
     # cd ..
     # git clone https://github.com/zhigenzhao/placo.git
     # cd placo
     # git checkout xml_experimental || { echo "Failed to checkout xml_experimental branch"; exit 1; }
     # bash setup.sh --install || { echo "Failed to run placo setup"; exit 1; }
 
-    cd ../..
+    cd ..
 
     pip install -e . || { echo "Failed to install teleop_demo_mujoco with pip"; exit 1; }
 
     echo -e "\n"
-    echo -e "[INFO] Created conda environment named '$ENV_NAME'.\n"
-    echo -e "To activate the environment, run: conda activate $ENV_NAME"
+    echo -e "[INFO] teleop_demo_mujoco is installed in conda environment '$ENV_NAME'.\n"
     echo -e "\n"
 else
     echo "Invalid argument. Use --conda to create a conda environment or --install to install the package."

setup_conda.sh

@@ -6,7 +6,6 @@
 import pinocchio
 import placo
 import meshcat.transformations as tf
-import pyroboticsservice as xr
 
 from placo_utils.visualization import (
     robot_viz,
@@ -20,6 +19,8 @@
     quat_diff_as_angle_axis,
 )
 
+from teleop_demo_mujoco.pico_client import PicoClient
+
 
 class MujocoTeleopController:
     def __init__(
@@ -53,6 +54,7 @@ def __init__(
         self.visualize_placo = visualize_placo
         self.scale_factor = scale_factor
         self.q_init = q_init
+        self.q_current = q_init
 
         # To be initialized later
         self.mj_model = None
@@ -76,8 +78,8 @@ def __init__(
             name: None for name in end_effector_config.keys()
         }
 
-    def initialize(self):
-        """Set up the MuJoCo simulation and the IK solver."""
+        self.pico_client = PicoClient()
+
         self._setup_mujoco()
         self._setup_placo()
 
@@ -196,10 +198,6 @@ def _setup_mocap_target(self):
                 f"Mocap ID for '{vis_target}' body: {self.target_mocap_idx[name]}"
             )
 
-    def xr_init(self):
-        """Initialize XR tracking."""
-        xr.init()
-
     def run(self):
         """Run the main teleoperation loop."""
 
@@ -215,7 +213,7 @@ def run(self):
                 self.t += self.dt
 
                 for name, config in self.end_effector_config.items():
-                    xr_grip = xr.get_key_value_by_name(
+                    xr_grip = self.pico_client.get_key_value_by_name(
                         config["control_trigger"]
                     )
                     active = xr_grip > 0.5
@@ -227,7 +225,9 @@ def run(self):
                                 self._get_end_effector_info(config["link_name"])
                             )
                             print(f"{name} is activated.")
-                        xr_pose = xr.get_pose_by_name(config["pose_source"])
+                        xr_pose = self.pico_client.get_pose_by_name(
+                            config["pose_source"]
+                        )
                         delta_xyz, delta_rot = self._process_xr_pose(
                             xr_pose, name
                         )
@@ -302,18 +302,36 @@ def _process_xr_pose(self, xr_pose, src_name):
         return delta_xyz, delta_rot
 
     def _update_kinematics(self):
-        # Solve IK
-        self.solver.solve(True)
-        self.placo_robot.update_kinematics()
+        try:
+            self.solver.solve(True)
+            self.placo_robot.update_kinematics()
+        except RuntimeError as e:  # Catch RuntimeError
+            if "QPError" in str(e) and "NaN in the QP solution" in str(e):
+                print(f"IK solver failed with QPError (NaN): {e}")
+                if self.floating_base:
+                    self.placo_robot.state.q = self.q_current
+                else:
+                    self.placo_robot.state.q[7:] = self.q_current
+                    self.placo_robot.state.q[:7] = np.array(
+                        [0, 0, 0, 1, 0, 0, 0]
+                    )
+
+                self.placo_robot.update_kinematics()
+            else:
+                print(f"An unexpected RuntimeError occurred in IK solver: {e}")
+                return
+        except Exception as e:
+            print(f"An unexpected error occurred in IK solver: {e}")
+            return
+
         if self.floating_base:
-            q = self.placo_robot.state.q
+            self.q_current = self.placo_robot.state.q
         else:
-            q = self.placo_robot.state.q[7:]
-
-        self.mj_data.ctrl = q
+            self.q_current = self.placo_robot.state.q[7:]
+        self.mj_data.ctrl = self.q_current
 
         if self.visualize_placo:
-            self.placo_vis.display(self.placo_robot.state.q)
+            self.placo_vis.display(self.q_current)
 
             for name, config in self.end_effector_config.items():
                 robot_frame_viz(self.placo_robot, config["link_name"])