def idle(self):
if self.transitPath and self.transferPath and self.retractPath:
#loop the path animation
n = len(self.transitPath)+len(self.transferPath)+len(self.retractPath)-2
u = (self.ttotal - self.animationTime)*3.0
i = int(math.floor(u))
s = u - i
i = i%(n-1)
if i+1 < len(self.transitPath):
q = vectorops.interpolate(self.transitPath[i],self.transitPath[i+1],s)
#set the robot configuration for display
self.robot.setConfig(q)
self.object.setTransform(*self.Tstart)
elif i+1 < len(self.transitPath)+len(self.transferPath)-1:
#index into the transfer path
i = i - (len(self.transitPath)-1)
q = vectorops.interpolate(self.transferPath[i],self.transferPath[i+1],s)
Tobj = graspedObjectTransform(self.robot,self.hand,self.transferPath[0],self.Tstart,q)
self.robot.setConfig(q)
self.object.setTransform(*Tobj)
else:
#index into the transfer path
i = i - (len(self.transitPath)-1) - (len(self.transferPath)-1)
q = vectorops.interpolate(self.retractPath[i],self.retractPath[i+1],s)
self.robot.setConfig(q)
self.object.setTransform(*self.Tgoal)
glutPostRedisplay()
def idle(self):
if self.transitPath and self.transferPath and self.retractPath:
#loop the path animation
n = len(self.transitPath) + len(self.transferPath) + len(
self.retractPath) - 2
u = (self.ttotal - self.animationTime) * 3.0
i = int(math.floor(u))
s = u - i
i = i % (n - 1)
if i + 1 < len(self.transitPath):
q = vectorops.interpolate(self.transitPath[i],
self.transitPath[i + 1], s)
#set the robot configuration for display
self.robot.setConfig(q)
self.object.setTransform(*self.Tstart)
elif i + 1 < len(self.transitPath) + len(self.transferPath) - 1:
#index into the transfer path
i = i - (len(self.transitPath) - 1)
q = vectorops.interpolate(self.transferPath[i],
self.transferPath[i + 1], s)
Tobj = graspedObjectTransform(self.robot, self.hand,
self.transferPath[0],
self.Tstart, q)
self.robot.setConfig(q)
self.object.setTransform(*Tobj)
else:
#index into the transfer path
i = i - (len(self.transitPath) - 1) - (len(self.transferPath) -
1)
q = vectorops.interpolate(self.retractPath[i],
self.retractPath[i + 1], s)
self.robot.setConfig(q)
self.object.setTransform(*self.Tgoal)
glutPostRedisplay()
def relevance(problem):
feasible = problem.concept()
d = vectorops.distance(problem.start, problem.goal)
h = 0.2
nsteps = int(math.ceil(d / h))
env_relevance = np.zeros((W, H))
#try including some adjacent features
#footprint = [(0,0),(-1,0),(1,0),(0,-1),(0,1)]
footprint = []
for i in range(nsteps + 1):
if nsteps == 0:
x = problem.start
else:
x = vectorops.interpolate(problem.start, problem.goal,
float(i) / nsteps)
x = (int(x[0]), int(x[1]))
if feasible or problem.env[x]:
env_relevance[x] = 1
for (dx, dy) in footprint:
if 0 <= x[0] + dx < W and 0 <= x[1] + dy < H:
env_relevance[x[0] + dx, x[1] + dy] = 1
if hierarchical:
features = [1] * 2 + [1] * 2
h = build_hierarchy(env_relevance)
for m in h:
features += [int(v) for v in m.flatten().tolist()]
return features
return [1] * 2 + [1] * 2 + [
int(v) for v in env_relevance.flatten().tolist()
]
def idle(self):
if self.running:
if time() - self.time_mark >= self.delay:
self.active_index += self.direction
self.time_mark = time()
if self.active_index <= 0:
# self.direction = 1
self.active_index = 0
# elif self.active_index >= len(self.plan)-1:
# #self.direction = -1
# self.active_index = len(self.plan)-1
elif self.active_index >= len(self.plan):
self.active_index = 0
logger.info("config {} of {}".format(self.active_index, len(self.plan) - 1))
if self.plan:
u = (time() - self.time_mark) / self.delay
if self.active_index + 1 < len(self.plan):
self.knowledge_base.robot_state.sensed_config = vectorops.interpolate(
self.plan[self.active_index], self.plan[self.active_index + 1], u
)
else:
self.knowledge_base.robot_state.sensed_config = self.plan[self.active_index]
glutPostRedisplay()
def idle(self):
if self.path:
#loop the path animation
u = (self.ttotal - self.animationTime)
i = int(math.floor(u))
s = u - i
i = i%(len(self.path)-1)
#set the robot configuration for display
q = vectorops.interpolate(self.path[i],self.path[i+1],s)
self.robot.setConfig(q)
glutPostRedisplay()
def idle(self):
if self.path:
#loop the path animation
u = (self.ttotal - self.animationTime)
i = int(math.floor(u))
s = u - i
i = i % (len(self.path) - 1)
#set the robot configuration for display
q = vectorops.interpolate(self.path[i], self.path[i + 1], s)
self.robot.setConfig(q)
glutPostRedisplay()
def idle(self):
if self.path:
#loop the path animation
u = (self.ttotal - self.animationTime)
i = int(math.floor(u))
s = u - i
i = i%(len(self.path)-1)
q = vectorops.interpolate(self.path[i],self.path[i+1],s)
#set the robot configuration for display
self.robot.setConfig(q)
#compute and set object transform for display
Tobj = graspedObjectTransform(self.robot,Hand('l'),self.qstart,self.Tstart,q)
self.object.setTransform(*Tobj)
glutPostRedisplay()
def idle(self):
if self.path:
#loop the path animation
u = (self.ttotal - self.animationTime)
i = int(math.floor(u))
s = u - i
i = i % (len(self.path) - 1)
q = vectorops.interpolate(self.path[i], self.path[i + 1], s)
#set the robot configuration for display
self.robot.setConfig(q)
#compute and set object transform for display
Tobj = graspedObjectTransform(self.robot, Hand('l'), self.qstart,
self.Tstart, q)
self.object.setTransform(*Tobj)
glutPostRedisplay()
def concept(problem): # compute y
d = vectorops.distance(problem.start, problem.goal)
h = 0.2
nsteps = int(math.ceil(d / h))
collisions = 0
#hacky "rasterization" by marching down segment
for i in range(nsteps + 1):
if nsteps == 0:
x = problem.start
else:
x = vectorops.interpolate(problem.start, problem.goal,
i / nsteps)
if sphere_collision(x, robot_radius, problem.env):
# colliding
return 0
#collision free
return 1
def relevance(problem):
feasible = problem.concept()
d = vectorops.distance(problem.start, problem.goal)
h = 0.2
nsteps = int(math.ceil(d / h))
env_relevance = np.zeros(workspace_size)
for i in range(nsteps + 1):
if nsteps == 0:
x = problem.start
else:
x = vectorops.interpolate(problem.start, problem.goal,
float(i) / nsteps)
for (a, b, c) in sphere_collision_tests(x, robot_radius,
problem.env):
if feasible or problem.env[(a, b, c)]:
env_relevance[(a, b, c)] = 1
return [1] * 3 + [1] * 3 + env_relevance.flatten().tolist()
def concept(problem):
d = vectorops.distance(problem.start, problem.goal)
h = 0.2
nsteps = int(math.ceil(d / h))
collisions = 0
#hacky "rasterization" by marching down segment
for i in range(nsteps + 1):
if nsteps == 0:
x = problem.start
else:
x = vectorops.interpolate(problem.start, problem.goal,
float(i) / nsteps)
if problem.env[int(x[0]), int(x[1])]:
collisions += 1
if collisions >= label_obstacle_threshold:
return 0
#collision free
return 1
def eval(self,u):
return vectorops.interpolate(self.a,self.b,u)
def eval(self, u):
return vectorops.interpolate(self.a, self.b, u)
def interpolate(self,a,b,u):
return vectorops.interpolate(a,b,u)
