def from_feature_vec(v, camera_intr=None, angle=None, depth=None, axis=None): """Creates a `SuctionPoint2D` instance from a feature vector and additional parameters. Parameters ---------- v : :obj:`numpy.ndarray` Feature vector, see `Grasp2D.feature_vec`. camera_intr : :obj:`perception.CameraIntrinsics` Frame of reference for camera that the grasp corresponds to. depth : float Hard-set the depth for the suction grasp. axis : :obj:`numpy.ndarray` Normalized 3-vector specifying the approach direction. """ # Read feature vec. center_px = v[:2] grasp_angle = 0 if v.shape[0] > 2 and angle is None: # grasp_angle = v[2] grasp_vec = v[2:] grasp_vec = grasp_vec / np.linalg.norm(grasp_vec) grasp_angle = np.arctan2(grasp_vec[1], grasp_vec[0]) elif angle is not None: grasp_angle = angle grasp_axis = np.array([1, 0, 0]) if axis is not None: grasp_axis = axis grasp_depth = 0.5 if depth is not None: grasp_depth = depth x_axis = grasp_axis y_axis = np.array([grasp_axis[1], -grasp_axis[0], 0]) if np.linalg.norm(y_axis) == 0: y_axis = np.array([1, 0, 0]) y_axis = y_axis / np.linalg.norm(y_axis) z_axis = np.cross(x_axis, y_axis) R = np.array([x_axis, y_axis, z_axis]).T R = R.dot(RigidTransform.x_axis_rotation(grasp_angle)) t = camera_intr.deproject_pixel( grasp_depth, Point(center_px, frame=camera_intr.frame)).data T = RigidTransform(rotation=R, translation=t, from_frame="grasp", to_frame=camera_intr.frame) # Compute center and angle. return MultiSuctionPoint2D(T, camera_intr=camera_intr)
def turn_handle(self, visualize=False): start_pose = ut.get_link_pose(self.robot, self.grasp_joint) start_quat = start_pose[1] amount_rotate = np.pi / 4. rot_y = RigidTransform.x_axis_rotation(amount_rotate) end_rot = np.dot(rot_y, Quaternion(start_quat).rotation_matrix) end_quat = Quaternion(matrix=end_rot).elements step_size = np.pi / 16. quat_waypoints = ut.get_quaternion_waypoints(start_pose, start_quat, end_quat) theta = np.pi end_theta = np.pi + amount_rotate n_waypoints = np.round((end_theta - theta) / (step_size)) thetas = np.linspace(theta, end_theta, n_waypoints) r = 0.10 center = self.door_knob_center.copy() center[1] -= 0.055 #franka_tool to 9/10 pose pos_traj = [] for theta, quat in zip(thetas, quat_waypoints): new_pt = (center[0] + r * np.cos(theta), center[1], center[2] + r * np.sin(theta)) pos_traj.append((new_pt, quat[1])) sph = ut.create_sphere(0.025) ut.set_point(sph, new_pt), joints_to_plan_for = ut.get_movable_joints(self.robot)[:-2] traj = [] for pos in pos_traj: new_jts = ut.inverse_kinematics( self.robot, self.grasp_joint, pos, movable_joints=joints_to_plan_for, null_space=self.get_null_space(joints_to_plan_for), ori_tolerance=0.02) if new_jts is not None: traj.append(new_jts) else: new_jts = ut.inverse_kinematics( self.robot, self.grasp_joint, pos, movable_joints=joints_to_plan_for, null_space=None, ori_tolerance=0.03) if new_jts is None: print("no jt solution found") else: traj.append(new_jts) assert (len(traj) > 0) if visualize: self.visualize_traj(traj) n_pts = min(10, len(traj)) mask = np.round(np.linspace(0, len(traj) - 1, n_pts)).astype(np.int32) traj = np.array(traj)[mask] assert (traj is not None) return traj
def straighten_transform(rt): angles = rt.euler_angles roll_off = angles[0] pitch_off = angles[1] roll_fix = RigidTransform( rotation=RigidTransform.x_axis_rotation(np.pi - roll_off), from_frame=rt.from_frame, to_frame=rt.from_frame) pitch_fix = RigidTransform( rotation=RigidTransform.y_axis_rotation(pitch_off), from_frame=rt.from_frame, to_frame=rt.from_frame) new_rt = rt * roll_fix * pitch_fix return new_rt
def _sample_suction_points(self, depth_im, camera_intr, num_samples, segmask=None, visualize=False, constraint_fn=None): """Sample a set of 2D suction point candidates from a depth image by choosing points on an object surface uniformly at random and then sampling around the surface normal. Parameters ---------- depth_im : :obj:"perception.DepthImage" Depth image to sample from. camera_intr : :obj:`perception.CameraIntrinsics` Intrinsics of the camera that captured the images. num_samples : int Number of grasps to sample. segmask : :obj:`perception.BinaryImage` Binary image segmenting out the object of interest. visualize : bool Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` Constraint function to apply to grasps. Returns ------- :obj:`list` of :obj:`SuctionPoint2D` List of 2D suction point candidates. """ # Compute edge pixels. filter_start = time() if self._depth_gaussian_sigma > 0: depth_im_mask = depth_im.apply(snf.gaussian_filter, sigma=self._depth_gaussian_sigma) else: depth_im_mask = depth_im.copy() if segmask is not None: depth_im_mask = depth_im.mask_binary(segmask) self._logger.debug("Filtering took %.3f sec" % (time() - filter_start)) if visualize: vis.figure() vis.subplot(1, 2, 1) vis.imshow(depth_im) vis.subplot(1, 2, 2) vis.imshow(depth_im_mask) vis.show() # Project to get the point cloud. cloud_start = time() point_cloud_im = camera_intr.deproject_to_image(depth_im_mask) normal_cloud_im = point_cloud_im.normal_cloud_im() nonzero_px = depth_im_mask.nonzero_pixels() num_nonzero_px = nonzero_px.shape[0] if num_nonzero_px == 0: return [] self._logger.debug("Normal cloud took %.3f sec" % (time() - cloud_start)) # Randomly sample points and add to image. sample_start = time() suction_points = [] k = 0 sample_size = min(self._max_num_samples, num_nonzero_px) indices = np.random.choice(num_nonzero_px, size=sample_size, replace=False) while k < sample_size and len(suction_points) < num_samples: # Sample a point uniformly at random. ind = indices[k] center_px = np.array([nonzero_px[ind, 1], nonzero_px[ind, 0]]) center = Point(center_px, frame=camera_intr.frame) axis = -normal_cloud_im[center.y, center.x] # depth = point_cloud_im[center.y, center.x][2] orientation = 2 * np.pi * np.random.rand() # Update number of tries. k += 1 # Skip bad axes. if np.linalg.norm(axis) == 0: continue # Rotation matrix. x_axis = axis y_axis = np.array([axis[1], -axis[0], 0]) if np.linalg.norm(y_axis) == 0: y_axis = np.array([1, 0, 0]) y_axis = y_axis / np.linalg.norm(y_axis) z_axis = np.cross(x_axis, y_axis) R = np.array([x_axis, y_axis, z_axis]).T # R_orig = np.copy(R) R = R.dot(RigidTransform.x_axis_rotation(orientation)) t = point_cloud_im[center.y, center.x] pose = RigidTransform(rotation=R, translation=t, from_frame="grasp", to_frame=camera_intr.frame) # Check center px dist from boundary. if (center_px[0] < self._min_dist_from_boundary or center_px[1] < self._min_dist_from_boundary or center_px[1] > depth_im.height - self._min_dist_from_boundary or center_px[0] > depth_im.width - self._min_dist_from_boundary): continue # Keep if the angle between the camera optical axis and the suction # direction is less than a threshold. dot = max(min(axis.dot(np.array([0, 0, 1])), 1.0), -1.0) psi = np.arccos(dot) if psi < self._max_suction_dir_optical_axis_angle: # Check distance to ensure sample diversity. candidate = MultiSuctionPoint2D(pose, camera_intr=camera_intr) # Check constraint satisfaction. if constraint_fn is None or constraint_fn(candidate): if visualize: vis.figure() vis.imshow(depth_im) vis.scatter(center.x, center.y) vis.show() suction_points.append(candidate) self._logger.debug("Loop took %.3f sec" % (time() - sample_start)) return suction_points
def process_frame(frame_json, camera_intrinsics, models): num_objects = len(frame_json['objects']) if len(frame_json['objects']) == 0: print "no objects in frame!" return frame_json # copy relevant fields of json updated_frame_json = {'camera_data': {}, 'objects': []} if frame_json['camera_data']: updated_frame_json['camera_data'][ 'location_worldframe'] = copy.deepcopy( frame_json['camera_data']['location_worldframe']) updated_frame_json['camera_data'][ 'quaternion_xyzw_worldframe'] = copy.deepcopy( frame_json['camera_data']['quaternion_xyzw_worldframe']) else: print 'Warning: no `camera_data` field!' for i in range(len(frame_json['objects'])): updated_frame_json['objects'].append({}) updated_frame_json['objects'][i]['class'] = copy.deepcopy( frame_json['objects'][i]['class']) updated_frame_json['objects'][i]['bounding_box'] = copy.deepcopy( frame_json['objects'][i]['bounding_box']) updated_frame_json['objects'][i]['instance_id'] = copy.deepcopy( frame_json['objects'][i]['instance_id']) updated_frame_json['objects'][i]['visibility'] = copy.deepcopy( frame_json['objects'][i]['visibility']) # get object poses and flip symmetrical objects if necessary object_poses = [] for object_index in range(num_objects): model_name = updated_frame_json['objects'][object_index]['class'] scene_object_json = frame_json['objects'][object_index] (translation, rotation) = object_pose_from_json(scene_object_json) object_pose = RigidTransform(rotation=rotation, translation=translation, from_frame='source_model', to_frame='camera') object_pose = object_pose.dot(models[model_name].model_transform) # Handle symmetrical objects: if z axis (= x axis in mesh) points towards camera, # rotate by 180 degrees around x (= z axis in mesh). # This always keeps the "green/yellow" short side (in nvdu_viz) pointed towards # the camera; the "blue/magenta" short side should never be visible. This is necessary # for DOPE training, since both sides look the same because the KLT box is symmetrical. symmetry_flip_tf = RigidTransform(from_frame='target_model', to_frame='target_model_original') z_axis = rotation.dot(np.array([0.0, 0.0, 1.0], dtype=np.float64)) if z_axis.dot(translation) < 0: symmetry_flip_tf.rotation = RigidTransform.x_axis_rotation(math.pi) object_pose = object_pose.dot(symmetry_flip_tf) object_poses.append(object_pose) updated_frame_json['objects'][object_index][ 'location'] = object_pose.translation.tolist() updated_frame_json['objects'][object_index][ 'quaternion_xyzw'] = np.roll(object_pose.quaternion, -1).tolist() # add pose_transform_permuted for object_index in range(num_objects): ptp_json = pose_transform_permuted_to_json( object_poses[object_index].translation, object_poses[object_index].rotation) updated_frame_json['objects'][object_index].update(ptp_json) # compute cuboid, cuboid_centroid, projected_cuboid, projected_cuboid_centroid, bounding_box for object_index in range(num_objects): model_name = updated_frame_json['objects'][object_index]['class'] cuboid_json = get_cuboid(object_poses[object_index], models[model_name].cuboid_dimensions, camera_intrinsics) updated_frame_json['objects'][object_index].update(cuboid_json) return updated_frame_json
print('Opening gripper all the way') fa.open_gripper() # joint controls print('Rotating last joint') joints = fa.get_joints() joints[6] += np.deg2rad(45) fa.goto_joints(joints) joints[6] -= np.deg2rad(45) fa.goto_joints(joints) # end-effector pose control print('Translation') T_ee_world = fa.get_pose() T_ee_world.translation += [0.1, 0, 0.1] fa.goto_pose(T_ee_world) T_ee_world.translation -= [0.1, 0, 0.1] fa.goto_pose(T_ee_world) print('Rotation in end-effector frame') T_ee_rot = RigidTransform( rotation=RigidTransform.x_axis_rotation(np.deg2rad(45)), from_frame='franka_tool', to_frame='franka_tool' ) T_ee_world_target = T_ee_world * T_ee_rot fa.goto_pose(T_ee_world_target) fa.goto_pose(T_ee_world) # reset franka back to home fa.reset_joints()