Faulty logic here
|
bbox_quat_in_world = link.get_position_orientation()[1] |
|
bbox_extent_in_link_frame = th.tensor( |
|
target_obj.native_link_bboxes[link_name]["collision"]["axis_aligned"]["extent"] |
|
) |
|
bbox_wrt_origin = T.relative_pose_transform( |
|
bbox_center_in_world, bbox_quat_in_world, *link.get_position_orientation() |
|
) |
|
origin_wrt_bbox = T.invert_pose_transform(*bbox_wrt_origin) |
|
|
|
joint_orientation = lazy.omni.isaac.core.utils.rotations.gf_quat_to_np_array( |
|
relevant_joint.get_attribute("physics:localRot0") |
|
)[[1, 2, 3, 0]] |
|
joint_axis = T.quat_apply(joint_orientation, th.tensor([1, 0, 0], dtype=th.float32)) |
|
joint_axis /= th.norm(joint_axis) |
|
origin_towards_bbox = th.tensor(bbox_wrt_origin[0]) |
|
open_direction = th.linalg.cross(joint_axis, origin_towards_bbox) |
|
open_direction /= th.norm(open_direction) |
|
lateral_axis = th.linalg.cross(open_direction, joint_axis) |