mirror of
https://github.com/zenlm/vjepa2.git
synced 2026-10-30 19:33:29 +00:00
227 lines
7.4 KiB
Python
227 lines
7.4 KiB
Python
# Copyright (c) Meta Platforms, Inc. and affiliates.
|
|
#
|
|
# This source code is licensed under the license found in the
|
|
# LICENSE file in the root directory of this source tree.
|
|
|
|
|
|
import numpy as np
|
|
import torch
|
|
from scipy.spatial.transform import Rotation
|
|
from tqdm import tqdm
|
|
|
|
from src.utils.logging import get_logger
|
|
|
|
logger = get_logger(__name__, force=True)
|
|
|
|
|
|
def l1(a, b):
|
|
return torch.mean(torch.abs(a - b), dim=-1)
|
|
|
|
|
|
def round_small_elements(tensor, threshold):
|
|
mask = torch.abs(tensor) < threshold
|
|
new_tensor = tensor.clone()
|
|
new_tensor[mask] = 0
|
|
return new_tensor
|
|
|
|
|
|
def cem(
|
|
context_frame,
|
|
context_pose,
|
|
goal_frame,
|
|
world_model,
|
|
rollout=1,
|
|
cem_steps=100,
|
|
momentum_mean=0.25,
|
|
momentum_std=0.95,
|
|
momentum_mean_gripper=0.15,
|
|
momentum_std_gripper=0.15,
|
|
samples=100,
|
|
topk=10,
|
|
verbose=False,
|
|
maxnorm=0.05,
|
|
axis={},
|
|
objective=l1,
|
|
close_gripper=None,
|
|
):
|
|
"""
|
|
:param context_frame: [B=1, T=1, HW, D]
|
|
:param goal_frame: [B=1, T=1, HW, D]
|
|
:param world_model: f(context_frame, action) -> next_frame [B, 1, HW, D]
|
|
:return: [B=1, rollout, 7] an action trajectory over rollout horizon
|
|
|
|
Cross-Entropy Method
|
|
-----------------------
|
|
1. for rollout horizon:
|
|
1.1. sample several actions
|
|
1.2. compute next states using WM
|
|
3. compute similarity of final states to goal_frames
|
|
4. select topk samples and update mean and std using topk action trajs
|
|
5. choose final action to be mean of distribution
|
|
"""
|
|
context_frame = context_frame.repeat(samples, 1, 1, 1) # Reshape to [S, 1, HW, D]
|
|
goal_frame = goal_frame.repeat(samples, 1, 1, 1) # Reshape to [S, 1, HW, D]
|
|
context_pose = context_pose.repeat(samples, 1, 1) # Reshape to [S, 1, 7]
|
|
|
|
# Current estimate of the mean/std of distribution over action trajectories
|
|
mean = torch.cat(
|
|
[
|
|
torch.zeros((rollout, 3), device=context_frame.device),
|
|
torch.zeros((rollout, 1), device=context_frame.device),
|
|
],
|
|
dim=-1,
|
|
)
|
|
|
|
std = torch.cat(
|
|
[
|
|
torch.ones((rollout, 3), device=context_frame.device) * maxnorm,
|
|
torch.ones((rollout, 1), device=context_frame.device),
|
|
],
|
|
dim=-1,
|
|
)
|
|
|
|
for ax in axis.keys():
|
|
mean[:, ax] = axis[ax]
|
|
|
|
def sample_action_traj():
|
|
"""Sample several action trajectories"""
|
|
action_traj, frame_traj, pose_traj = None, context_frame, context_pose
|
|
|
|
for h in range(rollout):
|
|
|
|
# -- sample new action
|
|
action_samples = torch.randn(samples, mean.size(1), device=mean.device) * std[h] + mean[h]
|
|
action_samples[:, :3] = torch.clip(action_samples[:, :3], min=-maxnorm, max=maxnorm)
|
|
action_samples[:, -1:] = torch.clip(action_samples[:, -1:], min=-0.75, max=0.75)
|
|
for ax in axis.keys():
|
|
action_samples[:, ax] = axis[ax]
|
|
action_samples = torch.cat(
|
|
[
|
|
action_samples[:, :3],
|
|
torch.zeros((len(action_samples), 3), device=mean.device),
|
|
action_samples[:, -1:],
|
|
],
|
|
dim=-1,
|
|
)[:, None]
|
|
if close_gripper is not None and h >= close_gripper:
|
|
action_samples[:, :, -1] = 1.0
|
|
|
|
action_traj = (
|
|
torch.cat([action_traj, action_samples], dim=1) if action_traj is not None else action_samples
|
|
)
|
|
|
|
# -- compute next state
|
|
next_frame, next_pose = world_model(frame_traj, action_traj, pose_traj)
|
|
frame_traj = torch.cat([frame_traj, next_frame], dim=1)
|
|
pose_traj = torch.cat([pose_traj, next_pose], dim=1)
|
|
|
|
return action_traj, frame_traj
|
|
|
|
def select_topk_action_traj(final_state, goal_state, actions):
|
|
"""Get the topk action trajectories that bring us closest to goal"""
|
|
sims = objective(final_state.flatten(1), goal_state.flatten(1))
|
|
indices = sims.topk(topk, largest=False).indices
|
|
selected_actions = actions[indices]
|
|
return selected_actions
|
|
|
|
for step in tqdm(range(cem_steps), disable=True):
|
|
action_traj, frame_traj = sample_action_traj()
|
|
selected_actions = select_topk_action_traj(
|
|
final_state=frame_traj[:, -1], goal_state=goal_frame, actions=action_traj
|
|
)
|
|
mean_selected_actions = selected_actions.mean(dim=0)
|
|
std_selected_actions = selected_actions.std(dim=0)
|
|
|
|
# -- Update new sampling mean and std based on the top-k samples
|
|
mean = torch.cat(
|
|
[
|
|
mean_selected_actions[..., :3] * (1.0 - momentum_mean) + mean[..., :3] * momentum_mean,
|
|
mean_selected_actions[..., -1:] * (1.0 - momentum_mean_gripper)
|
|
+ mean[..., -1:] * momentum_mean_gripper,
|
|
],
|
|
dim=-1,
|
|
)
|
|
std = torch.cat(
|
|
[
|
|
std_selected_actions[..., :3] * (1.0 - momentum_std) + std[..., :3] * momentum_std,
|
|
std_selected_actions[..., -1:] * (1.0 - momentum_std_gripper) + std[..., -1:] * momentum_std_gripper,
|
|
],
|
|
dim=-1,
|
|
)
|
|
|
|
logger.info(f"new mean: {mean.sum(dim=0)} {std.sum(dim=0)}")
|
|
|
|
new_action = torch.cat(
|
|
[
|
|
mean[..., :3],
|
|
torch.zeros((rollout, 3), device=mean.device),
|
|
round_small_elements(mean[..., -1:], 0.25),
|
|
],
|
|
dim=-1,
|
|
)[None, :]
|
|
|
|
return new_action
|
|
|
|
|
|
def compute_new_pose(pose, action):
|
|
"""
|
|
:param pose: [B, T=1, 7]
|
|
:param action: [B, T=1, 7]
|
|
:returns: [B, T=1, 7]
|
|
"""
|
|
device, dtype = pose.device, pose.dtype
|
|
pose = pose[:, 0].cpu().numpy()
|
|
action = action[:, 0].cpu().numpy()
|
|
# -- compute delta xyz
|
|
new_xyz = pose[:, :3] + action[:, :3]
|
|
# -- compute delta theta
|
|
thetas = pose[:, 3:6]
|
|
delta_thetas = action[:, 3:6]
|
|
matrices = [Rotation.from_euler("xyz", theta, degrees=False).as_matrix() for theta in thetas]
|
|
delta_matrices = [Rotation.from_euler("xyz", theta, degrees=False).as_matrix() for theta in delta_thetas]
|
|
angle_diff = [delta_matrices[t] @ matrices[t] for t in range(len(matrices))]
|
|
angle_diff = [Rotation.from_matrix(mat).as_euler("xyz", degrees=False) for mat in angle_diff]
|
|
new_angle = np.stack([d for d in angle_diff], axis=0) # [B, 7]
|
|
# -- compute delta gripper
|
|
new_closedness = pose[:, -1:] + action[:, -1:]
|
|
new_closedness = np.clip(new_closedness, 0, 1)
|
|
# -- new pose
|
|
new_pose = np.concatenate([new_xyz, new_angle, new_closedness], axis=-1)
|
|
return torch.from_numpy(new_pose).to(device).to(dtype)[:, None]
|
|
|
|
|
|
def poses_to_diff(start, end):
|
|
"""
|
|
:param start: [7]
|
|
:param end: [7]
|
|
"""
|
|
try:
|
|
start = start.numpy()
|
|
end = end.numpy()
|
|
except Exception:
|
|
pass
|
|
|
|
# --
|
|
|
|
s_xyz = start[:3]
|
|
e_xyz = end[:3]
|
|
xyz_diff = e_xyz - s_xyz
|
|
|
|
# --
|
|
|
|
s_thetas = start[3:6]
|
|
e_thetas = end[3:6]
|
|
s_rotation = Rotation.from_euler("xyz", s_thetas, degrees=False).as_matrix()
|
|
e_rotation = Rotation.from_euler("xyz", e_thetas, degrees=False).as_matrix()
|
|
rotation_diff = e_rotation @ s_rotation.T
|
|
theta_diff = Rotation.from_matrix(rotation_diff).as_euler("xyz", degrees=False)
|
|
|
|
# --
|
|
|
|
s_gripper = start[-1:]
|
|
e_gripper = end[-1:]
|
|
gripper_diff = e_gripper - s_gripper
|
|
|
|
action = np.concatenate([xyz_diff, theta_diff, gripper_diff], axis=0)
|
|
return torch.from_numpy(action)
|