From 38536fa6b85ec45a646a937ef16c44d25955f6f3 Mon Sep 17 00:00:00 2001 From: ChaoW <724183484zz@gmail.com> Date: Tue, 13 May 2025 13:45:36 +0800 Subject: [PATCH] chore: Use slerp to interpolate quaternion. --- serl_robot_infra/franka_env/envs/franka_env.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/serl_robot_infra/franka_env/envs/franka_env.py b/serl_robot_infra/franka_env/envs/franka_env.py index bd665460..befaaf7e 100644 --- a/serl_robot_infra/franka_env/envs/franka_env.py +++ b/serl_robot_infra/franka_env/envs/franka_env.py @@ -5,6 +5,7 @@ import cv2 import copy from scipy.spatial.transform import Rotation +from scipy.spatial.transform import Slerp import time import requests import queue @@ -289,8 +290,16 @@ def interpolate_move(self, goal: np.ndarray, timeout: float): goal = np.concatenate([goal[:3], euler_2_quat(goal[3:])]) steps = int(timeout * self.hz) self._update_currpos() + key_times = [0, 1] + key_rots = Rotation.from_quat([self.currpos[3:], goal[3:]]) + slerp = Slerp(key_times, key_rots) + steps = int(timeout * self.hz) + interpolate_times = np.linspace(0, 1, steps) + path_slerp = slerp(interpolate_times) + path_slerp = [rot.as_quat() for rot in path_slerp] path = np.linspace(self.currpos, goal, steps) - for p in path: + for idx, p in enumerate(path): + p[3:] = path_slerp[idx] self._send_pos_command(p) time.sleep(1 / self.hz) self.nextpos = p @@ -489,4 +498,4 @@ def close(self): if self.display_image: self.img_queue.put(None) cv2.destroyAllWindows() - self.displayer.join() \ No newline at end of file + self.displayer.join()