hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Clear the whole planning scene before approaching

Browse Source
This commit is contained in:
Michel Breyer 2021-11-09 17:10:39 +01:00
parent 1f6892bc2c
commit 2b8c66b318
1 changed files with 2 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/active_grasp/controller.py
View File

@ -141,10 +141,11 @@ class GraspController:
T_base_approach = T_base_grasp * Transform.t([0, 0, -0.06]) * self.T_grasp_ee T_base_approach = T_base_grasp * Transform.t([0, 0, -0.06]) * self.T_grasp_ee
success, plan = self.moveit.plan(T_base_approach, 0.2, 0.2) success, plan = self.moveit.plan(T_base_approach, 0.2, 0.2)
if success: if success:
self.remove_target_collision_object(grasp) self.moveit.scene.clear()
self.moveit.execute(plan) self.moveit.execute(plan)
rospy.sleep(0.5) # Wait for the planning scene to be updated rospy.sleep(0.5) # Wait for the planning scene to be updated
self.moveit.gotoL(T_base_grasp * self.T_grasp_ee) self.moveit.gotoL(T_base_grasp * self.T_grasp_ee)
rospy.sleep(0.5)
self.gripper.grasp() self.gripper.grasp()
T_base_retreat = Transform.t([0, 0, 0.05]) * T_base_grasp * self.T_grasp_ee T_base_retreat = Transform.t([0, 0, 0.05]) * T_base_grasp * self.T_grasp_ee
self.moveit.gotoL(T_base_retreat) self.moveit.gotoL(T_base_retreat)
@ -185,15 +186,6 @@ class GraspController:
co = create_collision_object_from_mesh(name, frame, pose, mesh) co = create_collision_object_from_mesh(name, frame, pose, mesh)
self.moveit.scene.add_object(co) self.moveit.scene.add_object(co)
def remove_target_collision_object(self, grasp):
label, min_dist = None, np.inf
for i, hull in enumerate(self.hulls):
tip = grasp.pose.apply([0.0, 0.0, 0.05])
dist = trimesh.proximity.closest_point_naive(hull, np.array([tip]))[1]
if dist < min_dist:
label, min_dist = i, dist
self.moveit.scene.remove_world_object(f"object_{label}")
def postprocess(self, T_base_grasp): def postprocess(self, T_base_grasp):
rot = T_base_grasp.rotation rot = T_base_grasp.rotation
if rot.as_matrix()[:, 0][0] < 0: # Ensure that the camera is pointing forward if rot.as_matrix()[:, 0][0] < 0: # Ensure that the camera is pointing forward