diff --git a/serl_robot_infra/franka_env/envs/relative_env.py b/serl_robot_infra/franka_env/envs/relative_env.py index 4b16b3b9..f8dfdda7 100644 --- a/serl_robot_infra/franka_env/envs/relative_env.py +++ b/serl_robot_infra/franka_env/envs/relative_env.py @@ -107,7 +107,7 @@ def construct_adjoint_matrix(self, tcp_pose): adjoint_matrix = np.zeros((6, 6)) adjoint_matrix[:3, :3] = rotation adjoint_matrix[3:, 3:] = rotation - adjoint_matrix[:3, 3:] = skew_matrix @ rotation + adjoint_matrix[3:, :3] = skew_matrix @ rotation return adjoint_matrix def construct_homogeneous_matrix(self, tcp_pose):