def _get_packaged_pose(self, mesh): # Make copy of argument mesh = mesh.copy() # Compute stable pose with least z-axis height tfs, _ = mesh.compute_stable_poses() min_z, min_tf = np.infty, None for tf in tfs: m = mesh.copy().apply_transform(tf) z_ext = m.extents[2] if z_ext < min_z: min_z = z_ext min_tf = tf # Put mesh in that stable pose mesh.apply_transform(min_tf) # Rotate the mesh about that stable pose, pick one that gives largest y extent theta = 0 dtheta = 0.1 min_rot = np.eye(4) min_x_ext = mesh.extents[0] while theta < np.pi: rot = np.eye(4) rot[:3, :3] = RigidTransform.z_axis_rotation(theta) m = mesh.copy() m.apply_transform(rot) x_ext = m.extents[0] if x_ext < min_x_ext: min_x_ext = x_ext min_rot = rot theta += dtheta return min_rot.dot(min_tf)
def __eq__(self, other): """ Check equivalence by rotation about the z axis """ if not isinstance(other, StablePose): raise ValueError('Can only compare stable pose objects') R0 = self.r R1 = other.r dR = R1.T.dot(R0) theta = 0 while theta < 2 * np.pi: Rz = RigidTransform.z_axis_rotation(theta) dR = R1.T.dot(Rz).dot(R0) if np.linalg.norm(dR - np.eye(3)) < 1e-5: return True theta += d_theta return False
def rotate_callback(viewer, key, rot, stf): rot.rotation = RigidTransform.z_axis_rotation(np.pi / 2.0).dot( rot.rotation) vis.get_object(key).T_obj_world = rot.dot(stf)
def sample(self): """ Samples a state from the space Returns ------- :obj:`HeapState` state of the object pile """ # Start physics engine self._physics_engine.start() # setup workspace workspace_obj_states = [] workspace_objs = self._config['workspace']['objects'] for work_key, work_config in list(workspace_objs.items()): # make paths absolute mesh_filename = work_config['mesh_filename'] pose_filename = work_config['pose_filename'] if not os.path.isabs(mesh_filename): mesh_filename = os.path.join( os.path.dirname(os.path.realpath(__file__)), '..', mesh_filename) if not os.path.isabs(pose_filename): pose_filename = os.path.join( os.path.dirname(os.path.realpath(__file__)), '..', pose_filename) # load mesh mesh = trimesh.load_mesh(mesh_filename) mesh.density = self.obj_density pose = RigidTransform.load(pose_filename) workspace_obj = ObjectState(work_key, mesh, pose) self._physics_engine.add(workspace_obj, static=True) workspace_obj_states.append(workspace_obj) # sample state train = True if np.random.rand() > self._train_pct: train = False sample_keys = self.test_keys self._logger.info('Sampling from test') else: sample_keys = self.train_keys self._logger.info('Sampling from train') total_num_objs = len(sample_keys) # sample object ids num_objs = min(self.num_objs_rv.rvs(size=1)[0], total_num_objs - 1) + 1 num_objs = min(num_objs, self.max_objs) num_objs = max(num_objs, self.min_objs) obj_inds = np.random.permutation(np.arange(total_num_objs)) # log self._logger.info('Sampling %d objects' % (num_objs)) # sample pile center heap_center = self.heap_center_space.sample() t_heap_world = np.array([heap_center[0], heap_center[1], 0]) self._logger.debug('Sampled pile location: %.3f %.3f' % (t_heap_world[0], t_heap_world[1])) # sample object, center of mass, pose objs_in_heap = [] total_drops = 0 while total_drops < total_num_objs and len(objs_in_heap) < num_objs: obj_key = sample_keys[total_drops] obj_mesh = trimesh.load_mesh(self.mesh_filenames[obj_key]) obj_mesh.density = self.obj_density obj = ObjectState(obj_key, obj_mesh) _, radius = trimesh.nsphere.minimum_nsphere(obj.mesh) if 2 * radius > self.max_obj_diam: self._logger.warning('Obj too big, skipping .....') total_drops += 1 continue # sample center of mass delta_com = self.delta_com_rv.rvs(size=1) center_of_mass = obj.mesh.center_mass + delta_com obj.mesh.center_mass = center_of_mass # sample obj drop pose obj_orientation = self.obj_orientation_space.sample() az = obj_orientation[0] elev = obj_orientation[1] T_obj_table = RigidTransform.sph_coords_to_pose(az, elev).as_frames( 'obj', 'world') # sample object planar pose obj_planar_pose = self.obj_planar_pose_space.sample() theta = obj_planar_pose[2] R_table_world = RigidTransform.z_axis_rotation(theta) R_obj_drop_world = R_table_world.dot(T_obj_table.rotation) t_obj_drop_heap = np.array( [obj_planar_pose[0], obj_planar_pose[1], self.drop_height]) t_obj_drop_world = t_obj_drop_heap + t_heap_world obj.pose = RigidTransform(rotation=R_obj_drop_world, translation=t_obj_drop_world, from_frame='obj', to_frame='world') self._physics_engine.add(obj) try: v, w = self._physics_engine.get_velocity(obj.key) except: self._logger.warning( 'Could not get base velocity for object %s. Skipping ...' % (obj_key)) self._physics_engine.remove(obj.key) total_drops += 1 continue objs_in_heap.append(obj) # setup until approx zero velocity wait = time.time() objects_in_motion = True num_steps = 0 while objects_in_motion and num_steps < self.max_settle_steps: # step simulation self._physics_engine.step() # check velocities max_mag_v = 0 max_mag_w = 0 objs_to_remove = set() for o in objs_in_heap: try: v, w = self._physics_engine.get_velocity(o.key) except: self._logger.warning( 'Could not get base velocity for object %s. Skipping ...' % (o.key)) objs_to_remove.add(o) continue mag_v = np.linalg.norm(v) mag_w = np.linalg.norm(w) if mag_v > max_mag_v: max_mag_v = mag_v if mag_w > max_mag_w: max_mag_w = mag_w # Remove invalid objects for o in objs_to_remove: self._physics_engine.remove(o.key) objs_in_heap.remove(o) # check objs in motion if max_mag_v < self.mag_v_thresh and max_mag_w < self.mag_w_thresh: objects_in_motion = False num_steps += 1 # read out object poses objs_to_remove = set() for o in objs_in_heap: obj_pose = self._physics_engine.get_pose(o.key) o.pose = obj_pose.copy() # remove the object if its outside of the workspace if not self.in_workspace(obj_pose): self._logger.warning( 'Object {} fell out of the workspace!'.format(o.key)) objs_to_remove.add(o) # remove invalid objects for o in objs_to_remove: self._physics_engine.remove(o.key) objs_in_heap.remove(o) total_drops += 1 self._logger.debug('Waiting for zero velocity took %.3f sec' % (time.time() - wait)) # Stop physics engine self._physics_engine.stop() # add metadata for heap state and return it metadata = {'split': TRAIN_ID} if not train: metadata['split'] = TEST_ID return HeapState(workspace_obj_states, objs_in_heap, metadata=metadata)
def register(sensor, config): """ Registers a camera to a chessboard. Parameters ---------- sensor : :obj:`perception.RgbdSensor` the sensor to register config : :obj:`autolab_core.YamlConfig` or :obj:`dict` configuration file for registration Returns ------- :obj:`ChessboardRegistrationResult` the result of registration Notes ----- The config must have the parameters specified in the Other Parameters section. Other Parameters ---------------- num_transform_avg : int the number of independent registrations to average together num_images : int the number of images to read for each independent registration corners_x : int the number of chessboard corners in the x-direction corners_y : int the number of chessboard corners in the y-direction color_image_rescale_factor : float amount to rescale the color image for detection (numbers around 4-8 are useful) vis : bool whether or not to visualize the registration """ # read config num_transform_avg = config['num_transform_avg'] num_images = config['num_images'] sx = config['corners_x'] sy = config['corners_y'] point_order = config['point_order'] color_image_rescale_factor = config['color_image_rescale_factor'] flip_normal = config['flip_normal'] y_points_left = False if 'y_points_left' in config.keys() and sx == sy: y_points_left = config['y_points_left'] num_images = 1 vis = config['vis'] # read params from sensor logging.info('Registering camera %s' %(sensor.frame)) ir_intrinsics = sensor.ir_intrinsics # repeat registration multiple times and average results R = np.zeros([3,3]) t = np.zeros([3,1]) points_3d_plane = PointCloud(np.zeros([3, sx*sy]), frame=sensor.ir_frame) k = 0 while k < num_transform_avg: # average a bunch of depth images together depth_ims = None for i in range(num_images): start = time.time() small_color_im, new_depth_im, _ = sensor.frames() end = time.time() logging.info('Frames Runtime: %.3f' %(end-start)) if depth_ims is None: depth_ims = np.zeros([new_depth_im.height, new_depth_im.width, num_images]) depth_ims[:,:,i] = new_depth_im.data med_depth_im = np.median(depth_ims, axis=2) depth_im = DepthImage(med_depth_im, sensor.ir_frame) # find the corner pixels in an upsampled version of the color image big_color_im = small_color_im.resize(color_image_rescale_factor) corner_px = big_color_im.find_chessboard(sx=sx, sy=sy) if vis: plt.figure() plt.imshow(big_color_im.data) for i in range(sx): plt.scatter(corner_px[i,0], corner_px[i,1], s=25, c='b') plt.show() if corner_px is None: logging.error('No chessboard detected! Check camera exposure settings') continue # convert back to original image small_corner_px = corner_px / color_image_rescale_factor if vis: plt.figure() plt.imshow(small_color_im.data) for i in range(sx): plt.scatter(small_corner_px[i,0], small_corner_px[i,1], s=25, c='b') plt.axis('off') plt.show() # project points into 3D camera_intr = sensor.ir_intrinsics points_3d = camera_intr.deproject(depth_im) # get round chessboard ind corner_px_round = np.round(small_corner_px).astype(np.uint16) corner_ind = depth_im.ij_to_linear(corner_px_round[:,0], corner_px_round[:,1]) if corner_ind.shape[0] != sx*sy: logging.warning('Did not find all corners. Discarding...') continue # average 3d points points_3d_plane = (k * points_3d_plane + points_3d[corner_ind]) / (k + 1) logging.info('Registration iteration %d of %d' %(k+1, config['num_transform_avg'])) k += 1 # fit a plane to the chessboard corners X = np.c_[points_3d_plane.x_coords, points_3d_plane.y_coords, np.ones(points_3d_plane.num_points)] y = points_3d_plane.z_coords A = X.T.dot(X) b = X.T.dot(y) w = np.linalg.inv(A).dot(b) n = np.array([w[0], w[1], -1]) n = n / np.linalg.norm(n) if flip_normal: n = -n mean_point_plane = points_3d_plane.mean() # find x-axis of the chessboard coordinates on the fitted plane T_camera_table = RigidTransform(translation = -points_3d_plane.mean().data, from_frame=points_3d_plane.frame, to_frame='table') points_3d_centered = T_camera_table * points_3d_plane # get points along y if point_order == 'row_major': coord_pos_x = int(math.floor(sx*sy/2.0)) coord_neg_x = int(math.ceil(sx*sy/2.0)) points_pos_x = points_3d_centered[coord_pos_x:] points_neg_x = points_3d_centered[:coord_neg_x] x_axis = np.mean(points_pos_x.data, axis=1) - np.mean(points_neg_x.data, axis=1) x_axis = x_axis - np.vdot(x_axis, n)*n x_axis = x_axis / np.linalg.norm(x_axis) y_axis = np.cross(n, x_axis) else: coord_pos_y = int(math.floor(sx*(sy-1)/2.0)) coord_neg_y = int(math.ceil(sx*(sy+1)/2.0)) points_pos_y = points_3d_centered[:coord_pos_y] points_neg_y = points_3d_centered[coord_neg_y:] y_axis = np.mean(points_pos_y.data, axis=1) - np.mean(points_neg_y.data, axis=1) y_axis = y_axis - np.vdot(y_axis, n)*n y_axis = y_axis / np.linalg.norm(y_axis) x_axis = np.cross(-n, y_axis) # produce translation and rotation from plane center and chessboard basis rotation_cb_camera = RigidTransform.rotation_from_axes(x_axis, y_axis, n) translation_cb_camera = mean_point_plane.data T_cb_camera = RigidTransform(rotation=rotation_cb_camera, translation=translation_cb_camera, from_frame='cb', to_frame=sensor.frame) if y_points_left and np.abs(T_cb_camera.y_axis[1]) > 0.1: if T_cb_camera.x_axis[0] > 0: T_cb_camera.rotation = T_cb_camera.rotation.dot(RigidTransform.z_axis_rotation(-np.pi/2).T) else: T_cb_camera.rotation = T_cb_camera.rotation.dot(RigidTransform.z_axis_rotation(np.pi/2).T) T_camera_cb = T_cb_camera.inverse() # optionally display cb corners with detected pose in 3d space if config['debug']: # display image with axes overlayed cb_center_im = camera_intr.project(Point(T_cb_camera.translation, frame=sensor.ir_frame)) cb_x_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.x_axis * config['scale_amt'], frame=sensor.ir_frame)) cb_y_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.y_axis * config['scale_amt'], frame=sensor.ir_frame)) cb_z_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.z_axis * config['scale_amt'], frame=sensor.ir_frame)) x_line = np.array([cb_center_im.data, cb_x_im.data]) y_line = np.array([cb_center_im.data, cb_y_im.data]) z_line = np.array([cb_center_im.data, cb_z_im.data]) plt.figure() plt.imshow(small_color_im.data) plt.scatter(cb_center_im.data[0], cb_center_im.data[1]) plt.plot(x_line[:,0], x_line[:,1], c='r', linewidth=3) plt.plot(y_line[:,0], y_line[:,1], c='g', linewidth=3) plt.plot(z_line[:,0], z_line[:,1], c='b', linewidth=3) plt.axis('off') plt.title('Chessboard frame in camera %s' %(sensor.frame)) plt.show() return ChessboardRegistrationResult(T_camera_cb, points_3d_plane)
def get_camera_poses(config, frame='world'): """Get a list of camera-to-frame poses broken up uniformly about a viewsphere. Parameters ---------- config : autolab_core.YamlConfig A configuration containing parameters of the random variable. frame : str The name of the target world frame. Notes ----- Required parameters of config are specified in Other Parameters. Other Parameters ---------------- radius: Distance from camera to world origin. min : float max : float n : int azimuth: Azimuth (angle from x-axis) of camera in degrees. min : float max : float n : int elevation: Elevation (angle from z-axis) of camera in degrees. min : float max : float n : int roll: Roll (angle about view direction) of camera in degrees. min : float max : float n : int Returns ------- list of autolab_core.RigidTransform A list of camera-to-frame transforms. """ min_radius = config['radius']['min'] max_radius = config['radius']['max'] num_radius = config['radius']['n'] radii = np.linspace(min_radius, max_radius, num_radius) min_azimuth = config['azimuth']['min'] max_azimuth = config['azimuth']['max'] num_azimuth = config['azimuth']['n'] azimuths = np.linspace(min_azimuth, max_azimuth, num_azimuth) min_elev = config['elev']['min'] max_elev = config['elev']['max'] num_elev = config['elev']['n'] elevs = np.linspace(min_elev, max_elev, num_elev) min_roll = config['roll']['min'] max_roll = config['roll']['max'] num_roll = config['roll']['n'] rolls = np.linspace(min_roll, max_roll, num_roll) camera_to_frame_tfs = [] for r in radii: for a in azimuths: for e in elevs: for roll in rolls: cam_center = np.array([sph2cart(r, a, e)]).squeeze() cz = -cam_center / np.linalg.norm(cam_center) cx = np.array([cz[1], -cz[0], 0]) if np.linalg.norm(cx) == 0: cx = np.array([1.0, 0.0, 0.0]) cx = cx / np.linalg.norm(cx) cy = np.cross(cz, cx) cy = cy / np.linalg.norm(cy) if cy[2] > 0: cx = -cx cy = np.cross(cz, cx) cy = cy / np.linalg.norm(cy) R_cam_frame = np.array([cx, cy, cz]).T R_roll = RigidTransform.z_axis_rotation(roll) R_cam_frame = R_cam_frame.dot(R_roll) T_camera_frame = RigidTransform(R_cam_frame, cam_center, from_frame='camera', to_frame=frame) camera_to_frame_tfs.append(T_camera_frame) return camera_to_frame_tfs
from frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage, CartesianImpedanceSensorMessage from franka_interface_msgs.msg import SensorDataGroup from frankapy.utils import min_jerk, min_jerk_weight import rospy if __name__ == "__main__": fa = FrankaArm() fa.reset_joints() rospy.loginfo('Generating Trajectory') p0 = fa.get_pose() p1 = p0.copy() T_delta = RigidTransform(translation=np.array([0, 0, 0.2]), rotation=RigidTransform.z_axis_rotation( np.deg2rad(30)), from_frame=p1.from_frame, to_frame=p1.from_frame) p1 = p1 * T_delta fa.goto_pose(p1) T = 5 dt = 0.02 ts = np.arange(0, T, dt) weights = [min_jerk_weight(t, T) for t in ts] pose_traj = [p1.interpolate_with(p0, w) for w in weights] z_stiffness_traj = [min_jerk(100, 800, t, T) for t in ts] rospy.loginfo('Initializing Sensor Publisher')