Example #1
0
    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())
Example #2
0
 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()
Example #3
0
    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()
Example #4
0
 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)
Example #5
0
 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)