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')
