Skip to content

Commit cbe47f2

Browse files
DozgulbasDozgulbas
committed
Helper scripts
1 parent 7f6da7c commit cbe47f2

3 files changed

Lines changed: 336 additions & 0 deletions

File tree

‎scripts/move_to_zero.py‎

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
#!/usr/bin/env python3
2+
import openarm_can as oa
3+
import time
4+
5+
print("=== Moving arms to zero (slow and smooth) ===")
6+
7+
# Reduced gains for slower, smoother movement
8+
# Original LeRobot: kp=[240, 240, 240, 240, 24, 31, 25, 25], kd=[5, 5, 3, 5, 0.3, 0.3, 0.3, 0.3]
9+
# Slower version: reduce kp by ~75%, keep kd proportional
10+
kp_values = [60.0, 60.0, 60.0, 60.0, 6.0, 8.0, 6.0, 6.0] # ~25% of original
11+
kd_values = [2.0, 2.0, 1.5, 2.0, 0.2, 0.2, 0.2, 0.2] # Reduced proportionally
12+
13+
right_arm = oa.OpenArm("can0", True)
14+
left_arm = oa.OpenArm("can1", True)
15+
16+
motor_types = [
17+
oa.MotorType.DM8009, oa.MotorType.DM8009,
18+
oa.MotorType.DM4340, oa.MotorType.DM4340,
19+
oa.MotorType.DM4310, oa.MotorType.DM4310,
20+
oa.MotorType.DM4310
21+
]
22+
send_ids = [0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07]
23+
recv_ids = [0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17]
24+
25+
right_arm.init_arm_motors(motor_types, send_ids, recv_ids)
26+
right_arm.init_gripper_motor(oa.MotorType.DM4310, 0x08, 0x18)
27+
left_arm.init_arm_motors(motor_types, send_ids, recv_ids)
28+
left_arm.init_gripper_motor(oa.MotorType.DM4310, 0x08, 0x18)
29+
30+
right_arm.set_callback_mode_all(oa.CallbackMode.IGNORE)
31+
left_arm.set_callback_mode_all(oa.CallbackMode.IGNORE)
32+
right_arm.enable_all()
33+
left_arm.enable_all()
34+
right_arm.recv_all()
35+
left_arm.recv_all()
36+
37+
print("Moving to zero position (slowly)...")
38+
right_arm.set_callback_mode_all(oa.CallbackMode.STATE)
39+
left_arm.set_callback_mode_all(oa.CallbackMode.STATE)
40+
41+
zero_params = [
42+
oa.MITParam(kp_values[i], kd_values[i], 0.0, 0, 0)
43+
for i in range(7)
44+
]
45+
gripper_zero = [oa.MITParam(kp_values[7], kd_values[7], 0.0, 0, 0)]
46+
47+
for step in range(600): # Double the time (10 seconds instead of 5)
48+
right_arm.get_arm().mit_control_all(zero_params)
49+
right_arm.get_gripper().mit_control_all(gripper_zero)
50+
left_arm.get_arm().mit_control_all(zero_params)
51+
left_arm.get_gripper().mit_control_all(gripper_zero)
52+
right_arm.recv_all()
53+
left_arm.recv_all()
54+
time.sleep(1.0/60.0)
55+
56+
print("✓ At zero! Disabling...")
57+
right_arm.disable_all()
58+
left_arm.disable_all()
59+
right_arm.recv_all(1000)
60+
left_arm.recv_all(1000)

‎scripts/sync_calibration.py‎

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
#!/usr/bin/env python3
2+
import openarm_can as oa
3+
import time
4+
5+
print("=== OpenArm Hardware-Synced Calibration ===")
6+
7+
# Step 1: Move to hardware zero
8+
print("\nStep 1: Moving arms to hardware-calibrated zero...")
9+
10+
right_arm = oa.OpenArm("can0", True)
11+
left_arm = oa.OpenArm("can1", True)
12+
13+
motor_types = [
14+
oa.MotorType.DM8009, oa.MotorType.DM8009,
15+
oa.MotorType.DM4340, oa.MotorType.DM4340,
16+
oa.MotorType.DM4310, oa.MotorType.DM4310,
17+
oa.MotorType.DM4310
18+
]
19+
send_ids = [0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07]
20+
recv_ids = [0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17]
21+
22+
right_arm.init_arm_motors(motor_types, send_ids, recv_ids)
23+
right_arm.init_gripper_motor(oa.MotorType.DM4310, 0x08, 0x18)
24+
left_arm.init_arm_motors(motor_types, send_ids, recv_ids)
25+
left_arm.init_gripper_motor(oa.MotorType.DM4310, 0x08, 0x18)
26+
27+
right_arm.set_callback_mode_all(oa.CallbackMode.IGNORE)
28+
left_arm.set_callback_mode_all(oa.CallbackMode.IGNORE)
29+
right_arm.enable_all()
30+
left_arm.enable_all()
31+
right_arm.recv_all()
32+
left_arm.recv_all()
33+
34+
right_arm.set_callback_mode_all(oa.CallbackMode.STATE)
35+
left_arm.set_callback_mode_all(oa.CallbackMode.STATE)
36+
37+
zero_params = [oa.MITParam(10.0, 2.0, 0.0, 0, 0) for _ in range(7)]
38+
gripper_zero = [oa.MITParam(10.0, 2.0, 0.0, 0, 0)]
39+
40+
for step in range(300):
41+
right_arm.get_arm().mit_control_all(zero_params)
42+
right_arm.get_gripper().mit_control_all(gripper_zero)
43+
left_arm.get_arm().mit_control_all(zero_params)
44+
left_arm.get_gripper().mit_control_all(gripper_zero)
45+
right_arm.recv_all()
46+
left_arm.recv_all()
47+
time.sleep(1.0/60.0)
48+
49+
print("✓ Arms at hardware zero!")
50+
51+
# Disable C++ interface
52+
print("\nDisabling C++ interface...")
53+
right_arm.disable_all()
54+
left_arm.disable_all()
55+
right_arm.recv_all(1000)
56+
left_arm.recv_all(1000)
57+
time.sleep(0.5)
58+
59+
# Step 2: Run LeRobot calibration
60+
print("\nStep 2: Running LeRobot calibration at hardware zero...")
61+
62+
from lerobot.robots.bi_openarm_follower.config_bi_openarm_follower import BiOpenArmFollowerConfig
63+
from lerobot.robots.openarm_follower.config_openarm_follower import OpenArmFollowerConfig
64+
from lerobot.robots import make_robot_from_config
65+
66+
# Create config
67+
left_config = OpenArmFollowerConfig(port='can1', side='left')
68+
right_config = OpenArmFollowerConfig(port='can0', side='right')
69+
config = BiOpenArmFollowerConfig(left_arm_config=left_config, right_arm_config=right_config)
70+
71+
# Create robot
72+
robot = make_robot_from_config(config)
73+
74+
# Connect WITHOUT calibration (arms already at zero)
75+
robot.connect(calibrate=False)
76+
77+
try:
78+
# Run calibration (will set current position as zero and save)
79+
robot.calibrate()
80+
finally:
81+
robot.disconnect()
82+
83+
print("\n✓ COMPLETE! LeRobot calibrated at hardware zero.")
84+
print("Test with: python ~/reset_to_zero_working.py")

‎tests/openarm_wave_demo.py‎

Lines changed: 192 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,192 @@
1+
#!/usr/bin/env python
2+
"""
3+
OpenArm Wave Demo - Right arm waves by rotating wrist (J6)
4+
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+
"""
10+
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
17+
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+
}
29+
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+
}
41+
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'
47+
48+
# Get state
49+
state = robot.get_observation()
50+
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)
76+
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)
111+
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)
149+
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")
165+
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=20, help='Number of complete waves (default: 20)')
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=60.0, help='Wave speed in degrees/second (default: 60.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)