From d93a307dc2e721ca0d7ad7c6652a72153416eb7a Mon Sep 17 00:00:00 2001 From: James Tigue Date: Tue, 8 Oct 2024 16:11:56 -0400 Subject: [PATCH] change orientation of the com properties to principle axes of inertia --- .../assets/articulation/articulation_data.py | 26 +++++++++---------- .../assets/rigid_object/rigid_object_data.py | 10 ++++--- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py index 630e0db40a..37a1ca4442 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py @@ -202,13 +202,7 @@ def update(self, dt: float): """Joint stiffness provided to simulation. Shape is (num_instances, num_joints).""" joint_damping: torch.Tensor = None - """Joint damping provided to simulation. Shape is (num_instances, num_joints).""" - - joint_armature: torch.Tensor = None - """Joint armature provided to simulation. Shape is (num_instances, num_joints).""" - - joint_friction: torch.Tensor = None - """Joint friction provided to simulation. Shape is (num_instances, num_joints).""" + """Joint damping provided to simulation. Shape is (num_instanbody_state_wnstances, num_joints).""" joint_limits: torch.Tensor = None """Joint limits provided to simulation. Shape is (num_instances, num_joints, 2).""" @@ -293,9 +287,12 @@ def root_com_state_w(self): orientation of the principle inertia. """ state = self.root_state_w.clone() - quat = state[:, 3:7] - # adjust position to center of mass - state[:, :3] += math_utils.quat_rotate(quat, self.com_pos_b[:, 0, :]) + # adjust pose to center of mass + pos, quat = math_utils.combine_frame_transforms(state[:,:3], + state[:,3:7], + self.com_pos_b[:,0,:], + self.com_quat_b[:,0,:]) + state[:, :7] += torch.cat((pos,quat),dim=-1) return state @property @@ -341,9 +338,12 @@ def body_com_state_w(self): principle inertia. """ state = self.body_state_w.clone() - quat = state[..., 3:7] - # adjust position to center of mass - state[..., :3] -= math_utils.quat_rotate(quat, self.com_pos_b) + # adjust pose to center of mass + pos, quat = math_utils.combine_frame_transforms(state[:,:3], + state[:,3:7], + self.com_pos_b, + self.com_quat_b) + state[..., :7] += torch.cat((pos,quat),dim=-1) return state @property diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py index 8d85672763..41af0f3227 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py @@ -135,13 +135,15 @@ def root_com_state_w(self): """Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13). The position, quaternion, and linear/angular velocity are of the rigid body's center of mass frame - relative to the world. Center of mass frame is assumed to be the same orientation as the link rather than the - orientation of the principle inertia. + relative to the world. Center of mass frame is the orientation principle axes of inertia. """ state = self.root_state_w.clone() - quat = state[:, 3:7] # adjust position to center of mass - state[:, :3] += math_utils.quat_rotate(quat, self.com_pos_b) + pos, quat = math_utils.combine_frame_transforms(state[:,:3], + state[:,3:7], + self.com_pos_b, + self.com_quat_b) + state[:, :7] += torch.cat((pos,quat),dim=-1) return state @property