74 lines
1.8 KiB
Python
74 lines
1.8 KiB
Python
|
|
|
|
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():
|
|
|
|
demo_position_control()
|
|
|
|
# kine_node = controller()
|
|
# kine_node.loop_run()
|
|
# print("main get returned kine_node")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|