def run(self): while self.pc is None and not rospy.is_shutdown(): print('Waiting for pointcloud') rospy.sleep(1) convexify_pointcloud.add_convexified_pointcloud_to_env( self.sim, self.pc, self.cam.get_pose())
def run(self): self.sim.update() while self.handle_pose is None and not rospy.is_shutdown(): print('Waiting for average handle pose') rospy.sleep(1) handle_pose = self.handle_pose while True: try: handle_pose.stamp = rospy.Time.now() tmp_handle_pose = tfx.convertToFrame(handle_pose, '/base_link') handle_pose = tmp_handle_pose break except: pass self.sim.plot_transform(self.sim.transform_from_to(handle_pose, handle_pose.frame, 'world')) print('Received handle pose') while self.pc is None and not rospy.is_shutdown(): print('Waiting for pointcloud') rospy.sleep(1) convexify_pointcloud.add_convexified_pointcloud_to_env(self.sim, self.pc, self.cam.get_pose()) print('Convexified pointcloud, press enter') raw_input() joints = self.planner.get_joint_trajectory(self.arm.get_joints(), handle_pose, ignore_orientation=True) poses = [tfx.pose(self.arm.fk(joint), 'base_link') for joint in joints] for pose in poses: self.sim.plot_transform(self.sim.transform_from_to(pose, pose.frame, 'world')) IPython.embed()
def run(self): self.sim.update() while self.handle_pose is None and not rospy.is_shutdown(): print('Waiting for average handle pose') rospy.sleep(1) handle_pose = self.handle_pose while True: try: handle_pose.stamp = rospy.Time.now() tmp_handle_pose = tfx.convertToFrame(handle_pose, '/base_link') handle_pose = tmp_handle_pose break except: pass self.sim.plot_transform( self.sim.transform_from_to(handle_pose, handle_pose.frame, 'world')) print('Received handle pose') while self.pc is None and not rospy.is_shutdown(): print('Waiting for pointcloud') rospy.sleep(1) convexify_pointcloud.add_convexified_pointcloud_to_env( self.sim, self.pc, self.cam.get_pose()) print('Convexified pointcloud, press enter') raw_input() joints = self.planner.get_joint_trajectory(self.arm.get_joints(), handle_pose, ignore_orientation=True) poses = [tfx.pose(self.arm.fk(joint), 'base_link') for joint in joints] for pose in poses: self.sim.plot_transform( self.sim.transform_from_to(pose, pose.frame, 'world')) IPython.embed()
def run(self): while not rospy.is_shutdown(): print('Getting most recent callbacks') rospy.sleep(0.05) self.sim.update() self.get_most_recent_callbacks() self.is_reset_kinfu = False graspable_points = self.graspable_points table_pose = self.table_pose table_extents = self.table_extents avg_handle_poses = self.avg_handle_poses if self.is_reset_kinfu: continue # fails if too few points print('Point cloud size: {0}'.format(graspable_points.width)) if graspable_points.width < 50: print('Not enough points: {0}'.format(graspable_points.width)) continue if len(avg_handle_poses) == 0: print('No handles found') continue #assert graspable_points.header.frame_id.replace('/','').count(table_pose.frame.replace('/','')) > 0 print('Convexifying point cloud') self.sim.update() self.sim.clear_kinbodies() try: convexify_pointcloud.add_convexified_pointcloud_to_env(self.sim, graspable_points, point_cloud_filter=self.point_cloud_filter) except Warning as e: print('Error convexifying point cloud: {0}'.format(e)) continue except Exception as e: print('Error convexifying point cloud: {0}'.format(e)) continue print('Adding table mesh') self.add_table(table_pose, table_extents) arm_pose = self.arm.get_pose() #avg_handle_poses = sorted(avg_handle_poses, key=lambda p: np.linalg.norm(p.position.array - arm_pose.position.array)) avg_handle_poses = sorted(avg_handle_poses, key=lambda p: -p.position.z) for i, handle_pose in enumerate(avg_handle_poses): if self.is_reset_kinfu: break print('Checking if handle pose {0} is collision free and reachable'.format(i)) grasp_joint_traj = self.get_grasp_trajectory(handle_pose) if grasp_joint_traj is not None: grasp_joints = grasp_joint_traj[-1] return_grasp_joint_traj = self.get_return_from_grasp_trajectory(grasp_joints, table_pose) if return_grasp_joint_traj is None: return_grasp_joint_traj = list() self.plot_joint_traj(grasp_joint_traj) self.plot_joint_traj(return_grasp_joint_traj) self.publish_joint_traj(self.grasp_joint_traj_pub, grasp_joint_traj) self.publish_joint_traj(self.return_grasp_joint_traj_pub, return_grasp_joint_traj) break rospy.sleep(0.1)
def run(self): while self.pc is None and not rospy.is_shutdown(): print('Waiting for pointcloud') rospy.sleep(1) convexify_pointcloud.add_convexified_pointcloud_to_env(self.sim, self.pc, self.cam.get_pose())
def run(self): while not rospy.is_shutdown(): print('Getting most recent callbacks') rospy.sleep(0.05) self.sim.update() self.get_most_recent_callbacks() self.is_reset_kinfu = False graspable_points = self.graspable_points table_pose = self.table_pose table_extents = self.table_extents avg_handle_poses = self.avg_handle_poses if self.is_reset_kinfu: continue # fails if too few points print('Point cloud size: {0}'.format(graspable_points.width)) if graspable_points.width < 50: print('Not enough points: {0}'.format(graspable_points.width)) continue if len(avg_handle_poses) == 0: print('No handles found') continue #assert graspable_points.header.frame_id.replace('/','').count(table_pose.frame.replace('/','')) > 0 print('Convexifying point cloud') self.sim.update() self.sim.clear_kinbodies() try: convexify_pointcloud.add_convexified_pointcloud_to_env( self.sim, graspable_points, point_cloud_filter=self.point_cloud_filter) except Warning as e: print('Error convexifying point cloud: {0}'.format(e)) continue except Exception as e: print('Error convexifying point cloud: {0}'.format(e)) continue print('Adding table mesh') self.add_table(table_pose, table_extents) arm_pose = self.arm.get_pose() #avg_handle_poses = sorted(avg_handle_poses, key=lambda p: np.linalg.norm(p.position.array - arm_pose.position.array)) avg_handle_poses = sorted(avg_handle_poses, key=lambda p: -p.position.z) for i, handle_pose in enumerate(avg_handle_poses): if self.is_reset_kinfu: break print( 'Checking if handle pose {0} is collision free and reachable' .format(i)) grasp_joint_traj = self.get_grasp_trajectory(handle_pose) if grasp_joint_traj is not None: grasp_joints = grasp_joint_traj[-1] return_grasp_joint_traj = self.get_return_from_grasp_trajectory( grasp_joints, table_pose) if return_grasp_joint_traj is None: return_grasp_joint_traj = list() self.plot_joint_traj(grasp_joint_traj) self.plot_joint_traj(return_grasp_joint_traj) self.publish_joint_traj(self.grasp_joint_traj_pub, grasp_joint_traj) self.publish_joint_traj(self.return_grasp_joint_traj_pub, return_grasp_joint_traj) break rospy.sleep(0.1)