[MISC] Picking boxes and Panda links with a MouseSpring #1443
Conversation
…n Links/Geoms instead of Entities; split OBB from AABB
| @@ -79,3 +72,33 @@ def from_center_and_size(cls, center: Vec3, size: Vec3) -> 'AABB': | |||
| max = center + 0.5 * size | |||
| bounds = np.stack((min.v, max.v)) | |||
| return cls(bounds) | |||
|
|
|||
|
|
|||
| class OBB(AABB): | |||
There was a problem hiding this comment.
Does not make sense. You should probably have some BaseBoundingBox abstraction.
| self.pose = pose | ||
|
|
||
| def raycast(self, ray: Ray) -> RayHit: | ||
| inv_pose = self.pose.get_inverse() |
There was a problem hiding this comment.
Do not compute inverse explicit. This is slow. You should rather implement inv_transform_point, inv_transform_direction
There was a problem hiding this comment.
actually, I think, pose.get_inverse has the same cost as transform_point -- one rotation of vector by quaternion being the main cost.
But yes, with the extra inv_pose, it's 3 such function calls instead of 2. Fixing
| def from_center_and_size(cls, size: Vec3, pose: Pose) -> 'OBB': | ||
| min = - 0.5 * size | ||
| max = + 0.5 * size | ||
| bounds = np.stack((min.v, max.v)) |
There was a problem hiding this comment.
Always specify on which access you stack for readability.
|
|
||
| def apply_force(self, control_point: Vec3, delta_time: float): | ||
| _ensure_torch_imported() | ||
|
|
||
| # note when threaded: apply_force is called before attach! |
There was a problem hiding this comment.
Could you confirm if it is fixed and clean these notes (which does NOT mean removing them entirely!)
There was a problem hiding this comment.
- I don't think I can confirm with threaded nondeterministic execution (?)
- the early return here was the first work around, and it fixed the crash
- later I added locks, which might also fix the cause of the earlier crash
- I have not tested removal of the original work around, but it's nondeterministic, so that wouldn't confirm anything, so I'm just leaving it here, with comments.
Would you like me to add the above notes to the source, or do you want me to write something else specific instead?
| link: RigidLink = self.held_geom.link | ||
| link_pos: Vec3 = Vec3.from_tensor(link.get_pos()) | ||
| link: RigidLink = self.held_link | ||
| assert link |
There was a problem hiding this comment.
This makes no sense if the type of link is RigidLink. Either the assert should be removed or typing should be replaced by RigidLink | None
| delta_3d_pos: Vec3 = new_mouse_3d_pos - self.prev_mouse_3d_pos | ||
| self.prev_mouse_3d_pos = new_mouse_3d_pos | ||
|
|
||
| use_force: bool = True |
There was a problem hiding this comment.
same. But you can keep them if you like them x)
| closest_hit = RayHit.no_hit() | ||
| for link in entity.links: | ||
| # how to check if the link is dynamic ?? | ||
| if 0.0 < link.get_mass(): |
There was a problem hiding this comment.
if link.get_mass() > 0.0: is more natural for me.
| else: | ||
| closest_hit = RayHit.no_hit() | ||
| for link in entity.links: | ||
| # how to check if the link is dynamic ?? |
There was a problem hiding this comment.
What does this comment means? Whether it can move ? If so, you need to change for its parent index. If it is "-1", then it is fixed wrt world (95% sure). Another option is to check the value of is_fixed, which is arguably much clearer.
| return Pose.from_geom(entity.geoms[0]) | ||
| def _get_geom_placeholder_obb(self, geom: 'RigidGeom') -> OBB: | ||
| pose = Pose.from_geom(geom) | ||
| size = 0.125 * Vec3.one() |
There was a problem hiding this comment.
Could you add def full(fill_value): ? That would be more convenient and efficient.
| quat = Quat.from_tensor(geom.get_quat()) | ||
| aabb = AABB.from_center_and_size(pos, size) | ||
| if obb: | ||
| aabb: AABB = AABB.from_center_and_size(obb.pose.pos, obb.extents) | ||
| aabb.expand(0.01) |
There was a problem hiding this comment.
Use keyword arguments when passing constants to methods. This makes the code more self-explanatory.
Description
Added:
poseproperty to RigidLinks, but that's just an ideaMotivation and Context
Allow interaction with the scene.
How Has This Been / Can This Be Tested?
Manual testing only ST/MT
Screenshots (if appropriate):
721.Interaction.with.Boxes.and.Panda.mp4
Checklist:
Submitting Code Changessection of CONTRIBUTING document.