[software] 添加16DOF早期训练仿真与Sim2Real闭环
This commit is contained in:
@@ -0,0 +1,9 @@
|
||||
# IK 真机控制探索
|
||||
|
||||
该目录保存强化学习部署前的逆运动学真机控制代码。
|
||||
|
||||
- `sim2real_control_api.py`：真机控制接口
|
||||
- `trajectory_interpolator.py`：关节/姿态轨迹插值
|
||||
- `sim_to_real_deploy_beifen.py`：早期部署脚本备份
|
||||
|
||||
文件名中的 `beifen` 来自原始资料。为保持早期版本可追溯性，本次归档不修改源码和文件名。
|
||||
@@ -0,0 +1,274 @@
|
||||
"""Sim-to-real control/IK API extracted from mature mujoco_sim controller.
|
||||
|
||||
This module provides a deployment-friendly wrapper around:
|
||||
- wheel mode posture control
|
||||
- trot swing-leg IK + wheel assist
|
||||
- differential wheel speed mapping
|
||||
|
||||
No MuJoCo runtime is required for using the API itself.
|
||||
For trot IK, Pinocchio model is used via Dynamics.
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, Optional, Tuple
|
||||
import numpy as np
|
||||
|
||||
from config import (
|
||||
LEG_NAMES,
|
||||
LEG_JOINTS,
|
||||
WHEEL_JOINT,
|
||||
DEFAULT_JOINT_ANGLES,
|
||||
WHEEL_RADIUS,
|
||||
WHEEL_TRACK,
|
||||
WHEEL_VEL_MAX,
|
||||
KP_ROLL,
|
||||
KP_PITCH,
|
||||
GAIT_FREQ,
|
||||
GAIT_DUTY,
|
||||
SWING_HEIGHT,
|
||||
PHASE_OFFSETS,
|
||||
)
|
||||
from dynamics import Dynamics
|
||||
|
||||
|
||||
@dataclass
|
||||
class DeployState:
|
||||
"""Minimal state for deployment control."""
|
||||
|
||||
rpy: np.ndarray # (3,) roll, pitch, yaw
|
||||
|
||||
|
||||
class Sim2RealControlAPI:
|
||||
"""Deployment-friendly control and IK API.
|
||||
|
||||
Supported modes:
|
||||
- wheel: wheel differential drive + leg posture hold
|
||||
- trot: swing foot IK + stance posture + wheel assist
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
self.mode = "wheel"
|
||||
self.prone = False
|
||||
|
||||
self.vel_x = 0.0
|
||||
self.vel_y = 0.0
|
||||
self.yaw_rate = 0.0
|
||||
self.height = 0.33
|
||||
|
||||
self._gait_phase = 0.0
|
||||
self._smooth_vx = 0.0
|
||||
self._smooth_vy = 0.0
|
||||
self._smooth_yaw = 0.0
|
||||
|
||||
self._default_q = np.array([
|
||||
DEFAULT_JOINT_ANGLES["hip_abduction"],
|
||||
DEFAULT_JOINT_ANGLES["hip_pitch"],
|
||||
DEFAULT_JOINT_ANGLES["knee"],
|
||||
])
|
||||
|
||||
self.dynamics = Dynamics()
|
||||
self._swing_start_foot = {leg: np.zeros(3) for leg in LEG_NAMES}
|
||||
self._last_contact = {leg: True for leg in LEG_NAMES}
|
||||
|
||||
def set_mode(self, mode: str):
|
||||
if mode not in ("wheel", "trot"):
|
||||
raise ValueError("mode must be one of: wheel, trot")
|
||||
self.mode = mode
|
||||
|
||||
def set_command(self, vel_x: float, vel_y: float, yaw_rate: float, height: Optional[float] = None):
|
||||
self.vel_x = float(vel_x)
|
||||
self.vel_y = float(vel_y)
|
||||
self.yaw_rate = float(yaw_rate)
|
||||
if height is not None:
|
||||
self.height = float(height)
|
||||
|
||||
def compute(
|
||||
self,
|
||||
state: DeployState,
|
||||
dt: float,
|
||||
q_pin: Optional[np.ndarray] = None,
|
||||
dq_pin: Optional[np.ndarray] = None,
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""Compute leg and wheel commands.
|
||||
|
||||
Returns:
|
||||
leg_targets: (12,) [fl(3), fr(3), rl(3), rr(3)]
|
||||
wheel_targets: (4,) [fl, fr, rl, rr] in rad/s
|
||||
|
||||
Notes:
|
||||
- wheel mode does not require q_pin/dq_pin
|
||||
- trot mode requires q_pin/dq_pin for IK/FK through Pinocchio
|
||||
"""
|
||||
alpha = min(float(dt) * 3.0, 1.0)
|
||||
self._smooth_vx += alpha * (self.vel_x - self._smooth_vx)
|
||||
self._smooth_vy += alpha * (self.vel_y - self._smooth_vy)
|
||||
self._smooth_yaw += alpha * (self.yaw_rate - self._smooth_yaw)
|
||||
|
||||
if self.prone:
|
||||
return self._prone_mode()
|
||||
|
||||
if self.mode == "wheel":
|
||||
return self._wheel_mode(state)
|
||||
|
||||
if q_pin is None or dq_pin is None:
|
||||
raise ValueError("trot mode requires q_pin and dq_pin")
|
||||
return self._trot_mode(state, float(dt), q_pin, dq_pin)
|
||||
|
||||
def to_joint_dict(self, leg_targets: np.ndarray, wheel_targets: np.ndarray) -> Dict[str, float]:
|
||||
"""Convert array commands to named joint-command dictionary."""
|
||||
out: Dict[str, float] = {}
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
out[f"{leg}_{LEG_JOINTS[0]}"] = float(leg_targets[i * 3 + 0])
|
||||
out[f"{leg}_{LEG_JOINTS[1]}"] = float(leg_targets[i * 3 + 1])
|
||||
out[f"{leg}_{LEG_JOINTS[2]}"] = float(leg_targets[i * 3 + 2])
|
||||
out[f"{leg}_{WHEEL_JOINT}"] = float(wheel_targets[i])
|
||||
return out
|
||||
|
||||
def _prone_mode(self):
|
||||
leg_targets = np.zeros(12)
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
side = 1.0 if leg[1] == "l" else -1.0
|
||||
leg_targets[i * 3 + 0] = side * 0.3
|
||||
leg_targets[i * 3 + 1] = 1.5
|
||||
leg_targets[i * 3 + 2] = -2.65
|
||||
return leg_targets, np.zeros(4)
|
||||
|
||||
def _wheel_mode(self, state: DeployState):
|
||||
wheel_targets = self._differential_drive(self._smooth_vx, self._smooth_yaw)
|
||||
leg_targets = self._posture_control(state)
|
||||
return leg_targets, wheel_targets
|
||||
|
||||
def _posture_control(self, state: DeployState) -> np.ndarray:
|
||||
leg_targets = np.zeros(12)
|
||||
|
||||
_H = [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]
|
||||
_HIP = [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]
|
||||
_KNEE = [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]
|
||||
|
||||
h_clamp = np.clip(self.height, _H[0], _H[-1])
|
||||
q_hip_base = float(np.interp(h_clamp, _H, _HIP))
|
||||
q_knee_base = float(np.interp(h_clamp, _H, _KNEE))
|
||||
|
||||
roll_corr = -KP_ROLL * float(state.rpy[0])
|
||||
pitch_corr = -KP_PITCH * float(state.rpy[1])
|
||||
lateral_lean = 0.3 * self.vel_y
|
||||
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
side = 1.0 if leg[1] == "l" else -1.0
|
||||
leg_targets[i * 3 + 0] = np.clip(side * roll_corr + lateral_lean, -0.5, 0.5)
|
||||
leg_targets[i * 3 + 1] = np.clip(q_hip_base + pitch_corr, -1.0, 2.5)
|
||||
leg_targets[i * 3 + 2] = np.clip(q_knee_base, -2.6, -0.3)
|
||||
|
||||
return leg_targets
|
||||
|
||||
def _trot_mode(self, state: DeployState, dt: float, q_pin: np.ndarray, dq_pin: np.ndarray):
|
||||
self._gait_phase = (self._gait_phase + dt * GAIT_FREQ) % 1.0
|
||||
|
||||
contacts: Dict[str, bool] = {}
|
||||
for leg in LEG_NAMES:
|
||||
phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
|
||||
contacts[leg] = bool(phase < GAIT_DUTY)
|
||||
|
||||
self.dynamics.update(q_pin, dq_pin)
|
||||
|
||||
leg_targets = np.zeros(12)
|
||||
wheel_targets = np.zeros(4)
|
||||
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
if contacts[leg]:
|
||||
leg_targets[i * 3:(i + 1) * 3] = self._stance_leg_target(state, leg)
|
||||
self._swing_start_foot[leg] = self.dynamics.get_foot_pos(leg)
|
||||
self._last_contact[leg] = True
|
||||
wheel_targets[i] = self._differential_drive_single(self._smooth_vx, self._smooth_yaw, leg)
|
||||
else:
|
||||
swing_phase = self._get_swing_phase(leg)
|
||||
target_foot = self._compute_swing_target(leg, state, swing_phase)
|
||||
q_ik = self.dynamics.inverse_kinematics(leg, target_foot, q_pin)
|
||||
leg_targets[i * 3:(i + 1) * 3] = q_ik
|
||||
self._last_contact[leg] = False
|
||||
wheel_targets[i] = 0.0
|
||||
|
||||
return leg_targets, wheel_targets
|
||||
|
||||
def _stance_leg_target(self, state: DeployState, leg: str) -> np.ndarray:
|
||||
_H = [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]
|
||||
_HIP = [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]
|
||||
_KNEE = [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]
|
||||
|
||||
h_clamp = np.clip(self.height, _H[0], _H[-1])
|
||||
q_hip = float(np.interp(h_clamp, _H, _HIP))
|
||||
q_knee = float(np.interp(h_clamp, _H, _KNEE))
|
||||
|
||||
roll_corr = -KP_ROLL * float(state.rpy[0])
|
||||
pitch_corr = -KP_PITCH * float(state.rpy[1])
|
||||
side = 1.0 if leg[1] == "l" else -1.0
|
||||
lateral_lean = 0.3 * self.vel_y
|
||||
|
||||
return np.array([
|
||||
np.clip(side * roll_corr + lateral_lean, -0.5, 0.5),
|
||||
np.clip(q_hip + pitch_corr, -1.0, 2.5),
|
||||
np.clip(q_knee, -2.6, -0.3),
|
||||
])
|
||||
|
||||
def _differential_drive(self, vel_x: float, yaw_rate: float) -> np.ndarray:
|
||||
vel_left = (vel_x - 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
|
||||
vel_right = (vel_x + 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
|
||||
targets = np.zeros(4)
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
targets[i] = vel_left if leg[1] == "l" else vel_right
|
||||
return np.clip(targets, -WHEEL_VEL_MAX, WHEEL_VEL_MAX)
|
||||
|
||||
def _differential_drive_single(self, vel_x: float, yaw_rate: float, leg: str) -> float:
|
||||
if leg[1] == "l":
|
||||
v = (vel_x - 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
|
||||
else:
|
||||
v = (vel_x + 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
|
||||
return float(np.clip(v, -WHEEL_VEL_MAX, WHEEL_VEL_MAX))
|
||||
|
||||
def _get_swing_phase(self, leg: str) -> float:
|
||||
phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
|
||||
if phase < GAIT_DUTY:
|
||||
return 0.0
|
||||
return (phase - GAIT_DUTY) / (1.0 - GAIT_DUTY)
|
||||
|
||||
def _compute_swing_target(self, leg: str, state: DeployState, swing_phase: float) -> np.ndarray:
|
||||
p_start = self._swing_start_foot[leg]
|
||||
p_end = self._compute_touchdown(leg, state)
|
||||
|
||||
s = swing_phase
|
||||
s_mj = 10 * s**3 - 15 * s**4 + 6 * s**5
|
||||
|
||||
pos = p_start + (p_end - p_start) * s_mj
|
||||
|
||||
z_lift = 64.0 * s**3 * (1.0 - s)**3
|
||||
pos[2] = p_start[2] + SWING_HEIGHT * z_lift
|
||||
|
||||
return pos
|
||||
|
||||
def _compute_touchdown(self, leg: str, state: DeployState) -> np.ndarray:
|
||||
td = self._swing_start_foot[leg].copy()
|
||||
|
||||
t_stance = (1.0 / GAIT_FREQ) * GAIT_DUTY
|
||||
yaw = float(state.rpy[2])
|
||||
c, s = np.cos(yaw), np.sin(yaw)
|
||||
R_z = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
|
||||
cmd_vel_world = R_z @ np.array([self._smooth_vx, self._smooth_vy, 0.0])
|
||||
|
||||
td[0] += cmd_vel_world[0] * t_stance * 0.5
|
||||
td[1] += cmd_vel_world[1] * t_stance * 0.5
|
||||
td[2] = WHEEL_RADIUS
|
||||
return td
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
api = Sim2RealControlAPI()
|
||||
api.set_mode("wheel")
|
||||
api.set_command(vel_x=0.3, vel_y=0.0, yaw_rate=0.0, height=0.33)
|
||||
|
||||
state = DeployState(rpy=np.array([0.0, 0.0, 0.0]))
|
||||
leg, wheel = api.compute(state=state, dt=0.004)
|
||||
cmd = api.to_joint_dict(leg, wheel)
|
||||
|
||||
print("Example wheel-mode command:")
|
||||
for k, v in sorted(cmd.items()):
|
||||
print(f"{k}: {v:.6f}")
|
||||
@@ -0,0 +1,234 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse, sys, threading, time
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
|
||||
sys.path.append('/home/rc2/work/rcwork/control')
|
||||
from drivers.motor_driver import RobStrideDriver
|
||||
|
||||
ROOT = Path('/home/rc2/work/rcwork/wheelleg_deploy_swj/wheelleg_deploy/wheelleg_mjlab/beifen')
|
||||
sys.path += [str(ROOT), str(ROOT / 'mujoco_sim')]
|
||||
from sim2real_control_api import DeployState, Sim2RealControlAPI # type: ignore
|
||||
|
||||
sys.path.append('/home/rc2/work/rcwork')
|
||||
from trajectory_interpolator import TrajectoryInterpolator
|
||||
|
||||
LEGS = ('fl','fr','rl','rr')
|
||||
LJ = ('hip_abduction_joint','hip_pitch_joint','knee_joint')
|
||||
WJ = 'wheel_joint'
|
||||
JNS = [f'{l}_{j}' for l in LEGS for j in (*LJ, WJ)]
|
||||
|
||||
@dataclass
|
||||
class Cfg:
|
||||
mid:int; model:str; sign:float; off:float; bus:str
|
||||
|
||||
class Deploy:
|
||||
def __init__(self, can1, can2, hz=100.0, use_interpolation=True, interp_method='quintic', interp_time=1.5):
|
||||
self.d1, self.d2 = RobStrideDriver(can1, False), RobStrideDriver(can2, False)
|
||||
self.api = Sim2RealControlAPI(); self.dt = 1.0/hz; self.lk = threading.Lock()
|
||||
self.run = False; self.enabled = False; self.estop = True
|
||||
self.mode='stand'; self.vx=0.0; self.vy=0.0; self.yaw=0.0; self.h=0.33; self.roll=0.0; self.pitch=0.0
|
||||
self.prone = False
|
||||
self.kp_leg, self.kd_leg, self.kd_wheel = 80.0, 2.5, 2.0
|
||||
self.cfg = self._cfg(); self.q=np.zeros(23); self.dq=np.zeros(22)
|
||||
|
||||
self.use_interpolation = use_interpolation
|
||||
if self.use_interpolation:
|
||||
self.interpolator = TrajectoryInterpolator(method=interp_method, transition_time=interp_time)
|
||||
print(f'[Interpolation] Enabled: method={interp_method}, transition_time={interp_time}s')
|
||||
else:
|
||||
self.interpolator = None
|
||||
print('[Interpolation] Disabled')
|
||||
|
||||
def _cfg(self):
|
||||
sign={'fl_hip_abduction_joint':-1,'fl_hip_pitch_joint':-1,'fl_knee_joint':-1,'fl_wheel_joint':-1,'fr_hip_abduction_joint':-1,'fr_hip_pitch_joint':1,'fr_knee_joint':1,'fr_wheel_joint':1,'rl_hip_abduction_joint':1,'rl_hip_pitch_joint':-1,'rl_knee_joint':-1,'rl_wheel_joint':-1,'rr_hip_abduction_joint':1,'rr_hip_pitch_joint':1,'rr_knee_joint':1,'rr_wheel_joint':1}
|
||||
off={'fl_hip_abduction_joint':0.003,'fl_hip_pitch_joint':0.030,'fl_knee_joint':0.028,'fl_wheel_joint':0.0,'fr_hip_abduction_joint':0.004,'fr_hip_pitch_joint':0.038,'fr_knee_joint':0.011,'fr_wheel_joint':0.0,'rl_hip_abduction_joint':0.019,'rl_hip_pitch_joint':-0.034,'rl_knee_joint':0.025,'rl_wheel_joint':0.0,'rr_hip_abduction_joint':-0.001,'rr_hip_pitch_joint':0.039,'rr_knee_joint':0.018,'rr_wheel_joint':0.0}
|
||||
ids={'fl_hip_abduction_joint':1,'fl_hip_pitch_joint':2,'fl_knee_joint':3,'fl_wheel_joint':4,'fr_hip_abduction_joint':5,'fr_hip_pitch_joint':6,'fr_knee_joint':7,'fr_wheel_joint':8,'rl_hip_abduction_joint':1,'rl_hip_pitch_joint':2,'rl_knee_joint':3,'rl_wheel_joint':4,'rr_hip_abduction_joint':5,'rr_hip_pitch_joint':6,'rr_knee_joint':7,'rr_wheel_joint':8}
|
||||
bus={k:('can1' if k.startswith('f') else 'can2') for k in JNS}
|
||||
return {jn:Cfg(ids[jn],'rs-06',float(sign[jn]),float(off[jn]),bus[jn]) for jn in JNS}
|
||||
|
||||
def _drv(self, jn): return self.d1 if self.cfg[jn].bus=='can1' else self.d2
|
||||
|
||||
def connect(self):
|
||||
self.d1.connect(); self.d2.connect()
|
||||
for jn,c in self.cfg.items(): self._drv(jn).add_motor(jn,c.mid,c.model)
|
||||
|
||||
def enable_all(self):
|
||||
for jn in JNS: self._drv(jn).enable(jn)
|
||||
with self.lk: self.enabled=True; self.estop=False; self.vx=self.vy=self.yaw=0.0
|
||||
|
||||
def disable_all(self):
|
||||
for jn in JNS: self._drv(jn).disable(jn)
|
||||
with self.lk: self.enabled=False
|
||||
|
||||
def clear(self):
|
||||
for jn in JNS: self._drv(jn).clear_warnings(jn)
|
||||
|
||||
def set_estop(self,on):
|
||||
with self.lk:
|
||||
self.estop=on
|
||||
if on: self.vx=self.vy=self.yaw=0.0
|
||||
if on: self.disable_all()
|
||||
|
||||
def set_prone(self,on):
|
||||
with self.lk: self.prone=on; self.api.prone=on
|
||||
|
||||
def _update_pin(self,leg,wheel):
|
||||
q=np.zeros(23); dq=np.zeros(22); q[2]=self.h; q[6]=1.0
|
||||
for i,_ in enumerate(LEGS):
|
||||
b=7+i*4; q[b:b+3]=leg[i*3:i*3+3]; dq[6+i*4+3]=wheel[i]
|
||||
self.q,self.dq=q,dq
|
||||
|
||||
def step(self):
|
||||
with self.lk:
|
||||
if self.estop or (not self.enabled): return
|
||||
m=self.mode; vx=float(np.clip(self.vx,-0.8,0.8)); vy=float(np.clip(self.vy,-0.5,0.5)); yaw=float(np.clip(self.yaw,-3,3)); h=float(np.clip(self.h,0.157,0.448)); r=float(np.clip(self.roll,-0.4,0.4)); p=float(np.clip(self.pitch,-0.4,0.4)); prone=self.prone
|
||||
cm='trot' if m=='trot' else 'wheel'
|
||||
if m=='stand': vx=vy=yaw=0.0
|
||||
self.api.prone=prone; self.api.set_mode(cm); self.api.set_command(vx,vy,yaw,height=h)
|
||||
st=DeployState(rpy=np.array([r,p,0.0]))
|
||||
leg,wheel = self.api.compute(st,self.dt,self.q,self.dq) if cm=='trot' else self.api.compute(st,self.dt)
|
||||
self._update_pin(leg,wheel); cmd=self.api.to_joint_dict(leg,wheel)
|
||||
|
||||
if self.use_interpolation and self.interpolator is not None:
|
||||
leg_cmd = {jn: cmd[jn] for jn in JNS if not jn.endswith(WJ)}
|
||||
self.interpolator.set_target(leg_cmd)
|
||||
smooth_cmd = self.interpolator.update(self.dt)
|
||||
for jn in leg_cmd:
|
||||
cmd[jn] = smooth_cmd[jn]
|
||||
|
||||
for jn in JNS:
|
||||
d=self._drv(jn); c=self.cfg[jn]
|
||||
if jn.endswith(WJ): d.control_mit(jn,0.0,c.sign*float(cmd.get(jn,0.0)),0.0,self.kd_wheel,0.0)
|
||||
else: d.control_mit(jn,c.sign*float(cmd[jn])+c.off,0.0,self.kp_leg,self.kd_leg,0.0)
|
||||
|
||||
def loop(self):
|
||||
while self.run:
|
||||
t=time.time()
|
||||
try: self.step()
|
||||
except Exception as e: print('[control]',e)
|
||||
time.sleep(max(0.0,self.dt-(time.time()-t)))
|
||||
|
||||
def start(self): self.run=True; threading.Thread(target=self.loop,daemon=True).start()
|
||||
def stop(self):
|
||||
self.run=False; time.sleep(0.05)
|
||||
try: self.disable_all()
|
||||
finally: self.d1.disconnect(); self.d2.disconnect()
|
||||
|
||||
def status(self):
|
||||
with self.lk: return f'mode={self.mode} en={self.enabled} estop={self.estop} prone={self.prone} vx={self.vx:.2f} vy={self.vy:.2f} yaw={self.yaw:.2f} h={self.h:.3f}'
|
||||
|
||||
class CLI:
|
||||
def __init__(self,d): self.d=d
|
||||
def run(self):
|
||||
print('enable disable clear estop_on estop_off prone_on prone_off status')
|
||||
print('mode stand|wheel|trot, vx vy yaw h roll pitch, stop, quit')
|
||||
print('interp_on interp_off interp_time <sec>, interp_method linear|cubic|quintic|cosine')
|
||||
while True:
|
||||
try: s=input('cmd> ').strip().lower()
|
||||
except (EOFError,KeyboardInterrupt): s='quit'
|
||||
if s in ('quit','exit'): break
|
||||
if s=='enable': self.d.enable_all(); continue
|
||||
if s=='disable': self.d.disable_all(); continue
|
||||
if s=='clear': self.d.clear(); continue
|
||||
if s=='estop_on': self.d.set_estop(True); continue
|
||||
if s=='estop_off': self.d.set_estop(False); continue
|
||||
if s=='prone_on': self.d.set_prone(True); continue
|
||||
if s=='prone_off': self.d.set_prone(False); continue
|
||||
if s=='status': print(self.d.status()); continue
|
||||
if s=='stop':
|
||||
with self.d.lk: self.d.vx=self.d.vy=self.d.yaw=0.0
|
||||
continue
|
||||
if s=='interp_on':
|
||||
with self.d.lk: self.d.use_interpolation=True
|
||||
print('Interpolation enabled'); continue
|
||||
if s=='interp_off':
|
||||
with self.d.lk: self.d.use_interpolation=False
|
||||
print('Interpolation disabled'); continue
|
||||
if s.startswith('interp_time '):
|
||||
try:
|
||||
t=float(s.split()[1])
|
||||
if self.d.interpolator: self.d.interpolator.set_transition_time(t)
|
||||
print(f'Interpolation time set to {t}s')
|
||||
except Exception as e: print(f'Error: {e}')
|
||||
continue
|
||||
if s.startswith('interp_method '):
|
||||
try:
|
||||
method=s.split()[1]
|
||||
if self.d.interpolator: self.d.interpolator.set_method(method)
|
||||
print(f'Interpolation method set to {method}')
|
||||
except Exception as e: print(f'Error: {e}')
|
||||
continue
|
||||
if s.startswith('mode '):
|
||||
m=s.split()[1]
|
||||
if m in ('stand','wheel','trot'):
|
||||
with self.d.lk: self.d.mode=m
|
||||
else: print('bad mode')
|
||||
continue
|
||||
try:
|
||||
k,v=s.split()[0],float(s.split()[1])
|
||||
with self.d.lk:
|
||||
if k=='vx': self.d.vx=v
|
||||
elif k=='vy': self.d.vy=v
|
||||
elif k=='yaw': self.d.yaw=v
|
||||
elif k=='h': self.d.h=v
|
||||
elif k=='roll': self.d.roll=v
|
||||
elif k=='pitch': self.d.pitch=v
|
||||
else: print('unknown')
|
||||
except Exception: print('unknown/bad')
|
||||
|
||||
class GUI:
|
||||
def __init__(self,d):
|
||||
import tkinter as tk
|
||||
from tkinter import ttk
|
||||
self.d=d; self.root=tk.Tk(); self.root.title('WheelLeg Deploy')
|
||||
f=ttk.Frame(self.root,padding=8); f.grid(row=0,column=0,sticky='nsew')
|
||||
self.state=tk.StringVar(value='E-STOP ON'); ttk.Label(f,textvariable=self.state).grid(row=0,column=0,columnspan=4,sticky='w')
|
||||
ttk.Button(f,text='Enable',command=self.en).grid(row=1,column=0)
|
||||
ttk.Button(f,text='Disable',command=self.dis).grid(row=1,column=1)
|
||||
ttk.Button(f,text='E-STOP ON',command=lambda:self.es(True)).grid(row=1,column=2)
|
||||
ttk.Button(f,text='E-STOP OFF',command=lambda:self.es(False)).grid(row=1,column=3)
|
||||
ttk.Button(f,text='Prone ON',command=lambda:self.pr(True)).grid(row=2,column=2)
|
||||
ttk.Button(f,text='Prone OFF',command=lambda:self.pr(False)).grid(row=2,column=3)
|
||||
self.mode=tk.StringVar(value='stand'); self.vx=tk.DoubleVar(value=0.0); self.vy=tk.DoubleVar(value=0.0); self.yaw=tk.DoubleVar(value=0.0); self.h=tk.DoubleVar(value=0.33)
|
||||
self.roll=tk.DoubleVar(value=0.0); self.pitch=tk.DoubleVar(value=0.0)
|
||||
cb=ttk.Combobox(f,textvariable=self.mode,values=['stand','wheel','trot'],state='readonly'); cb.grid(row=3,column=0,columnspan=2,sticky='ew'); cb.bind('<<ComboboxSelected>>',lambda _:self.sync())
|
||||
ttk.Button(f,text='Stop',command=self.stp).grid(row=3,column=3)
|
||||
self.sl(f,4,'vx',self.vx,-0.8,0.8); self.sl(f,5,'vy',self.vy,-0.5,0.5); self.sl(f,6,'yaw',self.yaw,-3,3); self.sl(f,7,'height',self.h,0.157,0.448); self.sl(f,8,'roll',self.roll,-0.4,0.4); self.sl(f,9,'pitch',self.pitch,-0.4,0.4)
|
||||
self.info=tk.StringVar(value=''); ttk.Label(f,textvariable=self.info).grid(row=10,column=0,columnspan=4,sticky='w'); self.tick()
|
||||
def sl(self,f,r,n,v,lo,hi):
|
||||
from tkinter import ttk
|
||||
ttk.Label(f,text=n).grid(row=r,column=0,sticky='w'); ttk.Scale(f,from_=lo,to=hi,variable=v,command=lambda _:self.sync()).grid(row=r,column=1,columnspan=3,sticky='ew')
|
||||
def sync(self):
|
||||
with self.d.lk:
|
||||
self.d.mode=self.mode.get(); self.d.vx=float(self.vx.get()); self.d.vy=float(self.vy.get()); self.d.yaw=float(self.yaw.get()); self.d.h=float(self.h.get()); self.d.roll=float(self.roll.get()); self.d.pitch=float(self.pitch.get())
|
||||
def en(self): self.d.enable_all(); self.state.set('Enabled')
|
||||
def dis(self): self.d.disable_all(); self.state.set('Disabled')
|
||||
def es(self,on): self.d.set_estop(on); self.state.set('E-STOP ON' if on else 'E-STOP OFF')
|
||||
def pr(self,on): self.d.set_prone(on)
|
||||
def stp(self):
|
||||
with self.d.lk: self.d.vx=self.d.vy=self.d.yaw=0.0
|
||||
self.vx.set(0.0); self.vy.set(0.0); self.yaw.set(0.0)
|
||||
def tick(self): self.info.set(self.d.status()); self.root.after(150,self.tick)
|
||||
def run(self): self.root.mainloop()
|
||||
|
||||
def main():
|
||||
ap=argparse.ArgumentParser()
|
||||
ap.add_argument('--port-can1',default='/dev/can1')
|
||||
ap.add_argument('--port-can2',default='/dev/can2')
|
||||
ap.add_argument('--hz',type=float,default=100.0)
|
||||
ap.add_argument('--no-gui',action='store_true')
|
||||
ap.add_argument('--no-interp',action='store_true',help='Disable trajectory interpolation')
|
||||
ap.add_argument('--interp-method',default='quintic',choices=['linear','cubic','quintic','cosine'],help='Interpolation method')
|
||||
ap.add_argument('--interp-time',type=float,default=0.3,help='Interpolation transition time (seconds)')
|
||||
a=ap.parse_args()
|
||||
d=Deploy(a.port_can1,a.port_can2,a.hz,use_interpolation=not a.no_interp,interp_method=a.interp_method,interp_time=a.interp_time); d.connect(); d.start()
|
||||
try:
|
||||
cli=CLI(d); t=threading.Thread(target=cli.run,daemon=True); t.start()
|
||||
if a.no_gui:
|
||||
while t.is_alive(): time.sleep(0.2)
|
||||
else: GUI(d).run()
|
||||
finally: d.stop()
|
||||
|
||||
if __name__=='__main__': main()
|
||||
@@ -0,0 +1,237 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
轨迹插值模块 - 用于平滑关节角度过渡，避免突变和冲击
|
||||
|
||||
支持多种插值方法：
|
||||
- linear: 线性插值
|
||||
- cubic: 三次多项式（速度连续）
|
||||
- quintic: 五次多项式（速度和加速度连续，最平滑）
|
||||
- cosine: 余弦 S 曲线
|
||||
|
||||
使用示例：
|
||||
interp = TrajectoryInterpolator(method='quintic', transition_time=0.5)
|
||||
|
||||
# 设置新目标
|
||||
interp.set_target({'joint1': 1.5, 'joint2': 0.8})
|
||||
|
||||
# 每个控制周期调用
|
||||
smooth_q = interp.update(dt=0.01, current_q={'joint1': 0.5, 'joint2': 0.3})
|
||||
"""
|
||||
|
||||
import time
|
||||
from typing import Dict, Optional
|
||||
import numpy as np
|
||||
|
||||
|
||||
class TrajectoryInterpolator:
|
||||
def __init__(self, method: str = 'quintic', transition_time: float = 0.5):
|
||||
"""
|
||||
初始化轨迹插值器
|
||||
|
||||
Args:
|
||||
method: 插值方法 ('linear', 'cubic', 'quintic', 'cosine')
|
||||
transition_time: 过渡时间（秒）
|
||||
"""
|
||||
self.method = method
|
||||
self.transition_time = transition_time
|
||||
|
||||
self.q_start: Dict[str, float] = {}
|
||||
self.q_target: Dict[str, float] = {}
|
||||
self.q_current: Dict[str, float] = {}
|
||||
|
||||
self.transition_start_time: Optional[float] = None
|
||||
self.is_transitioning = False
|
||||
|
||||
self._interpolation_funcs = {
|
||||
'linear': self._linear,
|
||||
'cubic': self._cubic,
|
||||
'quintic': self._quintic,
|
||||
'cosine': self._cosine,
|
||||
}
|
||||
|
||||
if method not in self._interpolation_funcs:
|
||||
raise ValueError(f"Unknown interpolation method: {method}. "
|
||||
f"Available: {list(self._interpolation_funcs.keys())}")
|
||||
|
||||
def set_target(self, q_target: Dict[str, float], force_restart: bool = False):
|
||||
"""
|
||||
设置新的目标角度，开始新的过渡
|
||||
|
||||
Args:
|
||||
q_target: 目标关节角度字典 {joint_name: angle}
|
||||
force_restart: 是否强制重新开始过渡（即使已经在过渡中）
|
||||
"""
|
||||
if not self.q_current:
|
||||
self.q_current = q_target.copy()
|
||||
self.q_target = q_target.copy()
|
||||
self.q_start = q_target.copy()
|
||||
self.is_transitioning = False
|
||||
return
|
||||
|
||||
if not force_restart and self.is_transitioning:
|
||||
self.q_target = q_target.copy()
|
||||
return
|
||||
|
||||
self.q_start = self.q_current.copy()
|
||||
self.q_target = q_target.copy()
|
||||
self.transition_start_time = time.time()
|
||||
self.is_transitioning = True
|
||||
|
||||
def update(self, dt: float, current_q: Optional[Dict[str, float]] = None) -> Dict[str, float]:
|
||||
"""
|
||||
更新插值状态，返回当前应该下发的平滑角度
|
||||
|
||||
Args:
|
||||
dt: 时间步长（秒）
|
||||
current_q: 可选的当前实际角度（用于初始化或同步）
|
||||
|
||||
Returns:
|
||||
平滑后的关节角度字典
|
||||
"""
|
||||
if current_q is not None and not self.q_current:
|
||||
self.q_current = current_q.copy()
|
||||
self.q_start = current_q.copy()
|
||||
self.q_target = current_q.copy()
|
||||
return self.q_current.copy()
|
||||
|
||||
if not self.is_transitioning:
|
||||
return self.q_target.copy()
|
||||
|
||||
elapsed = time.time() - self.transition_start_time
|
||||
|
||||
if elapsed >= self.transition_time:
|
||||
self.q_current = self.q_target.copy()
|
||||
self.is_transitioning = False
|
||||
return self.q_current.copy()
|
||||
|
||||
s = elapsed / self.transition_time
|
||||
alpha = self._interpolation_funcs[self.method](s)
|
||||
|
||||
self.q_current = {}
|
||||
for joint_name in self.q_target:
|
||||
start_val = self.q_start.get(joint_name, 0.0)
|
||||
target_val = self.q_target[joint_name]
|
||||
self.q_current[joint_name] = start_val + (target_val - start_val) * alpha
|
||||
|
||||
return self.q_current.copy()
|
||||
|
||||
def reset(self, q_init: Optional[Dict[str, float]] = None):
|
||||
"""
|
||||
重置插值器状态
|
||||
|
||||
Args:
|
||||
q_init: 初始角度，如果为 None 则清空所有状态
|
||||
"""
|
||||
if q_init is None:
|
||||
self.q_start = {}
|
||||
self.q_target = {}
|
||||
self.q_current = {}
|
||||
else:
|
||||
self.q_start = q_init.copy()
|
||||
self.q_target = q_init.copy()
|
||||
self.q_current = q_init.copy()
|
||||
|
||||
self.transition_start_time = None
|
||||
self.is_transitioning = False
|
||||
|
||||
def is_done(self) -> bool:
|
||||
"""返回是否已完成当前过渡"""
|
||||
return not self.is_transitioning
|
||||
|
||||
def set_transition_time(self, t: float):
|
||||
"""动态修改过渡时间"""
|
||||
self.transition_time = max(0.01, t)
|
||||
|
||||
def set_method(self, method: str):
|
||||
"""动态修改插值方法"""
|
||||
if method not in self._interpolation_funcs:
|
||||
raise ValueError(f"Unknown method: {method}")
|
||||
self.method = method
|
||||
|
||||
@staticmethod
|
||||
def _linear(s: float) -> float:
|
||||
"""线性插值：alpha = s"""
|
||||
return np.clip(s, 0.0, 1.0)
|
||||
|
||||
@staticmethod
|
||||
def _cubic(s: float) -> float:
|
||||
"""三次多项式：alpha = 3s² - 2s³"""
|
||||
s = np.clip(s, 0.0, 1.0)
|
||||
return 3.0 * s**2 - 2.0 * s**3
|
||||
|
||||
@staticmethod
|
||||
def _quintic(s: float) -> float:
|
||||
"""五次多项式：alpha = 10s³ - 15s⁴ + 6s⁵"""
|
||||
s = np.clip(s, 0.0, 1.0)
|
||||
return 10.0 * s**3 - 15.0 * s**4 + 6.0 * s**5
|
||||
|
||||
@staticmethod
|
||||
def _cosine(s: float) -> float:
|
||||
"""余弦 S 曲线：alpha = (1 - cos(πs)) / 2"""
|
||||
s = np.clip(s, 0.0, 1.0)
|
||||
return (1.0 - np.cos(np.pi * s)) / 2.0
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
print("轨迹插值模块测试")
|
||||
print("=" * 60)
|
||||
|
||||
methods = ['linear', 'cubic', 'quintic', 'cosine']
|
||||
colors = ['blue', 'green', 'red', 'purple']
|
||||
|
||||
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 5))
|
||||
|
||||
for method, color in zip(methods, colors):
|
||||
interp = TrajectoryInterpolator(method=method, transition_time=1.0)
|
||||
|
||||
interp.reset({'joint1': 0.0})
|
||||
interp.set_target({'joint1': 1.0})
|
||||
|
||||
times = []
|
||||
positions = []
|
||||
velocities = []
|
||||
|
||||
t = 0.0
|
||||
dt = 0.01
|
||||
last_pos = 0.0
|
||||
|
||||
while t <= 1.0:
|
||||
q = interp.update(dt)
|
||||
pos = q['joint1']
|
||||
vel = (pos - last_pos) / dt if t > 0 else 0.0
|
||||
|
||||
times.append(t)
|
||||
positions.append(pos)
|
||||
velocities.append(vel)
|
||||
|
||||
last_pos = pos
|
||||
t += dt
|
||||
|
||||
ax1.plot(times, positions, label=method, color=color, linewidth=2)
|
||||
ax2.plot(times, velocities, label=method, color=color, linewidth=2)
|
||||
|
||||
ax1.set_xlabel('时间 (s)')
|
||||
ax1.set_ylabel('位置 (rad)')
|
||||
ax1.set_title('不同插值方法的位置曲线')
|
||||
ax1.legend()
|
||||
ax1.grid(True, alpha=0.3)
|
||||
|
||||
ax2.set_xlabel('时间 (s)')
|
||||
ax2.set_ylabel('速度 (rad/s)')
|
||||
ax2.set_title('不同插值方法的速度曲线')
|
||||
ax2.legend()
|
||||
ax2.grid(True, alpha=0.3)
|
||||
|
||||
plt.tight_layout()
|
||||
plt.savefig('/home/rc2/work/rcwork/trajectory_interpolation_comparison.png', dpi=150)
|
||||
print("已保存对比图到: trajectory_interpolation_comparison.png")
|
||||
|
||||
print("\n测试完成")
|
||||
print("=" * 60)
|
||||
print("推荐使用:")
|
||||
print(" - quintic: 最平滑，速度和加速度连续")
|
||||
print(" - cosine: 平滑且计算简单")
|
||||
print(" - cubic: 速度连续，比 quintic 稍快")
|
||||
print(" - linear: 最简单但速度会突变")
|
||||
Reference in New Issue
Block a user