Trying VLA (Part 4): Testing Teleoperation from the Command Line
In the previous article, I completed the assembly and calibration of the LeRobot SO-101.
This time, I will send commands to each joint individually and check whether the robot moves correctly.
First, let's check the current state of the robot.
Before doing that, connect the control PC to the Waveshare board on the LeRobot arm, and connect the 12V power supply as well.
For safety, I also keep the robot arm folded before starting.
Next, activate the Python virtual environment.
$ source ~/pyenv/lerobot/bin/activate
Now, let's check the current position of each joint.
This time, I will run the Python code directly from the command line. Of course, you could also save the same code in a Python file and execute it.
(lerobot)$ python - <<'PY'
from lerobot.robots.so_follower import SOFollower, SOFollowerRobotConfig
robot = SOFollower(
SOFollowerRobotConfig(
port="/dev/ttyACM0",
id="takeo_so101",
use_degrees=True,
)
)
robot.connect()
try:
obs = robot.get_observation()
for name, value in obs.items():
if name.endswith(".pos"):
print(f"{name:22s} = {value:.2f}")
finally:
robot.disconnect()
PY
If everything is working correctly, you should see values similar to the following:
shoulder_pan.pos = 5.67
shoulder_lift.pos = -102.59
elbow_flex.pos = 97.63
wrist_flex.pos = 22.77
wrist_roll.pos = 2.07
gripper.pos = 20.16
One thing worth mentioning here is what happens after calibration or after running a Python script that only reads the joint positions.
At that point, you can still move the arm manually.
However, if the arm is extended forward, it may suddenly drop downward under its own weight.
This happens because the default setting is:
disable_torque_on_disconnect=True
When the robot disconnects, the servo torque is turned off.
As a result, joints such as shoulder_lift, elbow_flex, and wrist_flex can no longer hold the weight of the arm against gravity.
The SOFollower follows this setting and disables torque when it disconnects.
Now let's create a small script that moves each motor only a little so that we can verify that every joint works correctly.
Testing Each Motor
Create the following file:
test_motor.py
#!/usr/bin/env python3
import sys
import time
from lerobot.robots.so_follower import SOFollower, SOFollowerRobotConfig
JOINTS = {
1: "shoulder_pan.pos",
2: "shoulder_lift.pos",
3: "elbow_flex.pos",
4: "wrist_flex.pos",
5: "wrist_roll.pos",
6: "gripper.pos",
}
def print_positions(title, obs):
print()
print("=" * 55)
print(title)
print("=" * 55)
for motor_id, joint in JOINTS.items():
value = obs[joint]
if motor_id == 6:
print(f"ID{motor_id} {joint:20s} = {value:8.2f}")
else:
print(f"ID{motor_id} {joint:20s} = {value:8.2f} deg")
print("=" * 55)
def main():
if len(sys.argv) != 3:
print("Usage:")
print(" python test_motor.py MOTOR_ID DELTA")
print()
print("Examples:")
print(" python test_motor.py 1 5")
print(" python test_motor.py 2 -2")
print(" python test_motor.py 6 5")
sys.exit(1)
motor_id = int(sys.argv[1])
delta = float(sys.argv[2])
if motor_id not in JOINTS:
print("ERROR: MOTOR_ID must be 1 - 6")
sys.exit(1)
# Do not move the joint too far during the initial motion test.
if abs(delta) > 10:
print("ERROR: For safety, DELTA must be between -10 and +10.")
sys.exit(1)
joint = JOINTS[motor_id]
robot = SOFollower(
SOFollowerRobotConfig(
port="/dev/ttyACM0",
id="takeo_so101",
use_degrees=True,
)
)
robot.connect()
try:
# -------------------------------------------------
# 1. Read the current positions.
# -------------------------------------------------
robot.bus.disable_torque()
before = robot.get_observation()
print_positions("BEFORE", before)
current = before[joint]
target_value = current + delta
print()
print(f"Selected motor : ID{motor_id}")
print(f"Joint : {joint}")
print(f"Current : {current:.2f}")
print(f"Delta : {delta:+.2f}")
print(f"Target : {target_value:.2f}")
print()
# -------------------------------------------------
# 2. Set the current position as the goal position
# for every motor.
# This prevents sudden jumps when torque is enabled.
# -------------------------------------------------
hold_action = {
name: before[name]
for name in JOINTS.values()
}
robot.send_action(hold_action)
input(
"Support the arm and make sure it is safe.\n"
"Press ENTER to enable torque and move the motor..."
)
# -------------------------------------------------
# 3. Torque ON
# -------------------------------------------------
robot.bus.enable_torque()
time.sleep(0.5)
# -------------------------------------------------
# 4. Change the target only for the selected motor.
# -------------------------------------------------
action = hold_action.copy()
action[joint] = target_value
print()
print(
f">>> Moving ID{motor_id} "
f"{joint}: {current:.2f} -> {target_value:.2f}"
)
robot.send_action(action)
# Wait briefly for the motor to move.
time.sleep(2.0)
# -------------------------------------------------
# 5. Display the position after the movement.
# -------------------------------------------------
after = robot.get_observation()
print_positions("AFTER", after)
print()
print(f"Requested target : {target_value:.2f}")
print(f"Actual position : {after[joint]:.2f}")
print()
input("Press ENTER to disable torque and exit...")
finally:
robot.disconnect()
print()
print("Torque OFF / disconnected.")
if __name__ == "__main__":
main()
Now let's actually run it.
I tested each motor using the following commands:
# ID1: Shoulder pan, +5°
python test_motor.py 1 5
# ID2: Shoulder lift, +2°
python test_motor.py 2 2
# ID3: Elbow, -5°
python test_motor.py 3 -5
# ID4: Wrist flex, +2°
python test_motor.py 4 2
# ID5: Wrist roll, +5°
python test_motor.py 5 5
# ID6: Gripper open/close, +5
python test_motor.py 6 5
When you run these commands, each motor moves only a small amount.
For Motor 3, I used a negative value to move it in the desired direction.
Also, once torque is turned off, the arm drops again under gravity.
During actual teleoperation, we normally keep the torque enabled so that the robot can maintain its posture instead of collapsing under its own weight.
Here are the actual results from my tests.
Motor 1: Shoulder Pan
(lerobot)$ python test_motor.py 1 5
=======================================================
BEFORE
=======================================================
ID1 shoulder_pan.pos = 10.33 deg
ID2 shoulder_lift.pos = -91.60 deg
ID3 elbow_flex.pos = 98.15 deg
ID4 wrist_flex.pos = 24.88 deg
ID5 wrist_roll.pos = 6.99 deg
ID6 gripper.pos = 24.70
=======================================================
Selected motor : ID1
Joint : shoulder_pan.pos
Current : 10.33
Delta : +5.00
Target : 15.33
Support the arm and make sure it is safe.
Press ENTER to enable torque and move the motor...
>>> Moving ID1 shoulder_pan.pos: 10.33 -> 15.33
=======================================================
AFTER
=======================================================
ID1 shoulder_pan.pos = 15.08 deg
ID2 shoulder_lift.pos = -91.60 deg
ID3 elbow_flex.pos = 97.89 deg
ID4 wrist_flex.pos = 24.88 deg
ID5 wrist_roll.pos = 6.99 deg
ID6 gripper.pos = 24.70
=======================================================
Requested target : 15.33
Actual position : 15.08
Press ENTER to disable torque and exit...
Torque OFF / disconnected.
Motor 1 moved from 10.33° to 15.08°, very close to the requested target of 15.33°.
Motor 2: Shoulder Lift
(lerobot)$ python test_motor.py 2 2
=======================================================
BEFORE
=======================================================
ID1 shoulder_pan.pos = 15.08 deg
ID2 shoulder_lift.pos = -91.60 deg
ID3 elbow_flex.pos = 97.89 deg
ID4 wrist_flex.pos = 24.88 deg
ID5 wrist_roll.pos = 6.99 deg
ID6 gripper.pos = 24.70
=======================================================
Selected motor : ID2
Joint : shoulder_lift.pos
Current : -91.60
Delta : +2.00
Target : -89.60
Support the arm and make sure it is safe.
Press ENTER to enable torque and move the motor...
>>> Moving ID2 shoulder_lift.pos: -91.60 -> -89.60
=======================================================
AFTER
=======================================================
ID1 shoulder_pan.pos = 15.08 deg
ID2 shoulder_lift.pos = -89.67 deg
ID3 elbow_flex.pos = 97.89 deg
ID4 wrist_flex.pos = 24.88 deg
ID5 wrist_roll.pos = 6.99 deg
ID6 gripper.pos = 24.70
=======================================================
Requested target : -89.60
Actual position : -89.67
Press ENTER to disable torque and exit...
Torque OFF / disconnected.
Motor 2 also reached almost exactly the requested position.
Motor 3: Elbow Flex
(lerobot)$ python test_motor.py 3 -5
=======================================================
BEFORE
=======================================================
ID1 shoulder_pan.pos = 15.08 deg
ID2 shoulder_lift.pos = -89.67 deg
ID3 elbow_flex.pos = 97.89 deg
ID4 wrist_flex.pos = 24.88 deg
ID5 wrist_roll.pos = 6.99 deg
ID6 gripper.pos = 24.70
=======================================================
Selected motor : ID3
Joint : elbow_flex.pos
Current : 97.89
Delta : -5.00
Target : 92.89
Support the arm and make sure it is safe.
Press ENTER to enable torque and move the motor...
>>> Moving ID3 elbow_flex.pos: 97.89 -> 92.89
=======================================================
AFTER
=======================================================
ID1 shoulder_pan.pos = 14.99 deg
ID2 shoulder_lift.pos = -89.76 deg
ID3 elbow_flex.pos = 95.08 deg
ID4 wrist_flex.pos = 24.88 deg
ID5 wrist_roll.pos = 6.99 deg
ID6 gripper.pos = 24.70
=======================================================
Requested target : 92.89
Actual position : 95.08
Press ENTER to disable torque and exit...
Torque OFF / disconnected.
Motor 3 also moved in the expected direction, although the actual position did not reach the requested target as closely as some of the other joints.
Motor 4: Wrist Flex
(lerobot)$ python test_motor.py 4 2
=======================================================
BEFORE
=======================================================
ID1 shoulder_pan.pos = 15.08 deg
ID2 shoulder_lift.pos = -89.76 deg
ID3 elbow_flex.pos = 98.07 deg
ID4 wrist_flex.pos = 25.05 deg
ID5 wrist_roll.pos = 6.99 deg
ID6 gripper.pos = 24.70
=======================================================
Selected motor : ID4
Joint : wrist_flex.pos
Current : 25.05
Delta : +2.00
Target : 27.05
Support the arm and make sure it is safe.
Press ENTER to enable torque and move the motor...
>>> Moving ID4 wrist_flex.pos: 25.05 -> 27.05
=======================================================
AFTER
=======================================================
ID1 shoulder_pan.pos = 15.08 deg
ID2 shoulder_lift.pos = -89.76 deg
ID3 elbow_flex.pos = 97.89 deg
ID4 wrist_flex.pos = 26.73 deg
ID5 wrist_roll.pos = 6.99 deg
ID6 gripper.pos = 24.70
=======================================================
Requested target : 27.05
Actual position : 26.73
Press ENTER to disable torque and exit...
Torque OFF / disconnected.
Motor 4 moved from 25.05° to 26.73°, close to the requested target of 27.05°.
Motor 5: Wrist Roll
(lerobot)$ python test_motor.py 5 5
=======================================================
BEFORE
=======================================================
ID1 shoulder_pan.pos = 15.08 deg
ID2 shoulder_lift.pos = -89.76 deg
ID3 elbow_flex.pos = 97.89 deg
ID4 wrist_flex.pos = 26.73 deg
ID5 wrist_roll.pos = 6.99 deg
ID6 gripper.pos = 24.70
=======================================================
Selected motor : ID5
Joint : wrist_roll.pos
Current : 6.99
Delta : +5.00
Target : 11.99
Support the arm and make sure it is safe.
Press ENTER to enable torque and move the motor...
>>> Moving ID5 wrist_roll.pos: 6.99 -> 11.99
=======================================================
AFTER
=======================================================
ID1 shoulder_pan.pos = 15.08 deg
ID2 shoulder_lift.pos = -89.76 deg
ID3 elbow_flex.pos = 97.89 deg
ID4 wrist_flex.pos = 26.73 deg
ID5 wrist_roll.pos = 11.56 deg
ID6 gripper.pos = 24.70
=======================================================
Requested target : 11.99
Actual position : 11.56
Press ENTER to disable torque and exit...
Torque OFF / disconnected.
Motor 5 also moved correctly.
Motor 6: Gripper
(lerobot)$ python test_motor.py 6 5
=======================================================
BEFORE
=======================================================
ID1 shoulder_pan.pos = 15.08 deg
ID2 shoulder_lift.pos = -89.76 deg
ID3 elbow_flex.pos = 97.89 deg
ID4 wrist_flex.pos = 26.73 deg
ID5 wrist_roll.pos = 11.56 deg
ID6 gripper.pos = 24.70
=======================================================
Selected motor : ID6
Joint : gripper.pos
Current : 24.70
Delta : +5.00
Target : 29.70
Support the arm and make sure it is safe.
Press ENTER to enable torque and move the motor...
>>> Moving ID6 gripper.pos: 24.70 -> 29.70
=======================================================
AFTER
=======================================================
ID1 shoulder_pan.pos = 15.08 deg
ID2 shoulder_lift.pos = -89.76 deg
ID3 elbow_flex.pos = 97.89 deg
ID4 wrist_flex.pos = 26.73 deg
ID5 wrist_roll.pos = 11.56 deg
ID6 gripper.pos = 29.31
=======================================================
Requested target : 29.70
Actual position : 29.31
Press ENTER to disable torque and exit...
Torque OFF / disconnected.
The gripper also responded correctly.
With this test, I was able to confirm that the assembly, wiring, motor ID assignment, and calibration were basically working correctly.
All six motors responded to commands and moved in the expected way.
So now the SO-101 is finally ready for actual operation.
For my setup, however, I am not planning to use a leader arm.
Instead, I want to try controlling the SO-101 using a 3D mouse (SpaceMouse).
That will be the next step.
Top comments (0)