Skip to content

Commit 3196339

Browse files
DozgulbasDozgulbas
committed
Fix
1 parent 47b699d commit 3196339

1 file changed

Lines changed: 181 additions & 57 deletions

File tree

‎tests/openarm_wave_demo.py‎

Lines changed: 181 additions & 57 deletions
Original file line numberDiff line numberDiff line change
@@ -1,68 +1,192 @@
1-
#!/usr/bin/env python3
2-
"""Test IK with reachable positions."""
1+
#!/usr/bin/env python
2+
"""
3+
OpenArm Wave Demo - Right arm waves by rotating wrist (J6)
34
4-
import numpy as np
5-
from openarm.kinematics.inverse.ikpy import IkpyInverseKinematics
6-
7-
print("=== Testing Reachable Positions ===\n")
5+
Usage:
6+
python wave_demo.py # Default: 20 waves, 30°, 60°/s
7+
python wave_demo.py --waves 10 # 10 waves
8+
python wave_demo.py --angle 40 --speed 90 # Faster, wider waves
9+
"""
810

9-
ik_solver = IkpyInverseKinematics()
10-
11-
# First, let's see where the arm is at some known joint configurations
12-
print("=== Forward Kinematics Test ===")
13-
print("Testing where the end-effector is at various joint angles:\n")
11+
import time
12+
import argparse
13+
import numpy as np
14+
from lerobot.robots.bi_openarm_follower import BiOpenArmFollower
15+
from lerobot.robots.bi_openarm_follower.config_bi_openarm_follower import BiOpenArmFollowerConfig
16+
from lerobot.robots.openarm_follower.config_openarm_follower import OpenArmFollowerConfig
1417

15-
test_configs = [
16-
("All zeros", np.zeros(7)),
17-
("J1=45°", np.deg2rad([45, 0, 0, 0, 0, 0, 0])),
18-
("J2=45°", np.deg2rad([0, 45, 0, 0, 0, 0, 0])),
19-
("J4=90°", np.deg2rad([0, 0, 0, 90, 0, 0, 0])),
20-
("Home pose", np.deg2rad([0, -30, 0, 60, 0, 0, 0])),
21-
]
18+
# Raised position for right arm
19+
RAISED_POSITION = {
20+
'joint_1': 13.34,
21+
'joint_2': 84.02,
22+
'joint_3': 62.29,
23+
'joint_4': 90.57,
24+
'joint_5': -78.25,
25+
'joint_6': 0.00,
26+
'joint_7': 0.00,
27+
'gripper': 0.00,
28+
}
2229

23-
reachable_positions = []
30+
# Zero/rest position
31+
ZERO_POSITION = {
32+
'joint_1': 0.0,
33+
'joint_2': 0.0,
34+
'joint_3': 0.0,
35+
'joint_4': 0.0,
36+
'joint_5': 0.0,
37+
'joint_6': 0.0,
38+
'joint_7': 0.0,
39+
'gripper': 0.0,
40+
}
2441

25-
for name, joint_angles in test_configs:
26-
# Build full joint array (12 links total)
27-
joint_angles_full = np.zeros(12)
28-
joint_angles_full[1:8] = joint_angles
42+
def move_smooth(robot, arm, start_pos, end_pos, duration=1.0, fps=60):
43+
"""Move arm smoothly from start to end position."""
44+
num_steps = int(duration * fps)
45+
dt = 1.0 / fps
46+
other_arm = 'left' if arm == 'right' else 'right'
2947

30-
# Compute FK
31-
fk_matrix = ik_solver._right_chain.forward_kinematics(joint_angles_full)
32-
pos = fk_matrix[:3, 3]
48+
# Get state
49+
state = robot.get_observation()
3350

34-
print(f"{name:15s}: x={pos[0]:>7.3f}, y={pos[1]:>7.3f}, z={pos[2]:>7.3f}")
35-
reachable_positions.append((name, pos))
51+
# Build arrays
52+
start = np.array([start_pos[f'joint_{i}'] for i in range(1, 8)])
53+
start = np.append(start, start_pos['gripper'])
54+
55+
end = np.array([end_pos[f'joint_{i}'] for i in range(1, 8)])
56+
end = np.append(end, end_pos['gripper'])
57+
58+
for step in range(num_steps + 1):
59+
t = step / num_steps
60+
smooth_t = 0.5 - 0.5 * np.cos(t * np.pi)
61+
pos = start + smooth_t * (end - start)
62+
63+
action = {}
64+
# Keep other arm still
65+
for i in range(1, 8):
66+
action[f"{other_arm}_joint_{i}.pos"] = state[f"{other_arm}_joint_{i}.pos"]
67+
action[f"{other_arm}_gripper.pos"] = state[f"{other_arm}_gripper.pos"]
68+
69+
# Move active arm
70+
for i in range(7):
71+
action[f"{arm}_joint_{i+1}.pos"] = float(pos[i])
72+
action[f"{arm}_gripper.pos"] = float(pos[7])
73+
74+
robot.send_action(action)
75+
time.sleep(dt)
3676

37-
print("\n=== Inverse Kinematics Test ===")
38-
print("Now testing IK to reach those same positions:\n")
77+
def wave_j6(robot, base_position, wave_angle=30.0, num_waves=20, speed=60):
78+
"""
79+
Wave by oscillating J6 (wrist pitch).
80+
81+
Args:
82+
robot: BiOpenArmFollower instance
83+
base_position: Base position dict
84+
wave_angle: Max angle to wave (degrees)
85+
num_waves: Number of complete waves
86+
speed: Wave speed in degrees/second
87+
"""
88+
# Calculate duration for one half-wave
89+
half_wave_duration = wave_angle / speed
90+
91+
# Create wave positions
92+
wave_left = base_position.copy()
93+
wave_left['joint_6'] = -wave_angle
94+
95+
wave_right = base_position.copy()
96+
wave_right['joint_6'] = wave_angle
97+
98+
print(f"Waving J6 between -{wave_angle}° and +{wave_angle}° ({num_waves} waves)...")
99+
100+
for i in range(num_waves):
101+
# Wave left
102+
move_smooth(robot, 'right', base_position if i == 0 else wave_right, wave_left, duration=half_wave_duration)
103+
# Wave right
104+
move_smooth(robot, 'right', wave_left, wave_right, duration=half_wave_duration)
105+
106+
if (i + 1) % 5 == 0:
107+
print(f" Completed {i + 1}/{num_waves} waves")
108+
109+
# Return to center
110+
move_smooth(robot, 'right', wave_right, base_position, duration=half_wave_duration)
39111

40-
for name, target_pos in reachable_positions:
41-
print(f"Target: {name} -> x={target_pos[0]:.3f}, y={target_pos[1]:.3f}, z={target_pos[2]:.3f}")
42-
43-
# Solve IK
44-
joint_solution = ik_solver.solve_right_arm(target_pos)
45-
joint_solution_deg = np.rad2deg(joint_solution)
46-
47-
# Check if solution is valid (not all zeros)
48-
if np.allclose(joint_solution, 0, atol=1e-3):
49-
print(" ✗ IK FAILED (returned zeros)")
50-
else:
51-
print(f" ✓ IK SUCCESS: {joint_solution_deg}")
112+
def main(num_waves=20, wave_angle=30.0, speed=60):
113+
"""Run wave demo with specified parameters."""
114+
print("="*60)
115+
print("OpenArm Simple Wave Demo - Right Arm J6 Wave")
116+
print("="*60)
117+
print(f"Parameters: {num_waves} waves, ±{wave_angle}°, {speed}°/s")
118+
print("="*60)
119+
120+
# Create robot configuration
121+
left_config = OpenArmFollowerConfig(port='can1', side='left')
122+
right_config = OpenArmFollowerConfig(port='can0', side='right')
123+
config = BiOpenArmFollowerConfig(
124+
left_arm_config=left_config,
125+
right_arm_config=right_config
126+
)
127+
128+
# Initialize robot
129+
print("\nConnecting to robot...")
130+
robot = BiOpenArmFollower(config)
131+
robot.connect()
132+
print("✓ Robot connected")
133+
134+
try:
135+
# Simple wave demo
136+
print("\n[1/3] Raising right arm to wave position...")
137+
state = robot.get_observation()
138+
current_pos = {f'joint_{i}': state[f"right_joint_{i}.pos"] for i in range(1, 8)}
139+
current_pos['gripper'] = state['right_gripper.pos']
140+
141+
move_smooth(robot, 'right', current_pos, RAISED_POSITION, duration=3.0)
142+
print("✓ Right arm raised")
143+
time.sleep(0.5)
144+
145+
print(f"\n[2/3] Waving {num_waves} times at {speed}°/s...")
146+
wave_j6(robot, RAISED_POSITION, wave_angle=wave_angle, num_waves=num_waves, speed=speed)
147+
print("✓ Wave complete")
148+
time.sleep(0.5)
52149

53-
# Verify with FK
54-
joint_full = np.zeros(12)
55-
joint_full[1:8] = joint_solution
56-
fk_check = ik_solver._right_chain.forward_kinematics(joint_full)
57-
pos_check = fk_check[:3, 3]
58-
error = np.linalg.norm(pos_check - target_pos)
59-
print(f" Verification: x={pos_check[0]:.3f}, y={pos_check[1]:.3f}, z={pos_check[2]:.3f}")
60-
print(f" Error: {error*1000:.2f}mm")
61-
print()
150+
print("\n[3/3] Returning to zero position...")
151+
move_smooth(robot, 'right', RAISED_POSITION, ZERO_POSITION, duration=3.0)
152+
print("✓ Returned to zero")
153+
154+
print("\n" + "="*60)
155+
print("✓ Demo complete!")
156+
print("="*60)
157+
158+
except KeyboardInterrupt:
159+
print("\n\nDemo interrupted by user")
160+
161+
finally:
162+
print("\nDisconnecting robot...")
163+
robot.disconnect()
164+
print("✓ Robot disconnected")
62165

63-
print("\n=== Workspace Analysis ===")
64-
print("Based on the FK results, the right arm workspace appears to be:")
65-
all_positions = np.array([pos for _, pos in reachable_positions])
66-
print(f" X range: {all_positions[:, 0].min():.3f} to {all_positions[:, 0].max():.3f}")
67-
print(f" Y range: {all_positions[:, 1].min():.3f} to {all_positions[:, 1].max():.3f}")
68-
print(f" Z range: {all_positions[:, 2].min():.3f} to {all_positions[:, 2].max():.3f}")
166+
if __name__ == "__main__":
167+
parser = argparse.ArgumentParser(
168+
description='OpenArm wave demo - makes right arm wave using J6',
169+
formatter_class=argparse.RawDescriptionHelpFormatter,
170+
epilog="""
171+
Examples:
172+
python wave_demo.py # Default: 20 waves, 30°, 60°/s
173+
python wave_demo.py --waves 10 # Quick demo with 10 waves
174+
python wave_demo.py --angle 40 --speed 90 # Faster, wider waves
175+
"""
176+
)
177+
178+
parser.add_argument('--waves', type=int, default=5, help='Number of complete waves (default: 5)')
179+
parser.add_argument('--angle', type=float, default=30.0, help='Wave angle in degrees (default: 30.0)')
180+
parser.add_argument('--speed', type=float, default=30.0, help='Wave speed in degrees/second (default: 30.0)')
181+
182+
args = parser.parse_args()
183+
184+
# Validate parameters
185+
if args.waves < 1:
186+
parser.error("--waves must be at least 1")
187+
if args.angle <= 0 or args.angle > 40:
188+
parser.error("--angle must be between 0 and 40 degrees (J6 limit is ±40°)")
189+
if args.speed <= 0 or args.speed > 120:
190+
parser.error("--speed must be between 0 and 120 degrees/second")
191+
192+
main(num_waves=args.waves, wave_angle=args.angle, speed=args.speed)

0 commit comments

Comments
 (0)