jJ d Z ddlZddlZddlZddlmZ ddlmZmZ ddl Z
ddl m Z ddl
mZ ddlmZmZ ded ee
j ee
j e
j f fd
Zd e
j d e
j d
ed e
j fdZded ee fdZded ee
j fdZded e
j fdZde
j ded e
j fdZ de
j de d
eded ee
j ee
j f f
dZ d e fdZ de de
j d e
j fd Z de d e
j d e
j d ee e
j f fd Z! d7d!e
j de d"e
j d#ee
j d
eded ee
j ee
j f fd$Z" d8d*ed+e#d,e$d
eded-ed.e d dfd/Z% d9d*ed+e#d,e$d
ed-ed.e d dfd0Z& d8d*ed+e#d,e$d
eded-ed.e d dfd1Z' d:d2e
j d3ee
j de d,e$d
ed*ee d4ed5 d.e d dfd6Z(dS );a Replay recorded arm motion.
Two source formats are supported, selected automatically based on the contents
of the episode directory:
Raw (``robot_data.npz``)
Joint commands are streamed directly at the original ~100 Hz timing,
resampled to ``control_hz``. No IK is required.
Processed (``lowdim/.pkl``)
EE-pose action sequence at ~30 fps is loaded, IK is solved at each
keyframe, and the result is interpolated to ``control_hz``.
The action layout stored in lowdim is::
[l_pos(3), l_rot9(9), l_gripper(1), r_pos(3), r_rot9(9), r_gripper(1)]
Poses are in the left-arm base frame. The right arm target is transformed
back to right-arm base coordinates using the bimanual calibration before IK.
N)Path)OptionalTuple)
Kinematics) get_yam_4310_linear_xml_path)RobotControllersmooth_move_jointsep_dirreturnc | dz }| st d| t j | }|d t j }| d }| | t j nd}|d t j }|||fS )u Load joint commands from a raw ``robot_data.npz`` file.
Returns:
joints_l: (N, 7) float32 — left arm commanded joint positions.
joints_r: (N, 7) float32 or None — right arm commanded joint positions.
timestamps: (N,) int64 nanoseconds.
robot_data.npzzNo robot_data.npz in follower_l_joint_cmdfollower_r_joint_cmdN
timestamps)existsFileNotFoundErrornploadastypefloat32getint64)r
npz_pathdatajoints_l joints_r_rawjoints_rr s :/home/robot-lab/cameron/raiden_fork/raiden/robot/replay.py_load_raw_jointsr # s ((H ?? B @ @ @AAA
78 D*+222:>>H88 233L2>2J|""2:...PTHl#**2844J Xz )) joints
timestamps_ns
control_hzc j |dz t t d d z
|z dz }t j d d | t j fdt j d D d t j S )zLResample a joint trajectory from variable-rate timestamps to ``control_hz``. eAr c P g | ]"}t j d d |f #S Nr interp).0dr! t_finet_srcs r
z$_resample_joints..B s3 PPPA65&A, / /PPPr axis) introundr linspacestackrangeshaper r )r! r" r# n_outr. r/ s ` @@r _resample_jointsr: 8 s
C E uRy58+z9:: ; ;a ?E
[q59e
4
4F
8PPPPPPv|A9O9OPPP
fRZr c p | dz }t | d }|s"t | d }|st d| g }|D ]Q}t |d 5 }| t j | ddd n # 1 swxY w Y R|S )z.` s. JJJaAo&bj 9 9 9JJJr )rH r r6 r
rE s r _load_joint_sequencerR V sJ v
&
&FfQi''t
8JJ6JJJ r c t | }d|d vrt d| t j d |D S )zAReturn (N, 26) float32 array of EE poses loaded from lowdim pkls.actionr z+'action' key missing in lowdim files under c Z g | ](}t j |d t j )S )rT rL rN rP s r r0 z)_load_action_sequence..j s- CCCqAhKrz 2 2 2CCCr )rH KeyErrorr r6 rQ s r _load_action_sequencerW d sZ
v
&
&Fvay MVMMNNN
8CCFCCC r
action_vecoffsetc , | ||dz t j }| |dz |dz dd t j }t j d }||dddf< ||ddddf< |S )u Extract a 4×4 TCP pose from a 26-D action vector.
Args:
action_vec: (26,) array.
offset: 0 for left arm, 13 for right arm.
N)r r float64reshapeeye)rX rY posrotTs r _pose_from_actionrd n s Vfqj(
)
0
0
<
._interp.. 3 Q Q QqRYvvtAAAqDz
2
2 Q Q Qr r' r1 r r6 r7 r8 r r )ri r. rl s `r _interpz!_upsample_joints.._interp \ x Q Q Q Q Q QE$*Q-r c : t t d S )N
grasp_site)r r r r _get_kinematicsr s 244l C CCr kinq6c | j j j }t j |t j }||dd< | | dddf t j S )z.Return (3,) EE position from 6-DOF arm joints.rL N r[ ) _configurationmodelnqr zerosr^ fkr r )r r r qs r
_fk_ee_xyzr s` ! $B
2:&&&A AbqbE 66!99RaRU " "2: . ..r target_poseinit_qc | j j j }t | |k r6t j |t j }||dt | < |}| |d| \ }}||dd fS )zJIK wrapper that pads init_q to model nq and returns only the 6 arm joints.rL Nr} )r r )r r r rs r r r^ ik)r r r r q_fullsuccessr s r _ik_6dofr s
! $B
6{{R"BJ/// &}V} \&AAJGQ AbqbE>r actions robot_l_init robot_r_initc t | }||z }t d t }|dd t
j } |9|dd t
j nd}
t d| d t j |dft
j } |r"t j |dft
j nd} | dddf t
j }
|r)| dddf t
j nd}t | D ]\ }}t |d
}t ||| \ }} | | |< |r,|
*t |d
}t |||
\ }}
|
| |< |d z d
z d k s ||d z
k rt d|d z d| dd t dt
j dt
j dt
j fd} || |
}|r| || | nd}|d z
|z d z }t j
|t
j t j d |d z
| dt
j dt
j f fd } || }| || nd}||fS )a3 IK all keyframes then upsample to control_hz.
Action poses are stored per-arm in each arm's own base frame, so no
coordinate transform is needed before IK.
Returns:
traj_l: (M, 7) float32 joint trajectory for left arm.
traj_r: (M, 7) float32 or None if right arm not used.
zSetting up i2rt IK...Nr zSolving IK for z
keyframes...rL r\ r )rY
r' r z IK /
T)endflushkeysgripsr c t j | t j |d d d f gd S )Nr' r1 )r concatenater r )r r s r
_to_mujoco_7dz)_solve_ik_sequence.._to_mujoco_7d s? ~
[[
$
$eAAAtGn 5A
r trajc t j fdt j d D d t j S )Nc P g | ]"}t j d d |f #S r) r* )r, r- r. rl r s r r0 z7_solve_ik_sequence.._interp.. rm r r' r1 rn )r r. rl s `r ro z#_solve_ik_sequence.._interp rp r )rs printr copyr r r^ r r enumeraterd r ru rt r5 ) r rf r r r# rg rv rw r q_lq_rkeys_lkeys_rgrips_lgrips_riactT_tcp_l_T_tcp_rr traj_l_keys traj_r_keysrx ro ry rz r. rl s @@r _solve_ik_sequencer s, " \\FY&H
!"""
C rr
!
!
(
(
4
4C6B6N RaR &&rz222TX
1F
1
1
1222
Xvqk
4
4
4F8A
KRXvqk
4
4
4
4tFaaaen##BJ//G3<Fgaaaen##BJ///$GG$$ B B3#C222#w,,3q 'B777G c7C00FAsF1I
ER<1 VaZ *!a%**&**D A A A A GGG
BJ
rz
bj
-00K*3T8J
fg&&&PT
!x !#A
YvRZ
0
0
0F
[FQJ
*
*F bj RZ W[
!
!F%0%r bimanual ? r' F
recording_dirarmsspeedstride visualizec | dz rt | ||||| dS | dz rt | |||||| dS t d| )a Replay a recorded episode.
Auto-detects the source format:
- **Raw** (``robot_data.npz`` present): streams joint commands directly,
no IK required.
- **Processed** (``lowdim/`` present): solves IK from EE poses.
Args:
recording_dir: Path to a raw or converted episode directory.
arms: ``"bimanual"`` or ``"single"`` (left only).
speed: Playback speed multiplier. 1.0 = real-time.
control_hz: Command rate for streaming (default 150 Hz).
camera_hz: Frame rate of the lowdim action sequence (default 30 Hz,
processed source only).
stride: Subsample every N-th frame to match the shardify stride
(default 1 = native rate, 3 = 10 Hz from 30 Hz recordings).
r
)r r r# r r r< )r r r# rg r r z,Neither robot_data.npz nor lowdim/ found in N)r _run_raw_replay_run_processed_replayr )r r r r# rg r r s r
run_replayr s 6 ((0022
!
(
" * * , ,
!
J= J J
r c F t | \ }}}|dk o|du} |dk r%|dd| }| |dd| nd}|dd| }t ||| }
| rt ||| nd} t |
} |d |d z
dz }
t d| j d t d t | d
|
d d | d
t d| d| d t d| t d| d t |
| | ||| dS )zBReplay directly from raw joint commands in robot_data.npz (no IK).r Nr' r& r r% Recording : z (raw)z Samples : (.1fz s, stride=) Control Hz: z Hz (z frames after resample) Arms : Speed : x)rf r r# r )r r: rs r name_stream_trajectories)r r r r# r r r r r rf ry rz n_frames
duration_ss r r r 4 s &6m%D%D"Hh
";xt';I
zzHHfH%)1)=8HHfH%%4 &)
h
J
?
?FCL
V
h
J
?
?
?RVF6{{H R.:a=0C7J
4 +
4
4
4555
OX
O
O:
O
O
Of
O
O
OPPP
L
L
L8
L
L
LMMM
!
!
!
!"""
r c |dk }t | }|dk r |dd| }||z } t | }
|
| z } t d| j d t d|
d| dd | d
| d t d | d
|| z d t d| t d| d t |ddd } | | j r| j nt j d }
|r | j
r| j
nd}t ||o| j
du|
||| \ }}t |||||| | n # t $ r t d Y nw xY w| | dS # | | w xY w)z>Replay from processed lowdim pkl files using IK from EE poses.r r' Nr z (processed, IK)z Keyframes : r r z s at z
fps, stride=r r u Hz (upsample ×r r r TFuse_right_followeruse_left_followeruse_right_leaderuse_left_leaderrq )rf r r r# rg )rf r r# robotr
Replay interrupted.)rW rs r r r initialize_robots
follower_l
get_joint_posr r
follower_rr r KeyboardInterruptmove_to_home_positionsclose)r r r r# rg r r rf r effective_hzrv r r r r ry rz s r r r Z s
"I#M22G
zz((F(# &L
\\F,&J
> +
>
>
>??? \v\\*\\\ \\SY\\\
S
S
SjL6P
S
S
STTT
!
!
!
!""" $
E
050@ QE * * , , ,bhqkk
# .
E * * , , , , @E$4D$@%%!"
!
' ' '
%&&&&&' $$&&&
$$&&&
s+ BE+ *F4 +FF4 FF4 4*G ry rz r r c
|du }|r't |ddd }| d} g }
g } g } g }
t d|dz }|rddl}ddlm} | d d
}d }| | }| |d
d| d ||d }t d| t d| d| d| d| d t } t | } t d g }|j ?|
t j t |j | d fddd |rH|j A|?|
t j t |j |d fddd |D ]}| |D ]}| t d d ||z z }t) j }t- | D ]}|||z z }|t) j z
} | dk rt) j | |j |j | | |r)|j "| |j || |r| ||z dk rddl}| d | |
t5 | | |dd f | d!| |
gg d"g# |r^|\|
t5 | ||dd f | d$| | gg d%g# ||j q|
t5 | |j dd | d&| | gg d'g# |rx|j q|
t5 | |j dd | d(| |
gg d)g# |d*z dk r;t) j |z
} ||z } t d+| d,| d-| d.d/| d.d0 d12 t d3| d4| d5 n # t< $ r t d6 Y nw xY w|r*| | dS dS # |r)| | w w xY w)7zConnect to (or reuse) the robot, move to start, then stream joint trajectories.
If *robot* is None a new ``RobotController`` is created and closed when
done. Pass an already-initialized controller to avoid reconnecting.
NTFr r' r r )quote
raiden_replayi& i& ) grpc_port)web_port open_browserzhttp://localhost:z?url= )safez
Rerun viewer: zSSH tunnel: ssh -L z :localhost:z -L z
z
Moving to start position...g @d )time_interval_ssteps)targetargskwargsz Replaying... (Ctrl-C to stop)
r frame)sequencer ztrajectory/commanded/left)r r )colorsztrajectory/commanded/right)r x ztrajectory/actual/left)r P r ztrajectory/actual/right)r r r r z r z
frames (r z
s elapsed / z s total)r )r z
Replay complete (z frames at z Hz).r )!r r maxrerun urllib.parser init
serve_grpcserve_web_viewerr r rs r rA threadingThreadr r startjointime monotonicr7 sleepcommand_joint_posset_timer log LineStrips3Dr r r r ) ry rz rf r r# r r r
owns_robotrr_kin cmd_pts_l cmd_pts_r act_pts_l act_pts_r log_everyrrr r r
server_uri
viewer_urlr threadstdt_st_startr t_targetr elapsedtotal_ss r r r s $ J " ( " " !
!!! FIIIIAzR'((I #&&&&&&
]]Y]77
XEBBBTTTjr8R8R8RTT
.*..///
>X
>
>(
>
>
>
>)2
>
>
>
!""6{{H]
-... ' NN - *F1I6/2SAA
)5&:L NN - *F1I6/2SAA
A
GGIIII A
FFHHHH /000j5().""x 6 6 A T)H t~ / //Eqyy
5!!! + 226!9===
>U - 9f>P 226!9=== "
V /A MQ4F4F"""" Ga 000 FF1bqb5M!B!BCCC/OOYK
OFF
!3 $$Zq"1"u
%F%FGGGFF4 ]]]OLL
$ '3!(( &vu/?/M/M/O/OPRQRPR/S T T 4 OOYK O O O ! U%5%A!(( &vu/?/M/M/O/OPRQRPR/S T T 5 OOYK O P P
3w!|| .**W4"T/I I IX I I HI I29HI I I JHJJJJJKKKK ' ' '
%&&&&&' ( ( * * * KKMMMMM : ( ( * * * KKMMMM s+ r# sB *
" " " " " " " " - - - - - - : : : : : : G G G G G G G G* *
2:x
+RZ 78* * * ** J : Z d tDz (2:*> $ 2:
"*
c
bj
z
2:x
+ +, :D D D D D/J /BJ /2: / / / /
"$*
68j
4
&
J J
ZJJ *J 2:& J
J
J 2:x
+ +,J J J J^ 1
1
1
1
1
1
1
1
1
1
1
1
1
l
# ##
# # #
#
#
# # # #P B BB
B B B
B
B B
B B B BV %))-P P JP RZ P P P
P D>
P % &P P
P P P P P Pr