def execute_grasp(self, grasp): T_task_grasp = grasp.pose T_base_grasp = self.T_base_task * T_task_grasp T_grasp_pregrasp = Transform(Rotation.identity(), [0.0, 0.0, -0.05]) T_grasp_retreat = Transform(Rotation.identity(), [0.0, 0.0, -0.05]) T_base_pregrasp = T_base_grasp * T_grasp_pregrasp T_base_retreat = T_base_grasp * T_grasp_retreat self.pc.goto_pose(T_base_pregrasp * self.T_tcp_tool0, velocity_scaling=0.2) self.approach_grasp(T_base_grasp) if self.robot_error: return False self.pc.grasp(width=0.0, force=20.0) if self.robot_error: return False self.pc.goto_pose(T_base_retreat * self.T_tcp_tool0) # lift hand T_retreat_lift_base = Transform(Rotation.identity(), [0.0, 0.0, 0.1]) T_base_lift = T_retreat_lift_base * T_base_retreat self.pc.goto_pose(T_base_lift * self.T_tcp_tool0) if self.gripper_width > 0.004: return True else: return False
def apply_transform(voxel_grid, orientation, position): angle = np.pi / 2.0 * np.random.choice(4) R_augment = Rotation.from_rotvec(np.r_[0.0, 0.0, angle]) z_offset = np.random.uniform(6, 34) - position[2] t_augment = np.r_[0.0, 0.0, z_offset] T_augment = Transform(R_augment, t_augment) T_center = Transform(Rotation.identity(), np.r_[20.0, 20.0, 20.0]) T = T_center * T_augment * T_center.inverse() # transform voxel grid T_inv = T.inverse() matrix, offset = T_inv.rotation.as_matrix(), T_inv.translation voxel_grid[0] = ndimage.affine_transform(voxel_grid[0], matrix, offset, order=0) # transform grasp pose position = T.transform_point(position) orientation = T.rotation * orientation return voxel_grid, orientation, position
def acquire_tsdf(self, n, N=None): """Render synthetic depth images from n viewpoints and integrate into a TSDF. If N is None, the n viewpoints are equally distributed on circular trajectory. If N is given, the first n viewpoints on a circular trajectory consisting of N points are rendered. """ tsdf = TSDFVolume(self.size, 40) high_res_tsdf = TSDFVolume(self.size, 120) origin = Transform(Rotation.identity(), np.r_[self.size / 2, self.size / 2, 0]) r = 2.0 * self.size theta = np.pi / 6.0 N = N if N else n phi_list = 2.0 * np.pi * np.arange(n) / N extrinsics = [ camera_on_sphere(origin, r, theta, phi) for phi in phi_list ] timing = 0.0 for extrinsic in extrinsics: depth_img = self.camera.render(extrinsic)[1] tic = time.time() tsdf.integrate(depth_img, self.camera.intrinsic, extrinsic) timing += time.time() - tic high_res_tsdf.integrate(depth_img, self.camera.intrinsic, extrinsic) return tsdf, high_res_tsdf.get_cloud(), timing
def define_workspace(self): z_offset = -0.06 t_tag_task = np.r_[[-0.5 * self.size, -0.5 * self.size, z_offset]] T_tag_task = Transform(Rotation.identity(), t_tag_task) self.T_base_task = T_base_tag * T_tag_task self.tf_tree.broadcast_static(self.T_base_task, self.base_frame_id, "task") rospy.sleep(1.0) # wait for the TF to be broadcasted
def __init__(self, world): self.world = world self.urdf_path = Path("data/urdfs/panda/hand.urdf") self.max_opening_width = 0.08 self.finger_depth = 0.05 self.T_body_tcp = Transform(Rotation.identity(), [0.0, 0.0, 0.022]) self.T_tcp_body = self.T_body_tcp.inverse()
def execute_grasp(self, grasp, remove=True, allow_contact=False): T_world_grasp = grasp.pose T_grasp_pregrasp = Transform(Rotation.identity(), [0.0, 0.0, -0.05]) T_world_pregrasp = T_world_grasp * T_grasp_pregrasp approach = T_world_grasp.rotation.as_matrix()[:, 2] angle = np.arccos(np.dot(approach, np.r_[0.0, 0.0, -1.0])) if angle > np.pi / 3.0: # side grasp, lift the object after establishing a grasp T_grasp_pregrasp_world = Transform(Rotation.identity(), [0.0, 0.0, 0.1]) T_world_retreat = T_grasp_pregrasp_world * T_world_grasp else: T_grasp_retreat = Transform(Rotation.identity(), [0.0, 0.0, -0.1]) T_world_retreat = T_world_grasp * T_grasp_retreat self.gripper.reset(T_world_pregrasp) if self.gripper.detect_contact(): result = Label.FAILURE, self.gripper.max_opening_width else: self.gripper.move_tcp_xyz(T_world_grasp, abort_on_contact=True) if self.gripper.detect_contact() and not allow_contact: result = Label.FAILURE, self.gripper.max_opening_width else: self.gripper.move(0.0) self.gripper.move_tcp_xyz(T_world_retreat, abort_on_contact=False) if self.check_success(self.gripper): result = Label.SUCCESS, self.gripper.read() if remove: contacts = self.world.get_contacts(self.gripper.body) self.world.remove_body(contacts[0].bodyB) else: result = Label.FAILURE, self.gripper.max_opening_width self.world.remove_body(self.gripper.body) if remove: self.remove_and_wait() return result
def place_table(self, height): urdf = self.urdf_root / "setup" / "plane.urdf" pose = Transform(Rotation.identity(), [0.15, 0.15, height]) self.world.load_urdf(urdf, pose, scale=0.6) # define valid volume for sampling grasps lx, ux = 0.02, self.size - 0.02 ly, uy = 0.02, self.size - 0.02 lz, uz = height + 0.005, self.size self.lower = np.r_[lx, ly, lz] self.upper = np.r_[ux, uy, uz]
def draw_grasp(grasp, score, finger_depth): radius = 0.1 * finger_depth w, d = grasp.width, finger_depth color = cmap(float(score)) markers = [] # left finger pose = grasp.pose * Transform(Rotation.identity(), [0.0, -w / 2, d / 2]) scale = [radius, radius, d] msg = _create_marker_msg(Marker.CYLINDER, "task", pose, scale, color) msg.id = 0 markers.append(msg) # right finger pose = grasp.pose * Transform(Rotation.identity(), [0.0, w / 2, d / 2]) scale = [radius, radius, d] msg = _create_marker_msg(Marker.CYLINDER, "task", pose, scale, color) msg.id = 1 markers.append(msg) # wrist pose = grasp.pose * Transform(Rotation.identity(), [0.0, 0.0, -d / 4]) scale = [radius, radius, d / 2] msg = _create_marker_msg(Marker.CYLINDER, "task", pose, scale, color) msg.id = 2 markers.append(msg) # palm pose = grasp.pose * Transform( Rotation.from_rotvec(np.pi / 2 * np.r_[1.0, 0.0, 0.0]), [0.0, 0.0, 0.0]) scale = [radius, radius, w] msg = _create_marker_msg(Marker.CYLINDER, "task", pose, scale, color) msg.id = 3 markers.append(msg) pubs["grasp"].publish(MarkerArray(markers=markers))
def render_images(sim, n): height, width = sim.camera.intrinsic.height, sim.camera.intrinsic.width origin = Transform(Rotation.identity(), np.r_[sim.size / 2, sim.size / 2, 0.0]) extrinsics = np.empty((n, 7), np.float32) depth_imgs = np.empty((n, height, width), np.float32) for i in range(n): r = np.random.uniform(1.6, 2.4) * sim.size theta = np.random.uniform(0.0, np.pi / 4.0) phi = np.random.uniform(0.0, 2.0 * np.pi) extrinsic = camera_on_sphere(origin, r, theta, phi) depth_img = sim.camera.render(extrinsic)[1] extrinsics[i] = extrinsic.to_list() depth_imgs[i] = depth_img return depth_imgs, extrinsics
def generate_pile_scene(self, object_count, table_height): # place box urdf = self.urdf_root / "setup" / "box.urdf" pose = Transform(Rotation.identity(), np.r_[0.02, 0.02, table_height]) box = self.world.load_urdf(urdf, pose, scale=1.3) # drop objects urdfs = self.rng.choice(self.object_urdfs, size=object_count) for urdf in urdfs: rotation = Rotation.random(random_state=self.rng) xy = self.rng.uniform(1.0 / 3.0 * self.size, 2.0 / 3.0 * self.size, 2) pose = Transform(rotation, np.r_[xy, table_height + 0.2]) scale = self.rng.uniform(0.8, 1.0) self.world.load_urdf(urdf, pose, scale=self.global_scaling * scale) self.wait_for_objects_to_rest(timeout=1.0) # remove box self.world.remove_body(box) self.remove_and_wait()
import rospy import sensor_msgs.msg from vgn import vis from vgn.baselines import GPD from vgn.experiments.clutter_removal import Logger, State from vgn.detection import VGN from vgn.perception import * from vgn.utils import ros_utils from vgn.utils.transform import Rotation, Transform from vgn.utils.panda_control import PandaCommander vis.set_size(0.3) # tag lies on the table in the center of the workspace T_base_tag = Transform(Rotation.identity(), [0.42, 0.02, 0.21]) round_id = 0 class PandaGraspController(object): def __init__(self, args): self.robot_error = False self.base_frame_id = rospy.get_param("~base_frame_id") self.tool0_frame_id = rospy.get_param("~tool0_frame_id") self.T_tool0_tcp = Transform.from_dict(rospy.get_param("~T_tool0_tcp")) # TODO self.T_tcp_tool0 = self.T_tool0_tcp.inverse() self.finger_depth = rospy.get_param("~finger_depth") self.size = 6.0 * self.finger_depth self.scan_joints = rospy.get_param("~scan_joints")