mirror of
https://github.com/huggingface/lerobot.git
synced 2026-06-26 20:57:28 +00:00
Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 Pepijn
|
93bc43771b |
@@ -1,140 +0,0 @@
|
||||
import time
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
from os.path import dirname
|
||||
|
||||
from lerobot.teleoperators.openarms.openarms_leader import OpenArmsLeader
|
||||
from lerobot.teleoperators.openarms.config_openarms_leader import OpenArmsLeaderConfig
|
||||
|
||||
|
||||
same_direction = {"joint_4", "gripper"}
|
||||
|
||||
idx = {
|
||||
"joint_1": 0,
|
||||
"joint_2": 1,
|
||||
"joint_3": 2,
|
||||
"joint_4": 3,
|
||||
"joint_5": 4,
|
||||
"joint_6": 5,
|
||||
"joint_7": 6,
|
||||
"gripper": 7,
|
||||
}
|
||||
|
||||
# joints to freeze
|
||||
frozen = {"joint_6", "joint_7", "gripper"}
|
||||
initial_pose = {}
|
||||
|
||||
|
||||
def pos_deg(rob, obs):
|
||||
out = {}
|
||||
for side in ("left", "right"):
|
||||
for m in getattr(rob, f"bus_{side}").motors:
|
||||
k = f"{side}_{m}.pos"
|
||||
if k in obs:
|
||||
out[f"{side}_{m}"] = obs[k]
|
||||
return out
|
||||
|
||||
|
||||
def vel_rad(rob, obs):
|
||||
out = {}
|
||||
for side in ("left", "right"):
|
||||
for m in getattr(rob, f"bus_{side}").motors:
|
||||
k = f"{side}_{m}.vel"
|
||||
out[f"{side}_{m}"] = np.deg2rad(obs.get(k, 0.0))
|
||||
return out
|
||||
|
||||
|
||||
def main():
|
||||
cfg = OpenArmsLeaderConfig(
|
||||
port_left="can0",
|
||||
port_right="can1",
|
||||
can_interface="socketcan",
|
||||
id="openarms_bilateral",
|
||||
manual_control=False,
|
||||
)
|
||||
|
||||
rob = OpenArmsLeader(cfg)
|
||||
rob.connect(calibrate=True)
|
||||
|
||||
urdf = "/home/yope/Documents/lerobot_g1_integration/openarm_description/openarm_bimanual_pybullet.urdf"
|
||||
rob.pin_robot = pin.RobotWrapper.BuildFromURDF(urdf, dirname(urdf))
|
||||
rob.pin_robot.data = rob.pin_robot.model.createData()
|
||||
|
||||
dt = 0.005
|
||||
grav = 1.0
|
||||
fric = 0.3
|
||||
|
||||
# capture initial pose to freeze selected joints later
|
||||
obs0 = rob.get_action()
|
||||
for side in ("left", "right"):
|
||||
for m in getattr(rob, f"bus_{side}").motors:
|
||||
key = f"{side}_{m}.pos"
|
||||
if key in obs0 and m in frozen:
|
||||
initial_pose[f"{side}_{m}"] = obs0[key]
|
||||
|
||||
try:
|
||||
while True:
|
||||
obs = rob.get_action()
|
||||
|
||||
pdeg = pos_deg(rob, obs)
|
||||
prad = {k: np.deg2rad(v) for k, v in pdeg.items()}
|
||||
vrad = vel_rad(rob, obs)
|
||||
|
||||
tau_g = rob._gravity_from_q(prad)
|
||||
tau_f = rob._friction_from_velocity(vrad, friction_scale=fric)
|
||||
|
||||
# bilateral midpoint calculation
|
||||
cmd = {}
|
||||
for m in rob.bus_right.motors:
|
||||
kl = f"left_{m}.pos"
|
||||
kr = f"right_{m}.pos"
|
||||
if kl not in obs or kr not in obs:
|
||||
continue
|
||||
|
||||
ql = obs[kl]
|
||||
qr = obs[kr]
|
||||
|
||||
if m in same_direction:
|
||||
qmid = 0.5 * (ql + qr)
|
||||
else:
|
||||
qmid = 0.5 * (ql - qr)
|
||||
|
||||
# assign midpoint for both
|
||||
cmd[f"left_{m}"] = qmid
|
||||
cmd[f"right_{m}"] = qmid if m in same_direction else -qmid
|
||||
|
||||
# override midpoint with frozen values
|
||||
for key, val in initial_pose.items():
|
||||
cmd[key] = val
|
||||
|
||||
# single mit control call
|
||||
for side in ("left", "right"):
|
||||
bus = getattr(rob, f"bus_{side}")
|
||||
for m in bus.motors:
|
||||
base_key = f"{side}_{m}"
|
||||
kp = float(cfg.position_kp[idx[m]])
|
||||
kd = float(cfg.position_kd[idx[m]])
|
||||
torque = tau_g.get(base_key, 0.0) * grav + tau_f.get(base_key, 0.0)
|
||||
pos_cmd = cmd.get(base_key, pdeg.get(base_key, 0.0))
|
||||
|
||||
bus._mit_control(
|
||||
motor=m,
|
||||
kp=kp,
|
||||
kd=kd,
|
||||
position_degrees=pos_cmd,
|
||||
velocity_deg_per_sec=0.0,
|
||||
torque=torque,
|
||||
)
|
||||
|
||||
time.sleep(dt)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
|
||||
rob.bus_left.disable_torque()
|
||||
rob.bus_right.disable_torque()
|
||||
rob.disconnect()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -3,26 +3,28 @@ import numpy as np
|
||||
import pinocchio as pin
|
||||
from os.path import join, dirname, exists, expanduser
|
||||
|
||||
from lerobot.teleoperators.openarms.openarms_leader import OpenArmsLeader
|
||||
from lerobot.teleoperators.openarms.config_openarms_leader import OpenArmsLeaderConfig
|
||||
from lerobot.robots.openarms.openarms_follower import OpenArmsFollower
|
||||
from lerobot.robots.openarms.config_openarms_follower import OpenArmsFollowerConfig
|
||||
|
||||
|
||||
def main() -> None:
|
||||
config = OpenArmsLeaderConfig(
|
||||
config = OpenArmsFollowerConfig(
|
||||
port_left="can0",
|
||||
port_right="can1",
|
||||
can_interface="socketcan",
|
||||
id="openarms_leader",
|
||||
manual_control=False, # Enable torque control for gravity compensation
|
||||
id="openarms_follower",
|
||||
disable_torque_on_disconnect=True,
|
||||
max_relative_target=5.0,
|
||||
)
|
||||
|
||||
|
||||
print("Initializing robot...")
|
||||
follower = OpenArmsLeader(config)
|
||||
follower = OpenArmsFollower(config)
|
||||
follower.connect(calibrate=True)
|
||||
|
||||
# Load URDF for Pinocchio dynamics
|
||||
urdf_path = "/home/yope/Documents/lerobot_g1_integration/openarm_description/openarm_bimanual_pybullet.urdf"
|
||||
urdf_path = "/home/croissant/Documents/openarm_description/openarm_bimanual_pybullet.urdf"
|
||||
|
||||
pin_robot = pin.RobotWrapper.BuildFromURDF(urdf_path, dirname(urdf_path))
|
||||
pin_robot.data = pin_robot.model.createData()
|
||||
print(f"✓ Loaded Pinocchio model with {pin_robot.nq} DoFs")
|
||||
@@ -48,7 +50,7 @@ def main() -> None:
|
||||
loop_start = time.perf_counter()
|
||||
|
||||
# Get current joint positions from robot
|
||||
obs = follower.get_action()
|
||||
obs = follower.get_observation()
|
||||
|
||||
# Extract positions in degrees
|
||||
positions_deg = {}
|
||||
@@ -69,7 +71,8 @@ def main() -> None:
|
||||
|
||||
# Apply gravity compensation to right arm (all joints except gripper)
|
||||
for motor in follower.bus_right.motors:
|
||||
|
||||
if motor == "gripper":
|
||||
continue # Skip gripper
|
||||
|
||||
full_name = f"right_{motor}"
|
||||
position = positions_deg.get(full_name, 0.0)
|
||||
@@ -85,10 +88,10 @@ def main() -> None:
|
||||
torque=torque
|
||||
)
|
||||
|
||||
|
||||
# Apply gravity compensation to left arm (all joints except gripper)
|
||||
for motor in follower.bus_left.motors:
|
||||
|
||||
if motor == "gripper":
|
||||
continue # Skip gripper
|
||||
|
||||
full_name = f"left_{motor}"
|
||||
position = positions_deg.get(full_name, 0.0)
|
||||
@@ -103,7 +106,7 @@ def main() -> None:
|
||||
velocity_deg_per_sec=0.0,
|
||||
torque=torque
|
||||
)
|
||||
|
||||
|
||||
# Measure loop time
|
||||
loop_end = time.perf_counter()
|
||||
loop_time = loop_end - loop_start
|
||||
|
||||
@@ -1,618 +0,0 @@
|
||||
<?xml version='1.0' encoding='utf-8'?>
|
||||
<robot name="openarm">
|
||||
<link name="world" />
|
||||
<joint name="openarm_body_world_joint" type="fixed">
|
||||
<parent link="world" />
|
||||
<child link="openarm_body_link0" />
|
||||
<origin rpy="0 0 0" xyz="0 0 0" />
|
||||
</joint>
|
||||
<link name="openarm_body_link0">
|
||||
<visual name="openarm_body_link0_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/body/v10/visual/body_link0.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_body_link0_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/body/v10/collision/body_link0_symp.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
|
||||
<mass value="13.89" />
|
||||
<inertia ixx="1.653" ixy="0.0" ixz="0.0" iyy="1.653" iyz="0.0" izz="0.051" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_left_openarm_body_link0_joint" type="fixed">
|
||||
<parent link="openarm_body_link0" />
|
||||
<child link="openarm_left_link0" />
|
||||
<origin rpy="-1.5708 0 0" xyz="0.0 0.031 0.698" />
|
||||
</joint>
|
||||
<link name="openarm_left_link0">
|
||||
<visual name="openarm_left_link0_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link0.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_left_link0_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link0_symp.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0009483362816297526 -0.0001580207020448382 0.03076860287587199" />
|
||||
<mass value="1.1432284943239561" />
|
||||
<inertia ixx="0.001128" ixy="-4e-06" ixz="-3.3e-05" iyy="0.000962" iyz="-7e-06" izz="0.00147" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="openarm_left_link1">
|
||||
<visual name="openarm_left_link1_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0 0.0 -0.0625" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link1.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_left_link1_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0 0.0 -0.0625" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link1_symp.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0011467657911800769 -3.319987657026362e-05 0.05395284380736254" />
|
||||
<mass value="1.1416684646202298" />
|
||||
<inertia ixx="0.001567" ixy="-1e-06" ixz="-2.9e-05" iyy="0.001273" iyz="1e-06" izz="0.001016" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_left_joint1" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0625" />
|
||||
<parent link="openarm_left_link0" />
|
||||
<child link="openarm_left_link1" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="40" lower="-3.490659" upper="1.3962629999999998" velocity="16.754666" />
|
||||
</joint>
|
||||
<link name="openarm_left_link2">
|
||||
<visual name="openarm_left_link2_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0301 0.0 -0.1225" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link2.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_left_link2_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0301 0.0 -0.1225" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link2_symp.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.00839629182351943 2.0145102027597523e-08 0.03256649300522363" />
|
||||
<mass value="0.2775092746011571" />
|
||||
<inertia ixx="0.000359" ixy="1e-06" ixz="-0.000109" iyy="0.000376" iyz="1e-06" izz="0.000232" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_left_joint2" type="revolute">
|
||||
<origin rpy="-1.57079632679 0 0" xyz="-0.0301 0.0 0.06" />
|
||||
<parent link="openarm_left_link1" />
|
||||
<child link="openarm_left_link2" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="40" lower="-3.3161253267948965" upper="0.17453267320510335" velocity="16.754666" />
|
||||
</joint>
|
||||
<link name="openarm_left_link3">
|
||||
<visual name="openarm_left_link3_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0 -0.0 -0.18875" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link3.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_left_link3_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0 -0.0 -0.18875" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link3_symp.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.002104752099628911 -0.0005549085042607548 0.09047470545721961" />
|
||||
<mass value="1.073863338202347" />
|
||||
<inertia ixx="0.004372" ixy="1e-06" ixz="1.1e-05" iyy="0.004319" iyz="-3.6e-05" izz="0.000661" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_left_joint3" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0301 0.0 0.06625" />
|
||||
<parent link="openarm_left_link2" />
|
||||
<child link="openarm_left_link3" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="27" lower="-1.570796" upper="1.570796" velocity="5.445426" />
|
||||
</joint>
|
||||
<link name="openarm_left_link4">
|
||||
<visual name="openarm_left_link4_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0315 -0.3425" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link4.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_left_link4_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0315 -0.3425" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link4_symp.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0029006831074562967 -0.03030575826634669 0.06339637422196209" />
|
||||
<mass value="0.6348534566833373" />
|
||||
<inertia ixx="0.000623" ixy="-1e-06" ixz="-1.9e-05" iyy="0.000511" iyz="3.8e-05" izz="0.000334" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_left_joint4" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.0 0.0315 0.15375" />
|
||||
<parent link="openarm_left_link3" />
|
||||
<child link="openarm_left_link4" />
|
||||
<axis xyz="0 1 0" />
|
||||
<limit effort="27" lower="0.0" upper="2.443461" velocity="5.445426" />
|
||||
</joint>
|
||||
<link name="openarm_left_link5">
|
||||
<visual name="openarm_left_link5_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0 -0.0 -0.438" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link5.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_left_link5_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0 -0.0 -0.438" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link5_symp.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.003049665024221911 -0.0008866902457326625 0.043079803024980934" />
|
||||
<mass value="0.6156588026168502" />
|
||||
<inertia ixx="0.000423" ixy="-8e-06" ixz="6e-06" iyy="0.000445" iyz="-6e-06" izz="0.000324" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_left_joint5" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0 -0.0315 0.0955" />
|
||||
<parent link="openarm_left_link4" />
|
||||
<child link="openarm_left_link5" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="7" lower="-1.570796" upper="1.570796" velocity="20.943946" />
|
||||
</joint>
|
||||
<link name="openarm_left_link6">
|
||||
<visual name="openarm_left_link6_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0375 -0.0 -0.5585" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link6.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_left_link6_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0375 -0.0 -0.5585" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link6_symp.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.037136587005447405 -0.00033230528343419053 -9.498374522309838e-05" />
|
||||
<mass value="0.475202773187987" />
|
||||
<inertia ixx="0.000143" ixy="1e-06" ixz="1e-06" iyy="0.000157" iyz="1e-06" izz="0.000159" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_left_joint6" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0375 0.0 0.1205" />
|
||||
<parent link="openarm_left_link5" />
|
||||
<child link="openarm_left_link6" />
|
||||
<axis xyz="1 0 0" />
|
||||
<limit effort="7" lower="-0.785398" upper="0.785398" velocity="20.943946" />
|
||||
</joint>
|
||||
<link name="openarm_left_link7">
|
||||
<visual name="openarm_left_link7_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0 -0.5585" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link7.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_left_link7_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0 -0.5585" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link7_symp.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="6.875510271106056e-05 -0.01266175250761268 0.06951945409987448" />
|
||||
<mass value="0.4659771327380578" />
|
||||
<inertia ixx="0.000639" ixy="1e-06" ixz="1e-06" iyy="0.000497" iyz="8.9e-05" izz="0.000342" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_left_joint7" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.0375 0.0 0.0" />
|
||||
<parent link="openarm_left_link6" />
|
||||
<child link="openarm_left_link7" />
|
||||
<axis xyz="0 -1 0" />
|
||||
<limit effort="7" lower="-1.570796" upper="1.570796" velocity="20.943946" />
|
||||
</joint>
|
||||
<joint name="openarm_right_openarm_body_link0_joint" type="fixed">
|
||||
<parent link="openarm_body_link0" />
|
||||
<child link="openarm_right_link0" />
|
||||
<origin rpy="1.5708 0 0" xyz="0.0 -0.031 0.698" />
|
||||
</joint>
|
||||
<link name="openarm_right_link0">
|
||||
<visual name="openarm_right_link0_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link0.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_right_link0_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link0_symp.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0009483362816297526 0.0001580207020448382 0.03076860287587199" />
|
||||
<mass value="1.1432284943239561" />
|
||||
<inertia ixx="0.001128" ixy="-4e-06" ixz="-3.3e-05" iyy="0.000962" iyz="-7e-06" izz="0.00147" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="openarm_right_link1">
|
||||
<visual name="openarm_right_link1_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0 0.0 -0.0625" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link1.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_right_link1_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0 0.0 -0.0625" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link1_symp.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0011467657911800769 3.319987657026362e-05 0.05395284380736254" />
|
||||
<mass value="1.1416684646202298" />
|
||||
<inertia ixx="0.001567" ixy="-1e-06" ixz="-2.9e-05" iyy="0.001273" iyz="1e-06" izz="0.001016" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_right_joint1" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0625" />
|
||||
<parent link="openarm_right_link0" />
|
||||
<child link="openarm_right_link1" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="40" lower="-1.396263" upper="3.490659" velocity="16.754666" />
|
||||
</joint>
|
||||
<link name="openarm_right_link2">
|
||||
<visual name="openarm_right_link2_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0301 0.0 -0.1225" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link2.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_right_link2_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0301 0.0 -0.1225" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link2_symp.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.00839629182351943 -2.0145102027597523e-08 0.03256649300522363" />
|
||||
<mass value="0.2775092746011571" />
|
||||
<inertia ixx="0.000359" ixy="1e-06" ixz="-0.000109" iyy="0.000376" iyz="1e-06" izz="0.000232" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_right_joint2" type="revolute">
|
||||
<origin rpy="1.57079632679 0 0" xyz="-0.0301 0.0 0.06" />
|
||||
<parent link="openarm_right_link1" />
|
||||
<child link="openarm_right_link2" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="40" lower="-0.17453267320510335" upper="3.3161253267948965" velocity="16.754666" />
|
||||
</joint>
|
||||
<link name="openarm_right_link3">
|
||||
<visual name="openarm_right_link3_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0 -0.0 -0.18875" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link3.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_right_link3_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0 -0.0 -0.18875" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link3_symp.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.002104752099628911 0.0005549085042607548 0.09047470545721961" />
|
||||
<mass value="1.073863338202347" />
|
||||
<inertia ixx="0.004372" ixy="1e-06" ixz="1.1e-05" iyy="0.004319" iyz="-3.6e-05" izz="0.000661" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_right_joint3" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0301 0.0 0.06625" />
|
||||
<parent link="openarm_right_link2" />
|
||||
<child link="openarm_right_link3" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="27" lower="-1.570796" upper="1.570796" velocity="5.445426" />
|
||||
</joint>
|
||||
<link name="openarm_right_link4">
|
||||
<visual name="openarm_right_link4_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0315 -0.3425" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link4.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_right_link4_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0315 -0.3425" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link4_symp.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0029006831074562967 -0.03030575826634669 0.06339637422196209" />
|
||||
<mass value="0.6348534566833373" />
|
||||
<inertia ixx="0.000623" ixy="-1e-06" ixz="-1.9e-05" iyy="0.000511" iyz="3.8e-05" izz="0.000334" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_right_joint4" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.0 0.0315 0.15375" />
|
||||
<parent link="openarm_right_link3" />
|
||||
<child link="openarm_right_link4" />
|
||||
<axis xyz="0 1 0" />
|
||||
<limit effort="27" lower="0.0" upper="2.443461" velocity="5.445426" />
|
||||
</joint>
|
||||
<link name="openarm_right_link5">
|
||||
<visual name="openarm_right_link5_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0 -0.0 -0.438" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link5.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_right_link5_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0 -0.0 -0.438" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link5_symp.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.003049665024221911 0.0008866902457326625 0.043079803024980934" />
|
||||
<mass value="0.6156588026168502" />
|
||||
<inertia ixx="0.000423" ixy="-8e-06" ixz="6e-06" iyy="0.000445" iyz="-6e-06" izz="0.000324" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_right_joint5" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0 -0.0315 0.0955" />
|
||||
<parent link="openarm_right_link4" />
|
||||
<child link="openarm_right_link5" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="7" lower="-1.570796" upper="1.570796" velocity="20.943946" />
|
||||
</joint>
|
||||
<link name="openarm_right_link6">
|
||||
<visual name="openarm_right_link6_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0375 -0.0 -0.5585" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link6.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_right_link6_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.0375 -0.0 -0.5585" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link6_symp.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="-0.037136587005447405 0.00033230528343419053 -9.498374522309838e-05" />
|
||||
<mass value="0.475202773187987" />
|
||||
<inertia ixx="0.000143" ixy="1e-06" ixz="1e-06" iyy="0.000157" iyz="1e-06" izz="0.000159" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_right_joint6" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0375 0.0 0.1205" />
|
||||
<parent link="openarm_right_link5" />
|
||||
<child link="openarm_right_link6" />
|
||||
<axis xyz="1 0 0" />
|
||||
<limit effort="7" lower="-0.785398" upper="0.785398" velocity="20.943946" />
|
||||
</joint>
|
||||
<link name="openarm_right_link7">
|
||||
<visual name="openarm_right_link7_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0 -0.5585" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/visual/link7.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_right_link7_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0 -0.5585" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/arm/v10/collision/link7_symp.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="6.875510271106056e-05 0.01266175250761268 0.06951945409987448" />
|
||||
<mass value="0.4659771327380578" />
|
||||
<inertia ixx="0.000639" ixy="1e-06" ixz="1e-06" iyy="0.000497" iyz="8.9e-05" izz="0.000342" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_right_joint7" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.0375 0.0 0.0" />
|
||||
<parent link="openarm_right_link6" />
|
||||
<child link="openarm_right_link7" />
|
||||
<axis xyz="0 1 0" />
|
||||
<limit effort="7" lower="-1.570796" upper="1.570796" velocity="20.943946" />
|
||||
</joint>
|
||||
<link name="openarm_left_hand">
|
||||
<visual name="openarm_left_hand_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.6585" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/ee/openarm_hand/visual/hand.dae" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_left_hand_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.6585" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/ee/openarm_hand/collision/hand.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.002 0.03" />
|
||||
<mass value="0.35" />
|
||||
<inertia ixx="0.0002473" ixy="1e-06" ixz="1e-06" iyy="1.763e-05" iyz="1e-06" izz="0.0002521" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="left_openarm_hand_joint" type="fixed">
|
||||
<parent link="openarm_left_link7" />
|
||||
<child link="openarm_left_hand" />
|
||||
<origin rpy="0 0 0" xyz="0 -0.0 0.1001" />
|
||||
</joint>
|
||||
<link name="openarm_left_hand_tcp">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.001" />
|
||||
<inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000001" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_left_hand_tcp_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 -0.0 0.08" />
|
||||
<parent link="openarm_left_hand" />
|
||||
<child link="openarm_left_hand_tcp" />
|
||||
</joint>
|
||||
<link name="openarm_left_left_finger">
|
||||
<visual name="openarm_left_left_finger_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.05 -0.673001" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/ee/openarm_hand/visual/finger.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_left_left_finger_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.05 -0.673001" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/ee/openarm_hand/collision/finger.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0064528 0.01702 0.0219685" />
|
||||
<mass value="0.03602545343277134" />
|
||||
<inertia ixx="2.3749999999999997e-06" ixy="1e-06" ixz="1e-06" iyy="2.3749999999999997e-06" iyz="1e-06" izz="7.5e-07" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="openarm_left_right_finger">
|
||||
<visual name="openarm_left_right_finger_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.05 -0.673001" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/ee/openarm_hand/visual/finger.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_left_right_finger_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.05 -0.673001" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/ee/openarm_hand/collision/finger.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0064528 -0.01702 0.0219685" />
|
||||
<mass value="0.03602545343277134" />
|
||||
<inertia ixx="2.3749999999999997e-06" ixy="1e-06" ixz="1e-06" iyy="2.3749999999999997e-06" iyz="1e-06" izz="7.5e-07" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_left_finger_joint1" type="prismatic">
|
||||
<parent link="openarm_left_hand" />
|
||||
<child link="openarm_left_right_finger" />
|
||||
<origin rpy="0 0 0" xyz="0 -0.006 0.015" />
|
||||
<axis xyz="0 -1 0" />
|
||||
<limit effort="333" lower="0.0" upper="0.044" velocity="10.0" />
|
||||
</joint>
|
||||
<joint name="openarm_left_finger_joint2" type="prismatic">
|
||||
<parent link="openarm_left_hand" />
|
||||
<child link="openarm_left_left_finger" />
|
||||
<origin rpy="0 0 0" xyz="0 0.006 0.015" />
|
||||
<axis xyz="0 1 0" />
|
||||
<limit effort="333" lower="0.0" upper="0.044" velocity="10.0" />
|
||||
<mimic joint="openarm_left_finger_joint1" />
|
||||
</joint>
|
||||
<link name="openarm_right_hand">
|
||||
<visual name="openarm_right_hand_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.6585" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/ee/openarm_hand/visual/hand.dae" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_right_hand_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.6585" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/ee/openarm_hand/collision/hand.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.002 0.03" />
|
||||
<mass value="0.35" />
|
||||
<inertia ixx="0.0002473" ixy="1e-06" ixz="1e-06" iyy="1.763e-05" iyz="1e-06" izz="0.0002521" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="right_openarm_hand_joint" type="fixed">
|
||||
<parent link="openarm_right_link7" />
|
||||
<child link="openarm_right_hand" />
|
||||
<origin rpy="0 0 0" xyz="0 -0.0 0.1001" />
|
||||
</joint>
|
||||
<link name="openarm_right_hand_tcp">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.001" />
|
||||
<inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000001" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_right_hand_tcp_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 -0.0 0.08" />
|
||||
<parent link="openarm_right_hand" />
|
||||
<child link="openarm_right_hand_tcp" />
|
||||
</joint>
|
||||
<link name="openarm_right_left_finger">
|
||||
<visual name="openarm_right_left_finger_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.05 -0.673001" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/ee/openarm_hand/visual/finger.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_right_left_finger_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.05 -0.673001" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/ee/openarm_hand/collision/finger.stl" scale="0.001 0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0064528 0.01702 0.0219685" />
|
||||
<mass value="0.03602545343277134" />
|
||||
<inertia ixx="2.3749999999999997e-06" ixy="1e-06" ixz="1e-06" iyy="2.3749999999999997e-06" iyz="1e-06" izz="7.5e-07" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="openarm_right_right_finger">
|
||||
<visual name="openarm_right_right_finger_visual">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.05 -0.673001" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/ee/openarm_hand/visual/finger.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="openarm_right_right_finger_collision">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.05 -0.673001" />
|
||||
<geometry>
|
||||
<mesh filename="./meshes/ee/openarm_hand/collision/finger.stl" scale="0.001 -0.001 0.001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0064528 -0.01702 0.0219685" />
|
||||
<mass value="0.03602545343277134" />
|
||||
<inertia ixx="2.3749999999999997e-06" ixy="1e-06" ixz="1e-06" iyy="2.3749999999999997e-06" iyz="1e-06" izz="7.5e-07" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="openarm_right_finger_joint1" type="prismatic">
|
||||
<parent link="openarm_right_hand" />
|
||||
<child link="openarm_right_right_finger" />
|
||||
<origin rpy="0 0 0" xyz="0 -0.006 0.015" />
|
||||
<axis xyz="0 -1 0" />
|
||||
<limit effort="333" lower="0.0" upper="0.044" velocity="10.0" />
|
||||
</joint>
|
||||
<joint name="openarm_right_finger_joint2" type="prismatic">
|
||||
<parent link="openarm_right_hand" />
|
||||
<child link="openarm_right_left_finger" />
|
||||
<origin rpy="0 0 0" xyz="0 0.006 0.015" />
|
||||
<axis xyz="0 1 0" />
|
||||
<limit effort="333" lower="0.0" upper="0.044" velocity="10.0" />
|
||||
<mimic joint="openarm_right_finger_joint1" />
|
||||
</joint>
|
||||
</robot>
|
||||
@@ -1,76 +0,0 @@
|
||||
import time
|
||||
import math
|
||||
import numpy as np
|
||||
from lerobot.robots.openarms.openarms_follower import OpenArmsFollower
|
||||
from lerobot.robots.openarms.config_openarms_follower import OpenArmsFollowerConfig
|
||||
|
||||
|
||||
def main():
|
||||
cfg = OpenArmsFollowerConfig(
|
||||
port_left="can0",
|
||||
port_right="can1",
|
||||
can_interface="socketcan",
|
||||
id="openarms_test",
|
||||
manual_control=True, # direct position control
|
||||
)
|
||||
|
||||
print('connecting...')
|
||||
rob = OpenArmsFollower(cfg)
|
||||
rob.connect(calibrate=True)
|
||||
|
||||
# disable left torque fully — keep it still
|
||||
rob.bus_left.disable_torque()
|
||||
|
||||
# desired angular sweep = 1/4 of current joint range
|
||||
sweep_deg = 20.0 # tweak if you want bigger movement
|
||||
|
||||
# frequency of movement
|
||||
hz = 100.0
|
||||
dt = 1.0 / hz
|
||||
move_time = 1.0 # seconds per joint
|
||||
|
||||
print('starting right–arm joint test…')
|
||||
print('support the arm and keep clear')
|
||||
|
||||
time.sleep(1.0)
|
||||
|
||||
# iterate motors except gripper
|
||||
for motor in rob.bus_right.motors:
|
||||
if motor == 'gripper':
|
||||
continue
|
||||
|
||||
print(f'testing {motor} on right arm...')
|
||||
start = time.time()
|
||||
|
||||
# read current position as center
|
||||
obs = rob.get_action()
|
||||
key = f'right_{motor}.pos'
|
||||
center = obs.get(key, 0.0)
|
||||
|
||||
t = 0.0
|
||||
while time.time() - start < move_time:
|
||||
offset = sweep_deg * math.sin(2 * math.pi * t)
|
||||
pos_cmd = center + offset
|
||||
|
||||
rob.bus_right._mit_control(
|
||||
motor=motor,
|
||||
kp=3.0, # some stiffness so it tracks well
|
||||
kd=0.2,
|
||||
position_degrees=pos_cmd,
|
||||
velocity_deg_per_sec=0.0,
|
||||
torque=0.0
|
||||
)
|
||||
|
||||
t += dt
|
||||
time.sleep(dt)
|
||||
|
||||
print(f'done {motor}')
|
||||
|
||||
print('\nall right–arm joints tested')
|
||||
print('disabling torque…')
|
||||
rob.bus_right.disable_torque()
|
||||
rob.disconnect()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,278 @@
|
||||
#!/usr/bin/env python
|
||||
"""
|
||||
Script to find episodes with highest MSE between observation.state and action pairs.
|
||||
|
||||
This script:
|
||||
1. Downloads a LeRobot dataset (if needed, skipping videos)
|
||||
2. Computes MSE between observation.state and action for each frame
|
||||
3. Aggregates MSE per episode
|
||||
4. Returns the top 1% episodes with highest total MSE
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
from tqdm import tqdm
|
||||
|
||||
from lerobot.datasets.lerobot_dataset import LeRobotDataset
|
||||
from lerobot.utils.constants import HF_LEROBOT_HOME
|
||||
|
||||
logging.basicConfig(level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s")
|
||||
|
||||
|
||||
def compute_episode_mse(
|
||||
dataset: LeRobotDataset,
|
||||
state_key: str = "observation.state",
|
||||
action_key: str = "action",
|
||||
) -> dict[int, float]:
|
||||
"""
|
||||
Compute total MSE between state and action for each episode.
|
||||
|
||||
Args:
|
||||
dataset: LeRobotDataset to analyze
|
||||
state_key: Key for the observation state in the dataset
|
||||
action_key: Key for the action in the dataset
|
||||
|
||||
Returns:
|
||||
Dictionary mapping episode_index to total MSE for that episode
|
||||
"""
|
||||
episode_mse = {}
|
||||
|
||||
# Get all unique episode indices
|
||||
hf_dataset = dataset.hf_dataset
|
||||
|
||||
# Group frames by episode for efficient processing
|
||||
logging.info("Computing MSE for each episode...")
|
||||
|
||||
# Process all frames and accumulate MSE per episode
|
||||
for idx in tqdm(range(len(hf_dataset)), desc="Processing frames"):
|
||||
item = hf_dataset[idx]
|
||||
|
||||
ep_idx = item["episode_index"]
|
||||
if isinstance(ep_idx, torch.Tensor):
|
||||
ep_idx = ep_idx.item()
|
||||
|
||||
state = item[state_key]
|
||||
action = item[action_key]
|
||||
|
||||
if isinstance(state, torch.Tensor):
|
||||
state = state.numpy()
|
||||
if isinstance(action, torch.Tensor):
|
||||
action = action.numpy()
|
||||
|
||||
# Compute MSE for this frame (sum of squared differences across all dimensions)
|
||||
mse = np.mean((state - action) ** 2)
|
||||
|
||||
if ep_idx not in episode_mse:
|
||||
episode_mse[ep_idx] = 0.0
|
||||
episode_mse[ep_idx] += mse
|
||||
|
||||
return episode_mse
|
||||
|
||||
|
||||
def get_top_mse_episodes(
|
||||
episode_mse: dict[int, float],
|
||||
top_percent: float = 1.0,
|
||||
) -> list[int]:
|
||||
"""
|
||||
Get the top X% of episodes with highest total MSE.
|
||||
|
||||
Args:
|
||||
episode_mse: Dictionary mapping episode_index to total MSE
|
||||
top_percent: Percentage of episodes to return (default: 1%)
|
||||
|
||||
Returns:
|
||||
List of episode indices sorted by MSE (highest first)
|
||||
"""
|
||||
# Sort episodes by MSE in descending order
|
||||
sorted_episodes = sorted(episode_mse.items(), key=lambda x: x[1], reverse=True)
|
||||
|
||||
# Calculate number of episodes to return
|
||||
num_episodes = len(sorted_episodes)
|
||||
num_top = max(1, int(np.ceil(num_episodes * top_percent / 100)))
|
||||
|
||||
# Extract top episode indices
|
||||
top_episodes = [ep_idx for ep_idx, _ in sorted_episodes[:num_top]]
|
||||
|
||||
return top_episodes
|
||||
|
||||
|
||||
def find_high_mse_episodes(
|
||||
repo_id: str,
|
||||
root: str | Path | None = None,
|
||||
state_key: str = "observation.state",
|
||||
action_key: str = "action",
|
||||
top_percent: float = 1.0,
|
||||
force_download: bool = False,
|
||||
) -> tuple[list[int], dict[int, float]]:
|
||||
"""
|
||||
Find episodes with highest MSE between observation.state and action.
|
||||
|
||||
Args:
|
||||
repo_id: HuggingFace dataset repository ID
|
||||
root: Local directory for dataset storage (default: ~/.cache/huggingface/lerobot)
|
||||
state_key: Key for the observation state in the dataset
|
||||
action_key: Key for the action in the dataset
|
||||
top_percent: Percentage of episodes to return (default: 1%)
|
||||
force_download: Force re-download of the dataset
|
||||
|
||||
Returns:
|
||||
Tuple of (list of top episode indices, dict of all episode MSEs)
|
||||
"""
|
||||
logging.info(f"Loading dataset: {repo_id}")
|
||||
|
||||
# Load the dataset (skip video download since we only need state/action data)
|
||||
dataset = LeRobotDataset(
|
||||
repo_id=repo_id,
|
||||
root=root,
|
||||
download_videos=False,
|
||||
force_cache_sync=force_download,
|
||||
)
|
||||
|
||||
# Verify the dataset has the required features
|
||||
if state_key not in dataset.features:
|
||||
raise ValueError(f"Dataset does not contain '{state_key}' feature. "
|
||||
f"Available features: {list(dataset.features.keys())}")
|
||||
if action_key not in dataset.features:
|
||||
raise ValueError(f"Dataset does not contain '{action_key}' feature. "
|
||||
f"Available features: {list(dataset.features.keys())}")
|
||||
|
||||
# Check that state and action have the same shape
|
||||
state_shape = tuple(dataset.features[state_key]["shape"])
|
||||
action_shape = tuple(dataset.features[action_key]["shape"])
|
||||
if state_shape != action_shape:
|
||||
raise ValueError(f"State shape {state_shape} does not match action shape {action_shape}")
|
||||
|
||||
logging.info(f"Dataset loaded successfully:")
|
||||
logging.info(f" - Total episodes: {dataset.meta.total_episodes}")
|
||||
logging.info(f" - Total frames: {dataset.meta.total_frames}")
|
||||
logging.info(f" - State shape: {state_shape}")
|
||||
logging.info(f" - Action shape: {action_shape}")
|
||||
logging.info(f" - Feature names: {dataset.features[state_key].get('names', 'N/A')}")
|
||||
|
||||
# Compute MSE for each episode
|
||||
episode_mse = compute_episode_mse(dataset, state_key, action_key)
|
||||
|
||||
# Get top episodes
|
||||
top_episodes = get_top_mse_episodes(episode_mse, top_percent)
|
||||
|
||||
return top_episodes, episode_mse
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Find episodes with highest MSE between observation.state and action"
|
||||
)
|
||||
parser.add_argument(
|
||||
"repo_id",
|
||||
type=str,
|
||||
help="HuggingFace dataset repository ID (e.g., 'lerobot/aloha_sim_insertion_human')",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--root",
|
||||
type=str,
|
||||
default=None,
|
||||
help="Local directory for dataset storage (default: ~/.cache/huggingface/lerobot)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--state-key",
|
||||
type=str,
|
||||
default="observation.state",
|
||||
help="Key for observation state feature (default: 'observation.state')",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--action-key",
|
||||
type=str,
|
||||
default="action",
|
||||
help="Key for action feature (default: 'action')",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--top-percent",
|
||||
type=float,
|
||||
default=1.0,
|
||||
help="Percentage of episodes to return (default: 1.0)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--force-download",
|
||||
action="store_true",
|
||||
help="Force re-download of the dataset",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--show-all-mse",
|
||||
action="store_true",
|
||||
help="Show MSE values for all episodes",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--output",
|
||||
type=str,
|
||||
default=None,
|
||||
help="Output file to save results (optional)",
|
||||
)
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
# Find high MSE episodes
|
||||
top_episodes, all_mse = find_high_mse_episodes(
|
||||
repo_id=args.repo_id,
|
||||
root=args.root,
|
||||
state_key=args.state_key,
|
||||
action_key=args.action_key,
|
||||
top_percent=args.top_percent,
|
||||
force_download=args.force_download,
|
||||
)
|
||||
|
||||
# Print results
|
||||
print("\n" + "=" * 60)
|
||||
print(f"TOP {args.top_percent}% EPISODES WITH HIGHEST MSE")
|
||||
print("=" * 60)
|
||||
|
||||
print(f"\nTotal episodes analyzed: {len(all_mse)}")
|
||||
print(f"Number of top episodes (top {args.top_percent}%): {len(top_episodes)}")
|
||||
|
||||
print(f"\nTop {len(top_episodes)} episode(s) with highest MSE:")
|
||||
print("-" * 40)
|
||||
for i, ep_idx in enumerate(top_episodes, 1):
|
||||
print(f" {i:3d}. Episode {ep_idx:5d} - Total MSE: {all_mse[ep_idx]:.6f}")
|
||||
|
||||
# Statistics
|
||||
all_mse_values = list(all_mse.values())
|
||||
print(f"\nMSE Statistics:")
|
||||
print(f" - Mean MSE: {np.mean(all_mse_values):.6f}")
|
||||
print(f" - Std MSE: {np.std(all_mse_values):.6f}")
|
||||
print(f" - Min MSE: {np.min(all_mse_values):.6f}")
|
||||
print(f" - Max MSE: {np.max(all_mse_values):.6f}")
|
||||
print(f" - Median MSE: {np.median(all_mse_values):.6f}")
|
||||
|
||||
if args.show_all_mse:
|
||||
print(f"\nAll episodes sorted by MSE (descending):")
|
||||
print("-" * 40)
|
||||
sorted_episodes = sorted(all_mse.items(), key=lambda x: x[1], reverse=True)
|
||||
for ep_idx, mse in sorted_episodes:
|
||||
print(f" Episode {ep_idx:5d} - Total MSE: {mse:.6f}")
|
||||
|
||||
# Save results if output file specified
|
||||
if args.output:
|
||||
output_path = Path(args.output)
|
||||
with open(output_path, "w") as f:
|
||||
f.write(f"# High MSE Episodes Analysis\n")
|
||||
f.write(f"# Dataset: {args.repo_id}\n")
|
||||
f.write(f"# State key: {args.state_key}\n")
|
||||
f.write(f"# Action key: {args.action_key}\n")
|
||||
f.write(f"# Top percent: {args.top_percent}%\n\n")
|
||||
|
||||
f.write(f"Top {args.top_percent}% episodes:\n")
|
||||
for ep_idx in top_episodes:
|
||||
f.write(f"{ep_idx},{all_mse[ep_idx]:.6f}\n")
|
||||
|
||||
logging.info(f"Results saved to: {output_path}")
|
||||
|
||||
# Return the list for programmatic use
|
||||
return top_episodes
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -65,8 +65,8 @@ class OpenArmsLeaderConfig(TeleoperatorConfig):
|
||||
|
||||
# MIT control parameters (used when manual_control=False for torque control)
|
||||
# List of 8 values: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7, gripper]
|
||||
position_kp: list[float] = field(default_factory=lambda: [100.0, 100.0, 100.0, 48.0, 24.0, 31.0, 25.0, 16.0])
|
||||
position_kd: list[float] = field(default_factory=lambda: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
|
||||
position_kp: list[float] = field(default_factory=lambda: [240.0, 240.0, 240.0, 240.0, 24.0, 31.0, 25.0, 16.0])
|
||||
position_kd: list[float] = field(default_factory=lambda: [3.0, 3.0, 3.0, 3.0, 0.2, 0.2, 0.2, 0.2])
|
||||
|
||||
# Damping gains for stability when applying torque compensation (gravity/friction)
|
||||
# Used when kp=0 and only torque is applied
|
||||
|
||||
Reference in New Issue
Block a user