def go_to_waypoint(waypoint):
print waypoint
print("setting new goal")
navigation_msg = PoseStamped()
navigation_msg.header.frame_id = "base_link"
navigation_msg.header.stamp = rospy.Time.now()
navigation_msg.pose.position.x = waypoint[0]
navigation_msg.pose.position.y = waypoint[1]
q = quaternion_from_euler(0, 0, waypoint[2])
navigation_msg.pose.orientation = Quaternion(*q)
global nav_goal_pub
nav_goal_pub.publish(navigation_msg)
global rear_axle_center, rear_axle_theta, rear_axle_velocity, cmd_pub
rospy.wait_for_message("/ackermann_vehicle/ground_truth/state", Odometry,
1)
dx = waypoint[0] - rear_axle_center.position.x
dy = waypoint[1] - rear_axle_center.position.y
s0 = np.array([
rear_axle_center.position.x, rear_axle_center.position.y,
rear_axle_theta
])
fc = 30
v0 = 0
sf = waypoint
us, states = plan(s0, sf, v0, fc)
rate = rospy.Rate(fc)
target_distance = math.sqrt(dx * dx + dy * dy)
i = 0
while target_distance > waypoint_tol and i < us.shape[1]:
this_vel = np.clip(us[0][i], -max_vel, max_vel)
# this_acc = (this_vel - rear_axle_velocity.linear.x)/(1.0/fc)
this_ang = np.clip(us[1][i], -max_steering_angle, max_steering_angle)
# if (this_acc > 0) and (this_acc > max_acc):
# this_vel = rear_axle_velocity.linear.x + max_acc*(1.0/fc)
# elif (this_acc < 0) and (this_acc < max_dec):
# this_vel = rear_axle_velocity.linear.x + max_dec*(1.0/fc)
cmd = AckermannDriveStamped()
cmd.header.stamp = rospy.Time.now()
cmd.header.frame_id = "base_link"
cmd.drive.steering_angle = this_ang
cmd.drive.speed = this_vel
# print(cmd)
cmd_pub.publish(cmd)
# rospy.wait_for_message("/ackermann_vehicle/ground_truth/state", Odometry, 1)
dx = waypoint[0] - rear_axle_center.position.x
dy = waypoint[1] - rear_axle_center.position.y
target_distance = math.sqrt(dx * dx + dy * dy)
i += 1
rate.sleep()
def test_titles(spec, config):
"""Tests for a derived resource to exist"""
flow = planner.plan(1, spec, **config)
titles = [
"Creating CSV",
"Creating JSON",
"Creating ZIP",
"Generating views",
"Validating package contents",
"Copying source data",
"Creating Package",
]
for pipeline_id, pipeline_details in flow:
assert pipeline_details['title'] in titles
def show_planned(major, start_year, start_sem, fixed):
if request.method == 'POST':
return redirect(
url_for('show_edit',
major=major,
start_year=start_year,
start_sem=start_sem,
fixed=fixed))
context = {}
context['major'] = major
context['start_year'] = int(start_year)
context['start_sem'] = int(start_sem)
context['plan'] = plan(context['major'], context['start_sem'], eval(fixed))
return render_template("planned.html", **context)
def test_connected(spec, config):
"""Tests that the generated grapיh is connected"""
flow = planner.plan(1, spec, **config)
connected_set = set()
added = True
while added:
added = False
for pipeline_id, pipeline in flow:
pipeline_id = './' + pipeline_id
if pipeline_id not in connected_set:
deps = [x['pipeline'] for x in pipeline.get('dependencies')]
print(pipeline_id, deps)
if all(dep in connected_set for dep in deps):
connected_set.add(pipeline_id)
added = True
assert len(connected_set) == len([pipeline_id for pipeline_id, _ in flow])
def _internal_upload(owner, contents, registry, config=CONFIGS):
errors = []
dataset_name = dataset_getter(contents)
now = datetime.datetime.now()
update_time_setter(contents, now)
flow_id = None
dataset_id = registry.format_identifier(owner, dataset_name)
dataset_obj = registry.create_or_update_dataset(dataset_id, owner,
contents, now)
create_time_setter(contents, dataset_obj.get('created_at'))
period_in_seconds, schedule_errors = parse_schedule(contents)
if len(schedule_errors) == 0:
registry.update_dataset_schedule(dataset_id, period_in_seconds, now)
revision = registry.create_revision(dataset_id, now, STATE_PENDING,
errors)
revision = revision['revision']
flow_id = registry.format_identifier(owner, dataset_name, revision)
pipelines = planner.plan(revision, contents, **config)
pipeline_spec = dict(pipelines)
for pipeline_id, pipeline_details in pipeline_spec.items():
pipeline_spec[pipeline_id]
doc = dict(pipeline_id=pipeline_id,
flow_id=flow_id,
title=pipeline_details.get('title'),
pipeline_details=pipeline_details,
status=STATE_PENDING,
errors=errors,
logs=[],
stats={},
created_at=now,
updated_at=now)
registry.save_pipeline(doc)
runner.start(None,
yaml.dump(pipeline_spec).encode('utf-8'),
status_cb=PipelineStatusCallback(registry),
verbosity=verbosity)
else:
errors.extend(schedule_errors)
return dataset_id, flow_id, errors
def repayment():
if request.method == 'GET':
return (render_template("planner/form.html"))
elif request.method == 'POST':
form = None
for k in request.form.keys():
form = json.loads(k)
loan_data = planner.loan_handler(form['loans'])
asset_data = planner.asset_handler(form['assets'])
expense_data = planner.expense_handler(form['expenses'])
income_data = planner.income_handler(form['incomes'])
norm_df, ira_df = planner.plan(loan_data, asset_data,\
income_data, expense_data)
print(norm_df)
print(ira_df)
os.chdir("/var/www/ozzytocin/ozzytocin")
f = open("static/temp/savings.csv", 'w+')
f2 = open("static/temp/ira_option.csv", 'w+')
norm_df.to_csv(f)
ira_df.to_csv(f2)
return ''
def test_partial(spec, config):
"""Tests for a derived resource to exist"""
flow = planner.plan(1, spec, **config)
formats = []
fmt = res_name = None
for pipeline_id, pipeline_details in flow:
for step in pipeline_details['pipeline']:
if step['run'] == 'assembler.update_resource':
params = step['parameters']['update']
datahub_type = params['datahub']['type']
if datahub_type == 'derived/preview':
continue
fmt = params['format']
assert params['datahub']['type'] == 'derived/' + fmt
assert len(params['datahub']['derivedFrom']) == 1
res_name = params['name']
elif step['run'].endswith('to_s3'):
params = step['parameters']
formats.append((params['path'], res_name, fmt))
expected_num = len(config['allowed_types']) if spec['meta']['dataset'] == \
'empty' else len(config['allowed_types']) + 1
assert expected_num == len(formats)
def test_only_one_sink(spec, config):
"""Tests that the generated grapיh has only one sink"""
flow = planner.plan(1, spec, **config)
assert get_flow_sink(flow) is not None
def test_different_pipeline_ids(spec, config):
"""Tests that the generated grapיh has different pipline ids"""
flow = planner.plan(1, spec, **config)
pipeline_ids = set(pipeline_id for pipeline_id, pipeline in flow)
assert len(pipeline_ids) == len(flow)
def main(utterance, world, holding, objects, **_):
result = {}
result['utterance'] = utterance
trees = parse(utterance)
if not trees:
result['output'] = "Parse error!"
return result
result['trees'] = [str(t[2].node["sem"]) for t in trees]
import interpreter
result['goals'] = goals =[interpreter.interpret(tree,world,objects,holding) for tree in trees]
if not goals:
result['output'] = "Interpretation error!"
return result
#if there are several parse tree, we check every one and see how many works, we just
#ignore the one that does not work, and if there is only one left, then it is good
#if there are several left, we should ask question and let the user decide which one
#he want
parse_tree_for_ambiguity=[t[2].node["sem"] for t in trees]
flag=False
error_information=[]
goals_copy=copy.deepcopy(goals)
for i in range(0,len(goals_copy)):
if(isinstance(goals_copy[i],dict)):
flag=True
else:
error_information.append(goals_copy[i])
goals.remove(goals_copy[i])
try:
del parse_tree_for_ambiguity[i]
except:
pass
if(not flag):
del result['goals']
if(len(error_information)>1):
error=[]
for i in range(0,len(error_information)):
error.append(" In No."+str(i+1)+" parse tree , "+error_information[i])
result['output']="There are "+str(len(error_information))+" different way to interpret this sentence ~!"+" and ".join(error)
else:
result['output']=error_information.pop()
return result
#We use this module to solve these ambiguity problems
#First: there are several plannable goal, we ask questions
#Second: there are one plannable goal, but there are several objects to be moved and
#the quantifier is not all or any, we should tell the user
import ambiguity
response=ambiguity.is_ambiguous(goals,world,objects,parse_tree_for_ambiguity)
if response[0]:
del result['goals']
result['output']=response[1]
return result
#after check of ambiguity,then,there are only one element in the goals and it is a unambiguious goal
goal = goals[0]
#update the parse_trees,to exclude the trees that are not possible
result['trees']=[str(i) for i in parse_tree_for_ambiguity]
import planner
result['plan'] =plan=planner.plan(goal,world,holding,objects)
if not plan:
result['output'] = "Planning error!"
return result
elif isinstance(plan,str):
del result['plan']
result['output']=plan
return result
result['output'] = "Success!"
return result
def run_controller(controller,command_queue):
# simply reveals the shelf xform
while True:
c = command_queue.get()
if c != None:
# print "Running command",c
if c == 'x':
completed = False
global dq
dq = 0.1
while not completed:
completed = controller.commandGripper([1])[0]
time.sleep(0.05)
elif c == 'u':
for i in range(50):
controller.commandGripper([-1])
time.sleep(0.01)
elif c == 'r':
# time.sleep(.5)
# controller.randomMoveHand(range=1)
# time.sleep(.5)
python = sys.executable
if len(sys.argv) < 2:
sys.argv += ['auto']
os.execl(python, python, * sys.argv)
elif c == 'n':
# create a new file to save grasp data
dc.newFile()
# save current pre-grasp to be used when the simulator restarts from failed grasp
dc.saveInitialConfig()
elif c == 'l':
# load initial config, and move robot to this config
config = dc.loadInitialConfig()
# print "current config = ", simWorld.robot(0).getConfig()
# print "goal config = ", config
for i in [3,4,5,10,15,19]:
while not ((config[i] > -math.pi) and (config[i] < math.pi)):
if config[i] < 0:
config[i] += math.pi*2
else:
config[i] -= math.pi*2
# print "adjusted goal config = ", config
# move to this config
# TODO: implement a trajectory planning to reach this config w/o collision
# controller.appendMilestone(config)
start = controller.getCommandedConfig()
planWorld.rigidObject(0).geometry().setCollisionMargin(0.01)
# planWorld.rigidObject(0).geometry().setCollisionMargin(0)
path = planner.plan(start, config)
# print path
if path == None:
print "Planning failed"
else:
for i in range(len(path)):
controller.appendMilestone(path[i])
while controller.isMoving():
time.sleep(0.1)
planWorld.rigidObject(0).geometry().setCollisionMargin(0.0)
elif c == 's':
dc.update()
dc.save()
elif c == 'd':
print pg.distance()
pg.collisionFree(simWorld.robot(0).getConfig)
elif c == 't':
print "test"
# print simWorld.rigidObject(0).getTransform()
# simWorld.rigidObject(0).setTransform(so3.identity(), [1,1,1])
# visualizer.refresh()
# pg.resetWorld()
# simWorld.rigidObject(0).geometry().setCurrentTransform(so3.identity(), [1,1,1])
# simWorld.rigidObject(0).geometry().transform(so3.identity(), [1,1,1])
# obj = simWorld.rigidObject(0)
# xform = obj.getTransform()
# obj.geometry().transform(so3.identity(), vectorops.add(xform[1], [0,0,0.2]))
# poses = pg.randomPoses(1)
# dc.newFile()
# dc.update()
# dc.save()
## save current robot config (pre-grasp)
numPoses = 500
for i in range(numPoses):
for j in range(100):
controller.commandGripper([-1])
time.sleep(0.02)
time.sleep(0.5)
# print "iter:", i
poses = []
# poses = pg.randomPoses(1, range=i+1)
poses = pg.randomPoses(1, range=1)
# poses = pg.randomPoses(1)
# print pg.distance()
time.sleep(0.5)
controller.setMilestone(poses[0])
time.sleep(0.5)
completed = False
# global dq
dq = 0.1
while not completed:
# print dq
result = controller.commandGripper([1.0])
completed = result[0]
graspSuccess = result[1]
time.sleep(0.05)
completed = False
dq = 0.1
while not completed:
result = controller.commandGripper([1])
completed = result[0]
graspSuccess = result[1]
time.sleep(0.05)
# time.sleep()
if graspSuccess:
print "grasp #", i, "success, saving to log"
dc.update()
# save success case
dc.save(1)
else:
print "grasp #", i, "fail, saving to log"
dc.update()
# save failure case
dc.save(0)
command_queue.put('r')
break
# python = sys.executable
# os.execl(python, python, * sys.argv)
elif c == 'o':
# store current configuration as the "example"
command_queue.put('n')
command_queue.put('s')
for i in range(5):
command_queue.put('u')
command_queue.put('r')
command_queue.put('x')
command_queue.put('x')
command_queue.put('s')
else:
controller.moveHand(c)
else:
print "Waiting for command..."
time.sleep(0.1)
print "Done"
def runplanner(domain, problem, useheuristic, cost, visited, expanded):
(_, cost.value, visited.value,
expanded.value) = planner.plan(pddl.parse_domain(domain),
pddl.parse_problem(problem), useheuristic)