added teleop with placo only ik visualization with x7s example; added urdf visualization scripts; reorganized project folder structure

Author: zhigenzhao

README.md

@@ -49,29 +49,38 @@ The main dependencies are listed in the [`pyproject.toml`](pyproject.toml) file
 To run the teleoperation demo with a UR5e robot in MuJoCo simulation:
 
 ```bash
-python scripts/teleop_dual_ur5e_mujoco.py
+python scripts/simulation/teleop_dual_ur5e_mujoco.py
 ```
-This script initializes the [`MujocoTeleopController`](teleop_demo_python/mujoco_teleop_controller.py) with the UR5e model and starts the teleoperation loop.
+This script initializes the [`MujocoTeleopController`](teleop_demo_python/simulation/mujoco_teleop_controller.py) with the UR5e model and starts the teleoperation loop.
+
+### Running the Placo Visualization Demo
+
+To run the teleoperation demo with a UR5e robot in Placo visualization:
+
+```bash
+python scripts/simulation/teleop_x7s_placo.py
+```
+This script initializes the [`PlacoTeleopController`](teleop_demo_python/simulation/placo_teleop_controller.py) with the X7S robot and starts the teleoperation loop.
 
 ### Running the Hardware Demo (Dual UR Arms and Dynamixel Head)
 
 To run the teleoperation demo with the physical dual UR arms and Dynamixel-based head:
 
 1.  **Normal Operation:**
     ```bash
-    python scripts/teleop_dual_ur5e_hardware.py
+    python scripts/hardware/teleop_dual_ur5e_hardware.py
     ```
     This script initializes the [`DynamixelHeadController`](teleop_demo_python/hardware/dynamixel.py) and [`DualArmURController`](teleop_demo_python/hardware/ur.py) and starts the teleoperation loops for both head tracking and arm control.
 
 2.  **Resetting Arm Positions:**
     If you need to reset the UR arms to their initial/home positions and initialize the robotiq grippers, you can run the script with the `--reset` flag:
     ```bash
-    python scripts/teleop_dual_ur5e_hardware.py --reset
+    python scripts/hardware/teleop_dual_ur5e_hardware.py --reset
     ```
     This will execute the reset procedure defined in the [`DualArmURController`](teleop_demo_python/hardware/ur.py) and then exit.
 
 3.  **Visualizing IK results:**
     To visualize the inverse kinematics solution with placo during teleoperation, run the script with the `--visualize_placo` flag.
     ```bash
-    python scripts/teleop_dual_ur5e_hardware.py --visualize_placo
+    python scripts/hardware/teleop_dual_ur5e_hardware.py --visualize_placo
     ```

scripts/dual_arm_control.py scripts/hardware/dual_arm_control.pyscripts/dual_arm_control.py renamed to scripts/hardware/dual_arm_control.py

@@ -1,17 +1,14 @@
 import os
-from teleop_demo_python.mujoco_teleop_controller import (
+
+from teleop_demo_python.simulation.mujoco_teleop_controller import (
     MujocoTeleopController,
 )
 from teleop_demo_python.utils.path_utils import ASSET_PATH
 
 
 def main():
-    xml_path = os.path.join(
-        ASSET_PATH, "universal_robots_ur5e/scene_dual_arm.xml"
-    )
-    robot_urdf_path = os.path.join(
-        ASSET_PATH, "universal_robots_ur5e/dual_ur5e.urdf"
-    )
+    xml_path = os.path.join(ASSET_PATH, "universal_robots_ur5e/scene_dual_arm.xml")
+    robot_urdf_path = os.path.join(ASSET_PATH, "universal_robots_ur5e/dual_ur5e.urdf")
 
     config = {
         "right_hand": {

scripts/head_control.py scripts/hardware/head_control.pyscripts/head_control.py renamed to scripts/hardware/head_control.py

@@ -0,0 +1,57 @@
+import os
+
+from teleop_demo_python.simulation.placo_teleop_controller import (
+    PlacoTeleopController,
+)
+from teleop_demo_python.utils.path_utils import ASSET_PATH
+
+
+def main():
+
+    # 14 dof: only arms
+    # robot_urdf_path = os.path.join(ASSET_PATH, "X7S/X7S_arm_only.urdf")
+
+    # 17 dof: 2 arms, body tilting (joint2), head (joint3 and joint4)
+    # robot_urdf_path = os.path.join(
+    #     ASSET_PATH, "X7S/X7S_fixed_vertical_no_gripper.urdf"
+    # )
+
+    # 22 dof: all joints available
+    robot_urdf_path = os.path.join(ASSET_PATH, "X7S/X7S.urdf")
+
+    config = {
+        "right_hand": {
+            "link_name": "link20",
+            "pose_source": "right_controller",
+            "control_trigger": "right_grip",
+        },
+        "left_hand": {
+            "link_name": "link11",
+            "pose_source": "left_controller",
+            "control_trigger": "left_grip",
+        },
+    }
+
+    # Create and initialize the teleoperation controller
+    controller = PlacoTeleopController(
+        robot_urdf_path=robot_urdf_path,
+        end_effector_config=config,
+        scale_factor=1.5,
+    )
+
+    # additional constraints hardcoded here for now
+    joints_task = controller.solver.add_joints_task()
+    joints_task.set_joints({joint: 0.0 for joint in controller.robot.joint_names()})
+    joints_task.configure("joints_regularization", "soft", 5e-4)
+
+    if "joint2" in controller.robot.joint_names():
+        controller.robot.set_joint_limits(
+            "joint2", -0.5, 0.1
+        )  # to avoid excessive tilting of torso
+        controller.solver.enable_velocity_limits(True)
+
+    controller.run()
+
+
+if __name__ == "__main__":
+    main()

scripts/teleop_dual_ur5e_hardware.py …ts/hardware/teleop_dual_ur5e_hardware.pyscripts/teleop_dual_ur5e_hardware.py renamed to scripts/hardware/teleop_dual_ur5e_hardware.py scripts/teleop_dual_ur5e_hardware.py renamed to scripts/hardware/teleop_dual_ur5e_hardware.py

@@ -0,0 +1,31 @@
+import argparse
+import webbrowser
+
+import placo
+from placo_utils.visualization import robot_viz
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Visualize a URDF model using Placo and Meshcat.")
+    parser.add_argument(
+        "--urdf_path",
+        type=str,
+        help="Path to the URDF file to visualize.",
+    )
+    args = parser.parse_args()
+    robot = placo.RobotWrapper(args.urdf_path)
+    print(f"robot state: {robot.state.q}")
+    print(f"robot state shape: {robot.state.q.shape}")
+    # robot.state.q[:7] = np.array([0, 0, 0, 1, 0, 0, 0])
+    # robot.state.q[7:] = 0.5 * np.ones(robot.state.q[7:].shape)  # Set initial joint positions
+    robot.update_kinematics()
+    viz = robot_viz(robot)
+    webbrowser.open(viz.viewer.url())
+
+    while True:
+        robot.update_kinematics()
+        viz.display(robot.state.q)
+
+
+if __name__ == "__main__":
+    main()

scripts/teleop_dual_ur5e_mujoco.py …ts/simulation/teleop_dual_ur5e_mujoco.pyscripts/teleop_dual_ur5e_mujoco.py renamed to scripts/simulation/teleop_dual_ur5e_mujoco.py scripts/teleop_dual_ur5e_mujoco.py renamed to scripts/simulation/teleop_dual_ur5e_mujoco.py

@@ -39,12 +39,28 @@
 DEFAULT_DUAL_ARM_URDF_PATH = os.path.join(ASSET_PATH, "universal_robots_ur5e/dual_ur5e.urdf")
 DEFAULT_SCALE_FACTOR = 1.0
 
+DEFAULT_END_EFFECTOR_CONFIG = {
+    "left_arm": {
+        "link_name": "left_tool0",
+        "pose_source": "left_controller",
+        "control_trigger": "left_grip",
+        "gripper_trigger": "left_trigger",
+    },
+    "right_arm": {
+        "link_name": "right_tool0",
+        "pose_source": "right_controller",
+        "control_trigger": "right_grip",
+        "gripper_trigger": "right_trigger",
+    },
+}
+
 
 class DualArmURController:
     def __init__(
         self,
         pico_client: PicoClient,
         robot_urdf_path: str = DEFAULT_DUAL_ARM_URDF_PATH,  # Path to URDF for Placo
+        end_effector_config: dict = DEFAULT_END_EFFECTOR_CONFIG,
         left_robot_ip: str = LEFT_ROBOT_IP,
         right_robot_ip: str = RIGHT_ROBOT_IP,
         left_initial_joint_deg: np.ndarray = LEFT_INITIAL_JOINT_DEG,  # Use DEG for consistency
@@ -97,22 +113,7 @@ def __init__(
         self.solver.add_kinetic_energy_regularization_task(1e-6)
 
         # Define end-effector configuration (adjust link names and pico sources as needed)
-        self.end_effector_config = {
-            "left_arm": {
-                "link_name": "left_tool0",
-                "pose_source": "left_controller",
-                "control_trigger": "left_grip",
-                "gripper_trigger": "left_trigger",
-                "vis_target": "left_target",
-            },
-            "right_arm": {
-                "link_name": "right_tool0",
-                "pose_source": "right_controller",
-                "control_trigger": "right_grip",
-                "gripper_trigger": "right_trigger",
-                "vis_target": "right_target",
-            },
-        }
+        self.end_effector_config = end_effector_config
 
         self.effector_task = {}
         self.init_ee_xyz = {}
@@ -157,7 +158,7 @@ def __init__(
             for name, config in self.end_effector_config.items():
                 robot_frame_viz(self.placo_robot, config["link_name"])
                 frame_viz(
-                    config["vis_target"],
+                    f"vis_target_{name}",
                     self.effector_task[name].T_world_frame,
                 )
 
@@ -272,7 +273,7 @@ def calc_target_joint_position(self):
                 for name, config in self.end_effector_config.items():
                     robot_frame_viz(self.placo_robot, config["link_name"])
                     frame_viz(
-                        f"target_{name}",
+                        f"vis_target_{name}",
                         self.effector_task[name].T_world_frame,
                     )