def __init__(self, knowledge_base): PerceptionInterface.__init__(self, knowledge_base) world = WorldModel() robot = world.loadRobot(os.path.join('klampt_models', baxter.klampt_model_name)) robot.setConfig(self.knowledge_base.robot_state.sensed_config) self.base_xform = robot.getLink('{}_gripper'.format(self.knowledge_base.active_limb)).getTransform() if self.knowledge_base.active_limb == 'left': self.camera_xform = se3.mul(self.base_xform, self.knowledge_base.left_camera_offset_xform) else: self.camera_xform = se3.mul(self.base_xform, self.knowledge_base.right_camera_offset_xform)
def __init__(self, knowledge_base=None, title=None): world = WorldModel() # self.ground = world.loadTerrain('klampt_models/plane.env') self.shelf = None self.order_bin = None self.objects = {} self.n = 0 # instantiate the robot self.robot = world.loadRobot(os.path.join("klampt_models", baxter.klampt_model_name)) logger.debug("robot model loaded from {}".format(baxter.klampt_model_name)) self.knowledge_base = knowledge_base self.robot_state = None WorldViewer.__init__(self, world, title=title or "Knowledge Base Viewer")
from integration.control_server import ControlServer from integration.interface import PlanningInterface, ControlInterface, PerceptionInterface from api.shared import KnowledgeBase, RobotState from apc.motion import PhysicalLowLevelController as LowLevelController from klampt.robotsim import WorldModel from klampt import se3,so3 from time import sleep import json apc_order = json.load(open(os.path.join(os.path.dirname(__file__), 'example.json'))) knowledge_base_dir = './kb/' world = WorldModel() model = world.loadRobot('klampt_models/baxter_with_reflex_gripper_col.rob') print 'before' robot = LowLevelController(model, 'baxter_with_reflex_gripper_col.rob') print 'here' kb = KnowledgeBase() kb.robot_state = RobotState(robot) print kb.robot_state kb.active_limb = 'left' kb.bin_contents = apc_order['bin_contents'] # make fake bin vantage points, need to update to real vantage points at some point for k in [ 'bin_vantage_points', 'vantage_point_xforms' ]: path = os.path.join(knowledge_base_dir, '{}.json'.format(k)) data = json.load(open(path)) setattr(kb, k, data)
class ControlJob(BaseJob): def __init__(self, knowledge_base): # this knowledge base reference is used for updating the robot state self.knowledge_base = knowledge_base # create the viewer self.viewer = Viewer() BaseJob.__init__(self, None, None) def _get_interface(self): return ControlInterface def _handler(self): logger.debug('child {} start'.format(self.child.pid)) # hold a reference to the world so that it is not garbage collected self.world = WorldModel() # create the robot model for the controller self.robot = self.world.loadRobot(os.path.join('klampt_models', baxter.klampt_model_name)) logger.debug('robot model loaded from {}'.format(baxter.klampt_model_name)) self.controller = LowLevelController(self.robot, baxter.klampt_model_name) logger.debug('controller low-level controller initialized') # send the initial controller state self.child_pipe.send((None, RobotState(self.controller))) while True: # get the call information try: # update the viewer while waiting for a command while not self.child_pipe.poll(): self._update_viewer() sleep(interval) # receive the command (self.knowledge_base, self.method, self.args, self.kwargs) = self.child_pipe.recv() except EOFError: logger.info('child {} shutdown requested'.format(self.child.pid)) # signal to shut down break # distinguish between pure knowledge base updates and method calls if not self.method: self._update_viewer() self._send_result(None) continue # route remote calls self.init_args = (self.controller, self.knowledge_base) # hook the sleep method to update the viewer during execution self.kwargs['sleep'] = lambda n: self._sleep(n) BaseJob._handler(self) # clean up # need to call the superclass since this class overrides with empty method BaseJob._close() # TODO: shutdown the controller #controller.motion.shutdown() self.viewer.close() def _sleep(self, n): while n > 0: self._update_viewer() n -= interval sleep(interval) def _update_viewer(self): try: # check to see if the viewer is alive if self.viewer.heartbeat: # update the viewer logger.info('updating viewer') self.viewer.update(self.knowledge_base, RobotState(self.controller)) logger.info('done updating viewer') else: logger.info('viewer missed heartbeat') pass except Exception as e: logger.error('failed to update the viewer: {}'.format(e)) def _send_result(self, result): # attach the robot state to the job result BaseJob._send_result(self, (result, RobotState(self.controller))) def _check(self): # run even if child is still alive # check for a result before attempting a blocking call if self.pipe.poll(): self._process_result() self._done = True # extract the robot state and update the knowledge base if not self.error: (self._result, self.knowledge_base.robot_state) = self.result # check that the child is alive # this is done after the poll since the child could have sent a result and then executed if not self.alive: logger.error('child {} died unexpectedly: {}'.format(self.child.pid, self.child.exitcode)) # set this exception as the error result self._error = Exception('child died unexpectedly: {}'.format(self.child.exitcode)) self._done = True def _close(self): # keep the pipes open by default pass
from api.shared import KnowledgeBase from apc.motion import PhysicalLowLevelController as LowLevelController from integration.visualization import debug_cloud from integration.camera import RemoteCamera from integration.camera import uvtexture, invalid_mask from integration.io import pcd from perception.segmentation.shelf import shelf_subtract limb = sys.argv[1] if limb not in [ 'right', 'left' ]: logger.error('invalid limb: {}'.format(limb)) raise SystemExit bin = sys.argv[2] world = WorldModel() model = world.loadRobot(os.path.join('klampt_models', baxter.klampt_model_name)) shelf = world.loadRigidObject(os.path.join('klampt_models', 'north_shelf', 'shelf_with_bins.obj')) robot = LowLevelController(model, baxter.klampt_model_name) model.setConfig(robot.getSensedConfig()) shelf_xform = ( numpy.array([[ 0, 1, 0 ], [ -1, 0, 0 ], [ 0, 0, 1 ]]), [ 1.03, -0.055, -0.9046 ] ) shelf.setTransform(list(shelf_xform[0].flat), shelf_xform[1]) SHELF_DIMS_PATH = os.path.join('kb', 'shelf_dims.json') SHELF_CLOUD_PATH = os.path.join('perception', 'segmentation', 'models', 'shelf_thin_10000.npy')