441 lines
13 KiB
Python
441 lines
13 KiB
Python
import math
|
||
from typing import Any
|
||
|
||
import numpy as np
|
||
import torch
|
||
import tyro
|
||
from tqdm import tqdm
|
||
|
||
import mjlab
|
||
from mjlab.entity import Entity
|
||
from mjlab.scene import Scene
|
||
from mjlab.sim.sim import Simulation, SimulationCfg
|
||
from mjlab.tasks.tracking.config.g1.env_cfgs import unitree_g1_flat_tracking_env_cfg
|
||
from mjlab.utils.lab_api.math import (
|
||
axis_angle_from_quat,
|
||
quat_conjugate,
|
||
quat_mul,
|
||
quat_slerp,
|
||
)
|
||
from mjlab.viewer.offscreen_renderer import OffscreenRenderer
|
||
from mjlab.viewer.viewer_config import ViewerConfig
|
||
|
||
|
||
class MotionLoader:
|
||
def __init__(
|
||
self,
|
||
motion_file: str,
|
||
input_fps: int,
|
||
output_fps: int,
|
||
device: torch.device | str,
|
||
line_range: tuple[int, int] | None = None,
|
||
):
|
||
self.motion_file = motion_file
|
||
self.input_fps = input_fps
|
||
self.output_fps = output_fps
|
||
self.input_dt = 1.0 / self.input_fps
|
||
self.output_dt = 1.0 / self.output_fps
|
||
self.current_idx = 0
|
||
self.device = device
|
||
self.line_range = line_range
|
||
self._load_motion()
|
||
self._interpolate_motion()
|
||
self._compute_velocities()
|
||
|
||
def _load_motion(self):
|
||
"""Loads the motion from the csv file."""
|
||
if self.line_range is None:
|
||
motion = torch.from_numpy(np.loadtxt(self.motion_file, delimiter=","))
|
||
else:
|
||
motion = torch.from_numpy(
|
||
np.loadtxt(
|
||
self.motion_file,
|
||
delimiter=",",
|
||
skiprows=self.line_range[0] - 1,
|
||
max_rows=self.line_range[1] - self.line_range[0] + 1,
|
||
)
|
||
)
|
||
motion = motion.to(torch.float32).to(self.device)
|
||
# motion[:, 2] -= 0.05
|
||
self.motion_base_poss_input = motion[:, :3]
|
||
self.motion_base_rots_input = motion[:, 3:7]
|
||
self.motion_base_rots_input = self.motion_base_rots_input[
|
||
:, [3, 0, 1, 2]
|
||
] # convert to wxyz
|
||
self.motion_dof_poss_input = motion[:, 7:]
|
||
|
||
self.input_frames = motion.shape[0]
|
||
self.duration = (self.input_frames - 1) * self.input_dt
|
||
|
||
def _interpolate_motion(self):
|
||
"""Interpolates the motion to the output fps."""
|
||
# Include the end point when it lands on the output grid. The tolerance stops
|
||
# float rounding from dropping it (e.g. 2.3 / 0.02 = 114.99999999999999).
|
||
num_frames = math.floor(self.duration / self.output_dt + 1e-6) + 1
|
||
times = (
|
||
torch.arange(num_frames, device=self.device, dtype=torch.float32) * self.output_dt
|
||
)
|
||
self.output_frames = times.shape[0]
|
||
index_0, index_1, blend = self._compute_frame_blend(times)
|
||
self.motion_base_poss = self._lerp(
|
||
self.motion_base_poss_input[index_0],
|
||
self.motion_base_poss_input[index_1],
|
||
blend.unsqueeze(1),
|
||
)
|
||
self.motion_base_rots = self._slerp(
|
||
self.motion_base_rots_input[index_0],
|
||
self.motion_base_rots_input[index_1],
|
||
blend,
|
||
)
|
||
self.motion_dof_poss = self._lerp(
|
||
self.motion_dof_poss_input[index_0],
|
||
self.motion_dof_poss_input[index_1],
|
||
blend.unsqueeze(1),
|
||
)
|
||
print(
|
||
f"Motion interpolated, input frames: {self.input_frames}, "
|
||
f"input fps: {self.input_fps}, "
|
||
f"output frames: {self.output_frames}, "
|
||
f"output fps: {self.output_fps}"
|
||
)
|
||
|
||
def _lerp(
|
||
self, a: torch.Tensor, b: torch.Tensor, blend: torch.Tensor
|
||
) -> torch.Tensor:
|
||
"""Linear interpolation between two tensors."""
|
||
return a * (1 - blend) + b * blend
|
||
|
||
def _slerp(
|
||
self, a: torch.Tensor, b: torch.Tensor, blend: torch.Tensor
|
||
) -> torch.Tensor:
|
||
"""Spherical linear interpolation between two quaternions."""
|
||
slerped_quats = torch.zeros_like(a)
|
||
for i in range(a.shape[0]):
|
||
slerped_quats[i] = quat_slerp(a[i], b[i], float(blend[i]))
|
||
return slerped_quats
|
||
|
||
def _compute_frame_blend(
|
||
self, times: torch.Tensor
|
||
) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]:
|
||
"""Computes the frame blend for the motion."""
|
||
phase = times / self.duration
|
||
index_0 = (phase * (self.input_frames - 1)).floor().long()
|
||
index_1 = torch.minimum(index_0 + 1, torch.tensor(self.input_frames - 1))
|
||
blend = phase * (self.input_frames - 1) - index_0
|
||
return index_0, index_1, blend
|
||
|
||
def _compute_velocities(self):
|
||
"""Computes the velocities of the motion."""
|
||
self.motion_base_lin_vels = torch.gradient(
|
||
self.motion_base_poss, spacing=self.output_dt, dim=0
|
||
)[0]
|
||
self.motion_dof_vels = torch.gradient(
|
||
self.motion_dof_poss, spacing=self.output_dt, dim=0
|
||
)[0]
|
||
self.motion_base_ang_vels = self._so3_derivative(
|
||
self.motion_base_rots, self.output_dt
|
||
)
|
||
|
||
def _so3_derivative(self, rotations: torch.Tensor, dt: float) -> torch.Tensor:
|
||
"""Computes the derivative of a sequence of SO3 rotations.
|
||
|
||
Args:
|
||
rotations: shape (B, 4).
|
||
dt: time step.
|
||
Returns:
|
||
shape (B, 3).
|
||
"""
|
||
q_prev, q_next = rotations[:-2], rotations[2:]
|
||
q_rel = quat_mul(q_next, quat_conjugate(q_prev)) # shape (B−2, 4)
|
||
|
||
omega = axis_angle_from_quat(q_rel) / (2.0 * dt) # shape (B−2, 3)
|
||
omega = torch.cat(
|
||
[omega[:1], omega, omega[-1:]], dim=0
|
||
) # repeat first and last sample
|
||
return omega
|
||
|
||
def get_next_state(
|
||
self,
|
||
) -> tuple[
|
||
tuple[
|
||
torch.Tensor,
|
||
torch.Tensor,
|
||
torch.Tensor,
|
||
torch.Tensor,
|
||
torch.Tensor,
|
||
torch.Tensor,
|
||
],
|
||
bool,
|
||
]:
|
||
"""Gets the next state of the motion."""
|
||
state = (
|
||
self.motion_base_poss[self.current_idx : self.current_idx + 1],
|
||
self.motion_base_rots[self.current_idx : self.current_idx + 1],
|
||
self.motion_base_lin_vels[self.current_idx : self.current_idx + 1],
|
||
self.motion_base_ang_vels[self.current_idx : self.current_idx + 1],
|
||
self.motion_dof_poss[self.current_idx : self.current_idx + 1],
|
||
self.motion_dof_vels[self.current_idx : self.current_idx + 1],
|
||
)
|
||
self.current_idx += 1
|
||
reset_flag = False
|
||
if self.current_idx >= self.output_frames:
|
||
self.current_idx = 0
|
||
reset_flag = True
|
||
return state, reset_flag
|
||
|
||
|
||
def run_sim(
|
||
sim: Simulation,
|
||
scene: Scene,
|
||
joint_names,
|
||
input_file,
|
||
input_fps,
|
||
output_fps,
|
||
output_name,
|
||
render,
|
||
line_range,
|
||
renderer: OffscreenRenderer | None = None,
|
||
):
|
||
motion = MotionLoader(
|
||
motion_file=input_file,
|
||
input_fps=input_fps,
|
||
output_fps=output_fps,
|
||
device=sim.device,
|
||
line_range=line_range,
|
||
)
|
||
|
||
robot: Entity = scene["robot"]
|
||
robot_joint_indexes = robot.find_joints(joint_names, preserve_order=True)[0]
|
||
|
||
log: dict[str, Any] = {
|
||
"fps": [output_fps],
|
||
"joint_pos": [],
|
||
"joint_vel": [],
|
||
"body_pos_w": [],
|
||
"body_quat_w": [],
|
||
"body_lin_vel_w": [],
|
||
"body_ang_vel_w": [],
|
||
}
|
||
file_saved = False
|
||
|
||
frames = []
|
||
scene.reset()
|
||
|
||
print(f"\nStarting simulation with {motion.output_frames} frames...")
|
||
if render:
|
||
print("Rendering enabled - generating video frames...")
|
||
|
||
# Create progress bar
|
||
pbar = tqdm(
|
||
total=motion.output_frames,
|
||
desc="Processing frames",
|
||
unit="frame",
|
||
ncols=100,
|
||
bar_format="{l_bar}{bar}| {n_fmt}/{total_fmt} [{elapsed}<{remaining}, {rate_fmt}]",
|
||
)
|
||
|
||
frame_count = 0
|
||
while not file_saved:
|
||
(
|
||
(
|
||
motion_base_pos,
|
||
motion_base_rot,
|
||
motion_base_lin_vel,
|
||
motion_base_ang_vel,
|
||
motion_dof_pos,
|
||
motion_dof_vel,
|
||
),
|
||
reset_flag,
|
||
) = motion.get_next_state()
|
||
|
||
root_states = robot.data.default_root_state.clone()
|
||
root_states[:, 0:3] = motion_base_pos
|
||
root_states[:, :2] += scene.env_origins[:, :2]
|
||
root_states[:, 3:7] = motion_base_rot
|
||
root_states[:, 7:10] = motion_base_lin_vel
|
||
root_states[:, 10:] = motion_base_ang_vel
|
||
robot.write_root_state_to_sim(root_states)
|
||
|
||
joint_pos = robot.data.default_joint_pos.clone()
|
||
joint_vel = robot.data.default_joint_vel.clone()
|
||
joint_pos[:, robot_joint_indexes] = motion_dof_pos
|
||
joint_vel[:, robot_joint_indexes] = motion_dof_vel
|
||
robot.write_joint_state_to_sim(joint_pos, joint_vel)
|
||
|
||
sim.forward()
|
||
scene.update(sim.mj_model.opt.timestep)
|
||
if render and renderer is not None:
|
||
renderer.update(sim.data)
|
||
frames.append(renderer.render())
|
||
|
||
if not file_saved:
|
||
log["joint_pos"].append(robot.data.joint_pos[0, :].cpu().numpy().copy())
|
||
log["joint_vel"].append(robot.data.joint_vel[0, :].cpu().numpy().copy())
|
||
log["body_pos_w"].append(robot.data.body_link_pos_w[0, :].cpu().numpy().copy())
|
||
log["body_quat_w"].append(robot.data.body_link_quat_w[0, :].cpu().numpy().copy())
|
||
log["body_lin_vel_w"].append(
|
||
robot.data.body_link_lin_vel_w[0, :].cpu().numpy().copy()
|
||
)
|
||
log["body_ang_vel_w"].append(
|
||
robot.data.body_link_ang_vel_w[0, :].cpu().numpy().copy()
|
||
)
|
||
|
||
torch.testing.assert_close(
|
||
robot.data.body_link_lin_vel_w[0, 0], motion_base_lin_vel[0]
|
||
)
|
||
torch.testing.assert_close(
|
||
robot.data.body_link_ang_vel_w[0, 0], motion_base_ang_vel[0]
|
||
)
|
||
|
||
frame_count += 1
|
||
pbar.update(1)
|
||
|
||
if frame_count % 100 == 0: # Update every 100 frames to avoid spam
|
||
elapsed_time = frame_count / output_fps
|
||
pbar.set_description(f"Processing frames (t={elapsed_time:.1f}s)")
|
||
|
||
if reset_flag and not file_saved:
|
||
file_saved = True
|
||
pbar.close()
|
||
|
||
print("\nStacking arrays and saving data...")
|
||
for k in (
|
||
"joint_pos",
|
||
"joint_vel",
|
||
"body_pos_w",
|
||
"body_quat_w",
|
||
"body_lin_vel_w",
|
||
"body_ang_vel_w",
|
||
):
|
||
log[k] = np.stack(log[k], axis=0)
|
||
|
||
print("Saving to /tmp/motion.npz...")
|
||
np.savez("/tmp/motion.npz", **log)
|
||
|
||
print("Uploading to Weights & Biases...")
|
||
import wandb
|
||
|
||
COLLECTION = output_name
|
||
run = wandb.init(project="csv_to_npz", name=COLLECTION)
|
||
print(f"[INFO]: Logging motion to wandb: {COLLECTION}")
|
||
REGISTRY = "motions"
|
||
logged_artifact = run.log_artifact(
|
||
artifact_or_path="/tmp/motion.npz", name=COLLECTION, type=REGISTRY
|
||
)
|
||
run.link_artifact(
|
||
artifact=logged_artifact,
|
||
target_path=f"wandb-registry-{REGISTRY}/{COLLECTION}",
|
||
)
|
||
print(f"[INFO]: Motion saved to wandb registry: {REGISTRY}/{COLLECTION}")
|
||
|
||
if render:
|
||
import mediapy as media
|
||
|
||
print("Creating video...")
|
||
media.write_video("./motion.mp4", frames, fps=output_fps)
|
||
|
||
print("Logging video to wandb...")
|
||
wandb.log({"motion_video": wandb.Video("./motion.mp4", format="mp4")})
|
||
|
||
wandb.finish()
|
||
|
||
|
||
def main(
|
||
input_file: str,
|
||
output_name: str,
|
||
input_fps: float = 30.0,
|
||
output_fps: float = 50.0,
|
||
device: str = "cuda:0",
|
||
render: bool = False,
|
||
line_range: tuple[int, int] | None = None,
|
||
):
|
||
"""Replay motion from CSV file and output to npz file.
|
||
|
||
Args:
|
||
input_file: Path to the input CSV file.
|
||
output_name: Path to the output npz file.
|
||
input_fps: Frame rate of the CSV file.
|
||
output_fps: Desired output frame rate.
|
||
device: Device to use.
|
||
render: Whether to render the simulation and save a video.
|
||
line_range: Range of lines to process from the CSV file.
|
||
"""
|
||
if device.startswith("cuda") and not torch.cuda.is_available():
|
||
print("[WARNING]: CUDA is not available. Falling back to CPU. This may be slow.")
|
||
device = "cpu"
|
||
|
||
sim_cfg = SimulationCfg()
|
||
sim_cfg.mujoco.timestep = 1.0 / output_fps
|
||
|
||
scene = Scene(unitree_g1_flat_tracking_env_cfg().scene, device=device)
|
||
model = scene.compile()
|
||
|
||
sim = Simulation(num_envs=1, cfg=sim_cfg, model=model, device=device)
|
||
|
||
scene.initialize(sim.mj_model, sim.model, sim.data)
|
||
|
||
renderer = None
|
||
if render:
|
||
viewer_cfg = ViewerConfig(
|
||
height=480,
|
||
width=640,
|
||
origin_type=ViewerConfig.OriginType.ASSET_ROOT,
|
||
entity_name="robot",
|
||
distance=2.0,
|
||
elevation=-5.0,
|
||
azimuth=20,
|
||
)
|
||
renderer = OffscreenRenderer(
|
||
model=sim.mj_model,
|
||
cfg=viewer_cfg,
|
||
scene=scene,
|
||
)
|
||
renderer.initialize()
|
||
|
||
run_sim(
|
||
sim=sim,
|
||
scene=scene,
|
||
joint_names=[
|
||
"left_hip_pitch_joint",
|
||
"left_hip_roll_joint",
|
||
"left_hip_yaw_joint",
|
||
"left_knee_joint",
|
||
"left_ankle_pitch_joint",
|
||
"left_ankle_roll_joint",
|
||
"right_hip_pitch_joint",
|
||
"right_hip_roll_joint",
|
||
"right_hip_yaw_joint",
|
||
"right_knee_joint",
|
||
"right_ankle_pitch_joint",
|
||
"right_ankle_roll_joint",
|
||
"waist_yaw_joint",
|
||
"waist_roll_joint",
|
||
"waist_pitch_joint",
|
||
"left_shoulder_pitch_joint",
|
||
"left_shoulder_roll_joint",
|
||
"left_shoulder_yaw_joint",
|
||
"left_elbow_joint",
|
||
"left_wrist_roll_joint",
|
||
"left_wrist_pitch_joint",
|
||
"left_wrist_yaw_joint",
|
||
"right_shoulder_pitch_joint",
|
||
"right_shoulder_roll_joint",
|
||
"right_shoulder_yaw_joint",
|
||
"right_elbow_joint",
|
||
"right_wrist_roll_joint",
|
||
"right_wrist_pitch_joint",
|
||
"right_wrist_yaw_joint",
|
||
],
|
||
input_fps=input_fps,
|
||
input_file=input_file,
|
||
output_fps=output_fps,
|
||
output_name=output_name,
|
||
render=render,
|
||
line_range=line_range,
|
||
renderer=renderer,
|
||
)
|
||
|
||
|
||
if __name__ == "__main__":
|
||
tyro.cli(main, config=mjlab.TYRO_FLAGS)
|