test pin for ik, and mujoco

kine_ctrl/main.py

@@ -1,13 +1,72 @@
 
 
 from test_pin import KinematicsSolver as controller
+from rm75_mjc import MuJoCoPositionController
 
+import time
+from pathlib import Path
+
+def demo_position_control():
+    """Demonstrate pure position control"""
+
+    urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
+
+    if not Path(urdf_path).exists():
+        print(f"Error: URDF not found at {urdf_path}")
+        return
+
+    print("=" * 60)
+    print("Pure Position Control Demo")
+    print("=" * 60)
+
+    # Create controller
+    robot = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
+    robot.start()
+    time.sleep(1)
+
+    print("\n[Test 1] Move joint 1 to 45 degrees")
+    robot.send_command([0.785, 0, 0, 0, 0, 0, 0])
+    robot.wait_until_reached()
+    robot.print_state()
+    time.sleep(0.5)
+
+    print("\n[Test 2] Move joint 2 to -30 degrees")
+    robot.send_command([0, -0.524, 0, 0, 0, 0, 0])
+    robot.wait_until_reached()
+    robot.print_state()
+    time.sleep(0.5)
+
+    print("\n[Test 3] Move multiple joints simultaneously")
+    robot.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0])
+    robot.wait_until_reached()
+    robot.print_state()
+    time.sleep(0.5)
+
+    print("\n[Test 4] Return home")
+    robot.send_command([0, 0, 0, 0, 0, 0, 0])
+    robot.wait_until_reached()
+    robot.print_state()
+
+    print("\n" + "=" * 60)
+    print("✓ All tests passed! Robot is stable and controllable.")
+    print("=" * 60)
+    print("\nInteractive mode - close viewer to exit")
+
+    try:
+        while robot.viewer and robot.viewer.is_running():
+            time.sleep(0.1)
+    except KeyboardInterrupt:
+        pass
+
+    robot.stop()
 
 def main():
 
-    kine_node = controller()
-    kine_node.loop_run()
-    print("main get returned kine_node")
+    demo_position_control()
+
+    # kine_node = controller()
+    # kine_node.loop_run()
+    # print("main get returned kine_node")
 
 
 if __name__ == "__main__":