liyang
ae28d55f81
Source: RGB-D -> Dyn-HaMR -> L20 retargeting -> FoundationPose -> reference repair -> SPIDER, documented in docs/PIPELINE_LATEST.md and docs/SETUP_AND_WEIGHTS.md. Adds FoundationPose and nvdiffrast upstream snapshots, requirements/pipeline_venv.txt and the FoundationPose weight manifest/downloader. Assets (Git LFS): weights/ (WiLoR detector, HandFlow denoiser, UniDepth-L), FoundationPose checkpoints, HaMeR checkpoint, Dyn-HaMR HMP model and BMC constraints, L20 URDF/meshes, the 20260915_171525 D405 recording and the two box CADs. MANO models are not redistributed (third_party/hamer/_DATA/data/mano/README.txt). Environments, caches and run outputs excluded. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
19 lines
1.4 KiB
Python
19 lines
1.4 KiB
Python
"""Interpolate rigid rotations on SO(3), choosing a continuous Euler chart for SPIDER."""
|
|
from pathlib import Path
|
|
import numpy as np,json
|
|
from scipy.spatial.transform import Rotation as R,Slerp
|
|
O=Path(__file__).resolve().parents[1]/'output/collision_fix_20260915';T=O/'datasets/processed/current/l20/bimanual/boxes/0';a=np.load(O/'reference_video_rate.npz');old=np.load(T/'trajectory_kinematic_act.npz');arrays={k:old[k] for k in old.files};times=arrays['time'];t=a['time'];q=a['qpos'];qi=np.stack([np.interp(times,t,q[:,j]) for j in range(q.shape[1])],1)
|
|
def continuous_euler(rot,initial):
|
|
angles=rot.as_euler('XYZ');prev=initial.copy();rows=[]
|
|
for v in angles:
|
|
candidates=np.array([v,[v[0]+np.pi,np.pi-v[1],v[2]+np.pi]])
|
|
candidates+=2*np.pi*np.round((prev-candidates)/(2*np.pi));v=candidates[np.argmin(np.linalg.norm(candidates-prev,axis=1))];rows.append(v);prev=v
|
|
return np.array(rows)
|
|
for start in [0,27,54,60]:
|
|
rotation=Slerp(t,R.from_euler('XYZ',q[:,start+3:start+6]))(np.clip(times,t[0],t[-1]));qi[:,start+3:start+6]=continuous_euler(rotation,q[0,start+3:start+6])
|
|
arrays['qpos']=qi;arrays['qvel']=np.gradient(qi,.0025,axis=0);arrays['qvel'][0]=0
|
|
import mujoco
|
|
m=mujoco.MjModel.from_xml_path(str(T.parent/'scene_act.xml'));arrays['ctrl']=qi[:,m.jnt_qposadr[m.actuator_trnid[:,0]]]
|
|
np.savez_compressed(T/'trajectory_kinematic_act.npz',**arrays)
|
|
print('SO3 interpolation done',qi.shape)
|