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

Create separate collision objects

Browse Source
This commit is contained in:
Michel Breyer 2021-10-12 17:16:41 +02:00
parent b91ced08d9
commit 5815a2a66c
2 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/active_grasp/controller.py
View File

@ -5,6 +5,7 @@ from geometry_msgs.msg import Twist
import numpy as np import numpy as np
import rospy import rospy
from sensor_msgs.msg import Image from sensor_msgs.msg import Image
import trimesh
from .bbox import from_bbox_msg from .bbox import from_bbox_msg
from .timer import Timer from .timer import Timer
@ -12,7 +13,7 @@ from active_grasp.srv import Reset, ResetRequest
from robot_helpers.ros import tf from robot_helpers.ros import tf
from robot_helpers.ros.conversions import * from robot_helpers.ros.conversions import *
from robot_helpers.ros.panda import PandaArmClient, PandaGripperClient from robot_helpers.ros.panda import PandaArmClient, PandaGripperClient
from robot_helpers.ros.moveit import MoveItClient, create_collision_object_from_meshes from robot_helpers.ros.moveit import MoveItClient, create_collision_object_from_mesh
from robot_helpers.spatial import Rotation, Transform from robot_helpers.spatial import Rotation, Transform
from vgn.utils import look_at, cartesian_to_spherical, spherical_to_cartesian from vgn.utils import look_at, cartesian_to_spherical, spherical_to_cartesian
@ -28,6 +29,7 @@ class GraspController:
def load_parameters(self): def load_parameters(self):
self.base_frame = rospy.get_param("~base_frame_id") self.base_frame = rospy.get_param("~base_frame_id")
self.task_frame = self.policy.task_frame
self.T_grasp_ee = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv() self.T_grasp_ee = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv()
self.cam_frame = rospy.get_param("~camera/frame_id") self.cam_frame = rospy.get_param("~camera/frame_id")
self.depth_topic = rospy.get_param("~camera/depth_topic") self.depth_topic = rospy.get_param("~camera/depth_topic")
@ -50,8 +52,6 @@ class GraspController:
def init_moveit(self): def init_moveit(self):
self.moveit = MoveItClient("panda_arm") self.moveit = MoveItClient("panda_arm")
rospy.sleep(1.0) # Wait for connections to be established. rospy.sleep(1.0) # Wait for connections to be established.
# msg = to_pose_stamped_msg(Transform.t([0.4, 0, 0.4]), self.base_frame)
# self.moveit.scene.add_box("table", msg, size=(0.5, 0.5, 0.02))
def switch_to_cartesian_velocity_control(self): def switch_to_cartesian_velocity_control(self):
req = SwitchControllerRequest() req = SwitchControllerRequest()
@ -141,14 +141,12 @@ class GraspController:
plane_model, inliers = cloud.segment_plane(0.01, 3, 1000) plane_model, inliers = cloud.segment_plane(0.01, 3, 1000)
cloud = cloud.select_by_index(inliers, invert=True) cloud = cloud.select_by_index(inliers, invert=True)
labels = np.array(cloud.cluster_dbscan(eps=0.02, min_points=10)) labels = np.array(cloud.cluster_dbscan(eps=0.02, min_points=10))
hulls = []
for l in range(labels.max() + 1): for l in range(labels.max() + 1):
segment = cloud.select_by_index(np.flatnonzero(labels == l)) segment = cloud.select_by_index(np.flatnonzero(labels == l))
hulls.append(segment.compute_convex_hull()[0]) hull = trimesh.points.PointCloud(np.asarray(segment.points)).convex_hull
collision_object = create_collision_object_from_meshes( name, frame, pose = f"object_{l}", self.task_frame, Transform.identity()
"objects", self.policy.task_frame, Transform.identity(), hulls co = create_collision_object_from_mesh(name, frame, pose, hull)
) self.moveit.scene.add_object(co)
self.moveit.scene.add_object(collision_object)
rospy.sleep(1.0) rospy.sleep(1.0)
def execute_grasp(self, grasp): def execute_grasp(self, grasp):

2
src/active_grasp/policy.py
View File

@ -63,7 +63,7 @@ class Policy:
self.T_task_base = self.T_base_task.inv() self.T_task_base = self.T_base_task.inv()
tf.broadcast(self.T_base_task, self.base_frame, self.task_frame) tf.broadcast(self.T_base_task, self.base_frame, self.task_frame)
rospy.sleep(1.0) # Wait for tf tree to be updated rospy.sleep(1.0) # Wait for tf tree to be updated
self.vis.workspace(self.task_frame, 0.3) self.vis.roi(self.task_frame, 0.3)
def update(self, img, x, q): def update(self, img, x, q):
raise NotImplementedError raise NotImplementedError