mirror of
https://github.com/huggingface/lerobot.git
synced 2026-06-01 03:11:29 +00:00
141 lines
3.8 KiB
Python
141 lines
3.8 KiB
Python
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()
|