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) c = ControlInterface(robot, kb) p = PlanningInterface(kb)
logging.basicConfig(level=logging.DEBUG, format="%(asctime)-15s %(filename)s:%(lineno)d %(levelname)s: %(message)s") # allow importing from the repository root import sys, os sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from integration.control_server import ControlServer from integration.interface import ControlInterface from api.shared import KnowledgeBase, RobotState from apc.motion import FakeLowLevelController as LowLevelController from time import sleep from klampt.robotsim import WorldModel kb = KnowledgeBase() parallel = False if parallel: cs = ControlServer(kb) task = cs.start() while not task.done: print "starting..." sleep(1) print kb.robot_state.sensed_config for plan in ["test plan", "test plan 2"]: task = cs.execute(plan) if not task: print "restarting ControlServer"
import math 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') robot = LowLevelController(model) #initial config robot.setMilestone([ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.1, 0.0, 2.358, 0.0, 0.0, 0.3, -0.98, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.18, 0.0, 2.118, 0.04, 0.0, 0.62, 0.901, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) kb = KnowledgeBase() kb.robot_state = RobotState(robot) kb.active_limb = 'left' kb.bin_contents = apc_order['bin_contents'] kb.target_bin = 'bin_D' kb.withdraw_point_xforms = json.load(open('kb/withdraw_point_xforms.json')) kb.bin_bounds = json.load(open('kb/bin_bounds.json')) # kb.object_xforms['cheezit_bit_original'] = (se3.identity()[0], (0.7, 0.5, 1)) # TODO: Proper shelf transform needs created here r = PerceptionInterface(kb) # 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))
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') robot = LowLevelController(model) #initial config robot.setMilestone([ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.1, 0.0, 2.358, 0.0, 0.0, 0.3, -0.98, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.18, 0.0, 2.118, 0.04, 0.0, 0.62, 0.901, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) #bin f config # robot.setMilestone([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0999999999999996, 0.0, 2.358, 0.0, 0.0, 0.3, -0.9800000000000004, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.30158718316595046, 0.1653559427347274, 2.2363132059607045, 1.834400059027026, 1.0639366601020779, 0.0, -1.0132113159072906, 2.7856916491114374, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) kb = KnowledgeBase() kb.robot_state = RobotState(robot) kb.active_limb = 'left' kb.bin_contents = apc_order['bin_contents'] kb.withdraw_point_xforms = json.load(open('kb/withdraw_point_xforms.json')) kb.bin_bounds = json.load(open('kb/bin_bounds.json')) try: reader = csv.reader(open(os.path.join(knowledge_base_dir,'Object Dimensions.csv'))) header = None for line in reader: if header == None: header = line else: kb.object_minimum_dims[line[0]] = float(line[1])*0.01 kb.object_maximum_dims[line[0]] = float(line[2])*0.01
knowledge_base_dir = './kb/' world = WorldModel() model = world.loadRobot('klampt_models/baxter_with_reflex_gripper_col.rob') robot = LowLevelController(model) robot.setMilestone([ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.1, 0.0, 2.358, 0.0, 0.0, 0.3, -0.98, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.18, 0.0, 2.118, 0.04, 0.0, 0.62, 0.901, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # robot.setMilestone([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0999999999999996, 0.0, 2.358, 0.0, 0.0, 0.3, -0.9800000000000004, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06679184745163541, 0.365135941112352, 2.5041869579036335, 1.5646287249338475, 0.8377567543461182, 0.0, -1.097366825056886, 2.493786406695833, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) kb = KnowledgeBase() kb.robot_state = RobotState(robot) kb.active_limb = 'right' kb.withdraw_point_xforms = json.load(open('kb/withdraw_point_xforms.json')) kb.bin_bounds = json.load(open('kb/bin_bounds.json')) # kb.object_xforms['cheezit_bit_original'] = (se3.identity()[0], (0.7, 0.5, 1)) # 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) kb.shelf_xform = localizeShelf()