ezgripper_sim/demo_tennis_ball.py at main · vikashplus/ezgripper_sim · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#!/usr/bin/env python3
"""
Tennis Ball Grasp Demo for EZGripper

Demonstrates under-actuated grasping:
1. Gripper starts open above ball
2. Close gripper with moderate effort
3. L1 contacts ball first → stops
4. Tendon force overcomes L2 spring → L2 wraps
5. Adaptive conforming to spherical object
"""
import mujoco
import mujoco.viewer
import numpy as np
import time
import os

# Get the directory of this script
script_dir = os.path.dirname(os.path.abspath(__file__))
model_path = os.path.join(script_dir, "ezgripper.xml")

print("=" * 60)
print("EZGripper Tennis Ball Grasp Demo")
print("=" * 60)
print(f"Model: {model_path}")
print(f"MuJoCo version: {mujoco.__version__}")
print()

# Load the model
model = mujoco.MjModel.from_xml_path(model_path)
data = mujoco.MjData(model)

print("✓ Model loaded successfully!")
print(f"  - Bodies: {model.nbody}")
print(f"  - Joints: {model.njnt}")
print(f"  - Actuators: {model.nu}")
print(f"  - Tendons: {model.ntendon}")
print()

# Get actuator IDs (both fingers controlled identically)
gripper_f1_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, 'gripper_actuator_f1')
gripper_f2_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, 'gripper_actuator_f2')

# Demo sequence
print("Demo Sequence:")
print("  1. Gripper starts OPEN (springs extended)")
print("  2. Wait 2 seconds")
print("  3. CLOSE gripper slowly (watch L1 and L2 behavior)")
print("  4. L1 contacts ball → stops")
print("  5. L2 wraps around ball (tendon overcomes spring)")
print("  6. Hold grasp for 3 seconds")
print("  7. OPEN gripper (springs return)")
print()
print("Controls:")
print("  - Space: Pause/Resume")
print("  - ESC: Exit")
print("  - Double-click: Select body")
print("  - Right-drag: Rotate view")
print()
print("Starting in 2 seconds...")
time.sleep(2)

# Simulation parameters
sim_time = 0.0
phase_info = {"phase": "open"}

def update_control(model, data, phase_info):
    """Update gripper control - cycle between max open and max close every 5 seconds"""
    current_time = data.time

    # Calculate cycle: 5 seconds per direction
    cycle_time = current_time % 10.0  # 10 second total cycle (5s close, 5s open)

    # Tendon length range: 0.135 (closed) to 0.160 (open)
    open_length = 0.160   # meters (fully open - extended more)
    closed_length = 0.135  # meters (fully closed)

    if cycle_time < 5.0:
        # Closing phase (0-5 seconds) - gradually decrease tendon length
        progress = cycle_time / 5.0
        target_length = open_length - (open_length - closed_length) * progress
        data.ctrl[gripper_f1_id] = target_length
        data.ctrl[gripper_f2_id] = target_length
        if phase_info['phase'] != "closing":
            phase_info['phase'] = "closing"
            print(f"[{current_time:.2f}s] CLOSING (target length: {target_length:.4f}m)")
    else:
        # Opening phase (5-10 seconds) - gradually increase tendon length
        progress = (cycle_time - 5.0) / 5.0
        target_length = closed_length + (open_length - closed_length) * progress
        data.ctrl[gripper_f1_id] = target_length
        data.ctrl[gripper_f2_id] = target_length
        if phase_info['phase'] != "opening":
            phase_info['phase'] = "opening"
            print(f"[{current_time:.2f}s] OPENING (target length: {target_length:.4f}m)")

# Launch viewer with custom controller
print("\n" + "=" * 60)
print("Launching viewer...")
print("=" * 60 + "\n")

with mujoco.viewer.launch_passive(model, data) as viewer:
    # Initial setup
    viewer.cam.distance = 0.4
    viewer.cam.azimuth = 45
    viewer.cam.elevation = -20
    viewer.cam.lookat = np.array([0.14, 0.0, 0.08])

    # Run simulation
    while viewer.is_running():
        step_start = time.time()

        # Update control
        update_control(model, data, phase_info)

        # Step simulation
        mujoco.mj_step(model, data)
        sim_time += model.opt.timestep

        # Update viewer
        viewer.sync()

        # Maintain real-time
        time_until_next_step = model.opt.timestep - (time.time() - step_start)
        if time_until_next_step > 0:
            time.sleep(time_until_next_step)

print("\nDemo finished!")