follow adjoint matrix notation in modern robotics

rail-berkeley · Jan 14, 2024 · b46ec5d · b46ec5d
1 parent ad15b5c
commit b46ec5d
Showing 1 changed file with 1 addition and 1 deletion.
diff --git 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):