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')
# load the shelf subtraction data
bin_bounds = json.load(open(SHELF_DIMS_PATH))[bin]
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)
r = PerceptionInterface(kb)
except IndexError:
print 'missing argument'
print sys.argv[0], '[save base name] [link names]...'
raise SystemExit
if os.path.exists(filename):
print 'Save file', filename, 'already exists. Overwrite? [y/N]',
prompt = raw_input()
if prompt.lower() != 'y':
raise SystemExit
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)
limb = 'left'
bin = 'bin_A'
if limb == 'left':
realsense_pc = '192.168.0.103'
base_xform = model.getLink('left_gripper').getTransform()
camera_xform = se3.mul(base_xform, KnowledgeBase.left_camera_offset_xform)
else:
realsense_pc = '192.168.0.104'
base_xform = model.getLink('right_gripper').getTransform()
camera_xform = se3.mul(base_xform, KnowledgeBase.right_camera_offset_xform)
save_file = open('{}.json'.format(filename), 'w')
