class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.herb = herb
self.robot = herb.robot
self.boundary_limits = [[-5., -5., -numpy.pi], [5., 5., numpy.pi]]
lower_limits, upper_limits = self.boundary_limits
self.discrete_env = DiscreteEnvironment(resolution, lower_limits,
upper_limits)
self.resolution = resolution
self.ConstructActions()
def GenerateFootprintFromControl(self,
start_config,
control,
stepsize=0.01):
# Extract the elements of the control
ul = control.ul
ur = control.ur
dt = control.dt
# Initialize the footprint
config = start_config.copy()
footprint = [numpy.array([0., 0., config[2]])]
timecount = 0.0
while timecount < dt:
# Generate the velocities based on the forward model
xdot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.cos(
config[2])
ydot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.sin(
config[2])
tdot = self.herb.wheel_radius * (ul -
ur) / self.herb.wheel_distance
# Feed forward the velocities
if timecount + stepsize > dt:
stepsize = dt - timecount
config = config + stepsize * numpy.array([xdot, ydot, tdot])
if config[2] > numpy.pi:
config[2] -= 2. * numpy.pi
if config[2] < -numpy.pi:
config[2] += 2. * numpy.pi
footprint_config = config.copy()
footprint_config[:2] -= start_config[:2]
footprint.append(footprint_config)
timecount += stepsize
# Add one more config that snaps the last point in the footprint to the center of the cell
nid = self.discrete_env.ConfigurationToNodeId(config)
snapped_config = self.discrete_env.NodeIdToConfiguration(nid)
snapped_config[:2] -= start_config[:2]
footprint.append(snapped_config)
return footprint
def PlotActionFootprints(self, idx):
actions = self.actions[idx]
fig = pl.figure()
lower_limits, upper_limits = self.boundary_limits
pl.xlim([lower_limits[0], upper_limits[0]])
pl.ylim([lower_limits[1], upper_limits[1]])
for action in actions:
xpoints = [config[0] for config in action.footprint]
ypoints = [config[1] for config in action.footprint]
pl.plot(xpoints, ypoints, 'k')
pl.ion()
pl.show()
def ConstructActions(self):
# Actions is a dictionary that maps orientation of the robot to
# an action set
self.actions = dict()
wc = [0., 0., 0.]
grid_coordinate = self.discrete_env.ConfigurationToGridCoord(wc)
# Iterate through each possible starting orientation
for idx in range(int(self.discrete_env.num_cells[2])):
self.actions[idx] = []
grid_coordinate[2] = idx
start_config = self.discrete_env.GridCoordToConfiguration(
grid_coordinate)
# TODO: Here you will construct a set of actions
# to be used during the planning process
#
wl_max = 1
wr_max = 1
t_l = 2
t_s = 1
t_semi = 5 * math.pi / 8
t_cur = 5 * math.pi / 2
#turn left in place
C_1 = Control(-wl_max, wr_max, t_semi)
A_1 = Action(C_1, GenerateFootprintFromControl(start_config, C_1))
#Turn right in place
C_2 = Control(wl_max, -wr_max, t_semi)
A_2 = Action(C_2, GenerateFootprintFromControl(start_config, C_2))
#go front long
C_3 = Control(wl_max, wr_max, t_l)
A_3 = Action(C_3, GenerateFootprintFromControl(start_config, C_3))
# GO Back long
C_4 = Control(-wl_max, -wr_max, t_l)
A_4 = Action(C_4, GenerateFootprintFromControl(start_config, C_4))
# go front small
C_5 = Control(wl_max, wr_max, t_s)
A_5 = Action(C_5, GenerateFootprintFromControl(start_config, C_5))
# go back small
C_6 = Control(-wl_max, -wr_max, t_s)
A_6 = Action(C_6, GenerateFootprintFromControl(start_config, C_6))
# curve left
C_7 = Control(0.5 * wl_max, wr_max, t_cur)
A_7 = Action(C_7, GenerateFootprintFromControl(start_config, C_7))
# curve right
C_8 = Control(wl_max, 0.5 * wr_max, t_cur)
A_8 = Action(C_8, GenerateFootprintFromControl(start_config, C_8))
# curve back left
C_9 = Control(-0.5 * wl_max, -wr_max, t_cur)
A_9 = Action(C_9, GenerateFootprintFromControl(start_config, C_9))
# curve back right
C_10 = Control(-wl_max, -0.5 * wr_max, t_cur)
A_10 = Action(C_10,
GenerateFootprintFromControl(start_config, C_10))
self.Actions[idx] = [
A_1, A_2, A_3, A_4, A_5, A_6, A_7, A_8, A_9, A_10
]
PlotActionFootprints(0)
def GetSuccessors(self, node_id):
successors = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids and controls that represent the neighboring
# nodes
return successors
def ComputeDistance(self, start_id, end_id):
dist = 0
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
return cost
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.herb = herb
self.robot = herb.robot
self.boundary_limits = [[-5., -5., -numpy.pi], [5., 5., numpy.pi]]
lower_limits, upper_limits = self.boundary_limits
self.discrete_env = DiscreteEnvironment(resolution, lower_limits,
upper_limits)
self.resolution = resolution
self.ConstructActions()
def GenerateFootprintFromControl(self,
start_config,
control,
stepsize=0.01):
# Extract the elements of the control
ul = control.ul
ur = control.ur
dt = control.dt
start_config = numpy.array(start_config)
# Initialize the footprint
config = start_config.copy()
footprint = [numpy.array([0., 0., config[2]])]
timecount = 0.0
while timecount < dt:
# Generate the velocities based on the forward model
xdot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.cos(
config[2])
ydot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.sin(
config[2])
tdot = self.herb.wheel_radius * (-ul +
ur) / self.herb.wheel_distance
# Feed forward the velocities
if timecount + stepsize > dt:
stepsize = dt - timecount
config = config + stepsize * numpy.array([xdot, ydot, tdot])
if config[2] > numpy.pi:
config[2] -= 2. * numpy.pi
if config[2] < -numpy.pi:
config[2] += 2. * numpy.pi
footprint_config = config.copy()
footprint_config[:2] -= start_config[:2]
footprint.append(footprint_config)
timecount += stepsize
# Add one more config that snaps the last point in the footprint to the center of the cell
nid = self.discrete_env.ConfigurationToNodeId(config)
snapped_config = self.discrete_env.NodeIdToConfiguration(nid)
snapped_config[:2] -= start_config[:2]
footprint.append(snapped_config)
# print "Last footprint config = ", config
return footprint
def PlotActionFootprints(self, idx):
actions = self.actions[idx]
fig = pl.figure(1)
lower_limits, upper_limits = self.boundary_limits
pl.xlim([lower_limits[0], upper_limits[0]])
pl.ylim([lower_limits[1], upper_limits[1]])
i = 0
for action in actions:
xpoints = [config[0] for config in action.footprint]
ypoints = [config[1] for config in action.footprint]
if (i == 0):
pl.plot(xpoints, ypoints, 'k')
if (i == 1):
pl.plot(xpoints, ypoints, 'r')
if (i == 2):
pl.plot(xpoints, ypoints, 'y')
pl.plot(xpoints, ypoints, 'b')
i += 1
pl.ion()
pl.show()
def ConstructActions(self):
print "Constructing actions"
# Actions is a dictionary that maps orientation of the robot to
# an action set
self.actions = dict()
wc = [0., 0., 0.]
grid_coordinate = self.discrete_env.ConfigurationToGridCoord(wc)
# Iterate through each possible starting orientation
for idx in range(int(self.discrete_env.num_cells[2])):
self.actions[idx] = []
grid_coordinate[2] = idx
start_config = self.discrete_env.GridCoordToConfiguration(
grid_coordinate)
# TODO: Here you will construct a set of actions
# to be used during the planning process
#
# Control set for grid resolution 0.1
pi = numpy.pi
r = self.herb.wheel_radius
L = self.herb.wheel_distance
# theta = [0.75*pi, 0.5*pi, 0.25*pi, -0.25*pi, -0.5*pi, -0.75*pi]
# point_rot = [[-1,1,abs(0.5*th*L/r)] for th in theta[0:3]] + [[1,-1,abs(0.5*th*L/r)] for th in theta[3:]]
# theta = numpy.array([7*pi/8, 6*pi/8, 5*pi/8, 4*pi/8, 3*pi/8, 2*pi/8, 1*pi/8, -1*pi/8, -2*pi/8, -3*pi/8, -4*pi/8, -5*pi/8, -6*pi/8, -7*pi/8])
# point_rot = []
# point_rot = [[-1,1,abs(0.5*th*L/r)] for th in theta[0:7]] + [[1,-1,abs(0.5*th*L/r)] for th in theta[7:]]
point_rot = [[-1, 1, abs(0.5 * (pi / 8) * L / r)],
[-1, 1, abs(0.5 * (pi / 8) * L / r)]]
control_set = numpy.array([[1, 1, 0.5]] + point_rot)
# control_set = numpy.array([[1, 1, 1],[1, 1, 0.5], [0, 1, (pi/4)*L/r], [1, 0, (pi/4)*L/r]] + point_rot)
for c in control_set:
ctrl = Control(c[0], c[1], c[2])
footprint = self.GenerateFootprintFromControl(
start_config, ctrl, 0.001)
print "idx", idx, "Footprint: ", footprint[0], footprint[-1]
act = Action(ctrl, footprint)
self.actions[idx].append(act)
self.PlotActionFootprints(idx)
def GetSuccessors(self, node_id):
successors = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids and controls that represent the neighboring
# nodes
successor_actions = dict()
parent_config = numpy.array(
self.discrete_env.NodeIdToConfiguration(node_id))
parent_coord = self.discrete_env.NodeIdToGridCoord(node_id)
# print parent_coord
possible_actions = self.actions[parent_coord[2]]
for act in possible_actions:
fp = act.footprint
# child_node_id = self.discrete_env.ConfigurationToNodeId(fp[-1])
child_node_id = self.discrete_env.ConfigurationToNodeId(
numpy.append(parent_config[:2], 0) + fp[-1])
with self.robot:
config = self.discrete_env.NodeIdToConfiguration(child_node_id)
self.herb.SetCurrentConfiguration(config)
if (self.robot.GetEnv().CheckCollision(self.robot)) is False:
successors.append(child_node_id)
successor_actions[child_node_id] = (node_id, act
) # Parent-Action pair
return successors, successor_actions
def ComputeDistance(self, start_id, end_id):
dist = 0
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
coordStart = numpy.array(
self.discrete_env.NodeIdToConfiguration(start_id))
coordEnd = numpy.array(self.discrete_env.NodeIdToConfiguration(end_id))
dist = numpy.linalg.norm(coordEnd[:2] - coordStart[:2])
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
coordStart = numpy.array(
self.discrete_env.NodeIdToConfiguration(start_id))
coordEnd = numpy.array(
self.discrete_env.NodeIdToConfiguration(goal_id))
d = numpy.linalg.norm(coordEnd[:2] - coordStart[:2])
thetadiff = abs(coordStart[2] - coordEnd[2])
if d > 0.5:
cost = d
else:
cost = d + thetadiff * 0.01
return cost
class AStarPlanner(object):
def __init__(self, planning_env, visualize):
self.planning_env = planning_env
self.visualize = visualize
self.discrete_env = DiscreteEnvironment(self.planning_env.resolution,self.planning_env.lower_limits, self.planning_env.upper_limits)
def Plan(self, start_config, goal_config):
print "Searching..."
starttime = time.clock()
pointPlan = []
plan = []
actionlist = {}
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
Queue= []
visitedQueue=[]
dictionary={}
costQueue=[]
startPoint=self.discrete_env.ConfigurationToNodeId(start_config)
goalPoint=self.discrete_env.ConfigurationToNodeId(goal_config)
#print goalPoint
Queue.append(startPoint)
costQueue.append([self.planning_env.ComputeHeuristicCost(startPoint, goalPoint),startPoint])
costQueueElement=costQueue.pop(0)
visitedQueue.append(costQueueElement[1])
costQueue.append([0,startPoint])
#print costQueueElement[1]
while costQueueElement[1]!=goalPoint:
if len(Queue)==0:
plan=[]
return plan
successor=self.planning_env.GetSuccessors(costQueueElement[1])
for i in successor:
if self.visualize:
self.planning_env.PlotEdge2(self.discrete_env.NodeIdToConfiguration(i), self.discrete_env.NodeIdToConfiguration(costQueueElement[1]), "k", 1.5)
actionlist[i] = successor[i]
if i not in visitedQueue and i not in Queue:
Queue.append(i)
dictionary[i]=costQueueElement[1]
costG=0
X=i
while X!=startPoint:
costG=costG+self.planning_env.ComputeDistance(X, dictionary[X])
X=dictionary[X]
heurPlusG=self.planning_env.ComputeHeuristicCost(i, goalPoint) + costG
costQueue.append([heurPlusG,i])
#print costQueue
costQueue=sorted(costQueue)
costQueueElement=costQueue.pop(0)
Queue.remove(costQueueElement[1])
visitedQueue.append(costQueueElement[1])
if self.visualize:
pl.draw()
duration = time.clock() - starttime
print "Goal Found"
point=goalPoint
pathlength = 0
while point!=startPoint:
#print point
pointPlan.append(self.discrete_env.NodeIdToConfiguration(point))
pathlength = pathlength + self.planning_env.ComputeDistance(point, dictionary[point])
point=dictionary[point]
pointPlan.append(start_config)
pointPlan.reverse()
#print pointPlan
for i in pointPlan[1:]:
#print i
idx = self.discrete_env.ConfigurationToNodeId(i)
print idx
plan.append(actionlist[idx])
#print i
#print successor[i]
#print plan
print "Time =", duration, "s"
print "Nodes popped =", len(visitedQueue)
print "Path length =", pathlength, "m"
return plan
class HerbEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits, self.upper_limits = self.robot.GetActiveDOFLimits()
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits, self.upper_limits)
# account for the fact that snapping to the middle of the grid cell may put us over our
# upper limit
#
#Peter: we have deal with the close to upper limit in DiscreteEnviroment.py/GridCoordToConfiguration
#so this line will cause confusion
#upper_coord = [x - 1 for x in self.discrete_env.num_cells]
#upper_config = self.discrete_env.GridCoordToConfiguration(upper_coord)
#for idx in range(len(upper_config)):
# self.discrete_env.num_cells[idx] -= 1
# add a table and move the robot into place
table = self.robot.GetEnv().ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = numpy.array([[ 0, 0, -1, 0.7],
[-1, 0, 0, 0],
[ 0, 1, 0, 0],
[ 0, 0, 0, 1]])
table.SetTransform(table_pose)
# set the camera
camera_pose = numpy.array([[ 0.3259757 , 0.31990565, -0.88960678, 2.84039211],
[ 0.94516159, -0.0901412 , 0.31391738, -0.87847549],
[ 0.02023372, -0.9431516 , -0.33174637, 1.61502194],
[ 0. , 0. , 0. , 1. ]])
self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
def GetSuccessors(self, node_id):
successors = []
node_coord = numpy.array(self.discrete_env.NodeIdToGridCoord(node_id))
#for each direction there are two type neighbor
for idx in range(self.discrete_env.dimension):
#positive direction
if( (node_coord[idx] + 1) <= (self.discrete_env.num_cells[idx]-1) ):
n_node_coord = numpy.copy(node_coord)
n_node_coord[idx] += 1
n_node_config = self.discrete_env.GridCoordToConfiguration(n_node_coord)
if(self.no_collision(n_node_config)):
n_node_id = self.discrete_env.GridCoordToNodeId(n_node_coord)
successors.append(n_node_id)
#negation direction
if( (node_coord[idx] - 1) >= 0 ):
n_node_coord = numpy.copy(node_coord)
n_node_coord[idx] -= 1
n_node_config = self.discrete_env.GridCoordToConfiguration(n_node_coord)
if(self.no_collision(n_node_config)):
n_node_id = self.discrete_env.GridCoordToNodeId(n_node_coord)
successors.append(n_node_id)
return successors
def ComputeDistance(self, start_id, end_id):
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(end_id)
#calculate distance
dist = numpy.linalg.norm(numpy.array(start_config) - numpy.array(end_config))
return dist
def ComputeDistance_continue(self, start_id, end_id):
dist = numpy.linalg.norm(numpy.array(start_id) - numpy.array(end_id))
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by te two node ids
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
cost = self.ComputeDistance(start_id,goal_id)
return cost
def no_collision(self, n_config):
with self.robot:
#change the current position valuse wiht n_config
self.robot.SetActiveDOFValues(numpy.array(n_config))
#move robot to new positi
#print "checkcollision = ",self.robot.GetEnv().CheckCollision(self.robot)
#print "selfcheck= ",self.robot.CheckSelfCollision() i
flag = self.robot.GetEnv().CheckCollision(self.robot) or self.robot.CheckSelfCollision()
flag = not flag
return flag
def ComputePathLength(self, path):
path_length = 0
for milestone in range(1,len(path)):
dist = numpy.linalg.norm(numpy.array(path[milestone-1])-numpy.array(path[milestone]))
path_length = path_length + dist
return path_length
def GenerateRandomConfiguration(self):
config = [0] * len(self.robot.GetActiveDOFIndices())
#
# TODO: Generate and return a random configuration
#
# Brad: Generate and return a random, collision-free, configuration
lower_limits, upper_limits = self.robot.GetActiveDOFLimits()
collisionFlag = True
while collisionFlag is True:
for dof in range(len(self.robot.GetActiveDOFIndices())):
config[dof] = lower_limits[dof] + (upper_limits[dof] - lower_limits[dof]) * numpy.random.random_sample()
self.robot.SetActiveDOFValues(numpy.array(config))
if(self.robot.GetEnv().CheckCollision(self.robot)) is False:
if(self.robot.CheckSelfCollision()) is False:
collisionFlag = False
#else:
#print "Self Collision detected in random configuration"
#else:
#print "Collision Detected in random configuration"
return numpy.array(config)
def Extend(self, start_config, end_config):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
# Brad: Extend from start to end configuration and return sooner if collision or limits exceeded
lower_limits, upper_limits = self.robot.GetActiveDOFLimits()
resolution = 100
#Calculate incremental configuration changes
config_inc = [0] * len(self.robot.GetActiveDOFIndices())
for dof in range(len(self.robot.GetActiveDOFIndices())):
config_inc[dof] = (end_config[dof] - start_config[dof]) / float(resolution)
#Set initial config state to None to return if start_config violates conditions
config = None
#Move from start_config to end_config
for step in range(resolution+1):
prev_config = config
config = [0] * len(self.robot.GetActiveDOFIndices())
for dof in range(len(self.robot.GetActiveDOFIndices())):
#Calculate new config
config[dof] = start_config[dof] + config_inc[dof]*float(step)
#Check joint limits
if config[dof] > upper_limits[dof]:
print "Upper joint limit exceeded"
return prev_config
if config[dof] < lower_limits[dof]:
print "Lower joint limit exceeded"
return prev_config
#Set config and check for collision
#CHECK: Lock environment?
self.robot.SetActiveDOFValues(numpy.array(config))
if(self.robot.GetEnv().CheckCollision(self.robot)) is True:
#print "Collision Detected in extend"
return prev_config
if(self.robot.CheckSelfCollision()) is True:
#print "Self Collision Detected in extend"
return prev_config
return end_config
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.herb = herb
self.robot = herb.robot
self.boundary_limits = [[-5., -5., -numpy.pi], [5., 5., numpy.pi]]
self.lower_limits, self.upper_limits = self.boundary_limits
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits, self.upper_limits)
self.resolution = resolution
self.ConstructActions()
def GenerateFootprintFromControl(self, start_config, control, stepsize=0.01):
# Extract the elements of the control
ul = control.ul
ur = control.ur
dt = control.dt
# Initialize the footprint
config = start_config.copy()
footprint = [numpy.array([0., 0., config[2]])]
timecount = 0.0
while timecount < dt:
# Generate the velocities based on the forward model
xdot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.cos(config[2])
ydot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.sin(config[2])
tdot = self.herb.wheel_radius * (ul - ur) / self.herb.wheel_distance
# Feed forward the velocities
if timecount + stepsize > dt:
stepsize = dt - timecount
config = config + stepsize*numpy.array([xdot, ydot, tdot])
if config[2] > numpy.pi:
config[2] -= 2.*numpy.pi
if config[2] < -numpy.pi:
config[2] += 2.*numpy.pi
footprint_config = config.copy()
footprint_config[:2] -= start_config[:2]
footprint.append(footprint_config)
timecount += stepsize
# Add one more config that snaps the last point in the footprint to the center of the cell
nid = self.discrete_env.ConfigurationToNodeId(config)
snapped_config = self.discrete_env.NodeIdToConfiguration(nid)
snapped_config[:2] -= start_config[:2]
footprint.append(snapped_config)
return footprint
def PlotActionFootprints(self, idx):
actions = self.actions[idx]
fig = pl.figure()
lower_limits, upper_limits = self.boundary_limits
pl.xlim([lower_limits[0], upper_limits[0]])
pl.ylim([lower_limits[1], upper_limits[1]])
for action in actions:
xpoints = [config[0] for config in action.footprint]
ypoints = [config[1] for config in action.footprint]
pl.plot(xpoints, ypoints, 'k')
pl.ion()
pl.show()
def ConstructActions(self):
# Actions is a dictionary that maps orientation of the robot to
# an action set
self.actions = dict()
self.controlList = []
self.controlTest = Control(0,0,0)
wc = [0., 0., 0.]
grid_coordinate = self.discrete_env.ConfigurationToGridCoord(wc)
# Set control list
omega_list_l = numpy.arange(-1,1.5,0.5) #0 0.5 1.0
omega_list_r = numpy.arange(-1,1.5,0.5) #0 0.5 1.0
duration_list = numpy.arange(0,3.1,0.1) #2
"""
for omega_l in omega_list:
for omega_r in omega_list:
for dt in duration_list:
#print str(omega_l) + " " + str(omega_r) + " " + str(dt)
self.controlList.append(Control(omega_l,omega_r,dt))
"""
for omega_l in omega_list_l:
for omega_r in omega_list_r:
for dt in duration_list:
self.controlList.append(Control(omega_l,omega_r,dt))
#self.controlList.append(Control(1.0,1.0,0.5))
# Iterate through each possible starting orientation
for idx in range(int(self.discrete_env.num_cells[2])):
self.actions[idx] = []
grid_coordinate[2] = idx
start_config = self.discrete_env.GridCoordToConfiguration(grid_coordinate)
# TODO: Here you will construct a set of actions
# to be used during the planning process
#
for control in self.controlList:
footprint = self.GenerateFootprintFromControl(start_config, control)
self.actions[idx].append(Action(control,footprint))
def GetSuccessors(self, node_id):
self.successors = {}
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids and controls that represent the neighboring
# nodes
curr_coord = self.discrete_env.NodeIdToGridCoord(node_id)
curr_config = self.discrete_env.NodeIdToConfiguration(node_id)
if not self.BoundaryCheck(curr_config) or self.CollisionCheck(curr_config):
return self.successors
#need to modify
#omega_index = numpy.floor(((curr_config[2] - -numpy.pi)/(2*numpy.pi))*self.discrete_env.num_cells[2])
omega_index = 0
if omega_index == self.discrete_env.num_cells[2]:
omega_index = 0
curr_config[2] = (self.discrete_env.resolution[2] * omega_index) - numpy.pi
print "node_id = " + str(node_id)
print "curr_config = " + str(curr_config)
print "curr_coord = " + str(curr_coord[2])
test_config = [0,0,0]
for action in self.actions[curr_coord[2]]:
test_config[0] = curr_config[0] + action.footprint[-1][0] #test final point
test_config[1] = curr_config[1] + action.footprint[-1][1]
test_config[2] = action.footprint[-1][2]
print "test_config = " + str(test_config)
if not self.CollisionCheck(test_config) and self.BoundaryCheck(test_config):
index = self.discrete_env.ConfigurationToNodeId(test_config)
print "index = " + str(index)
self.successors[index] = action
return self.successors
def CollisionCheck(self,config):
#config = self.discrete_env.GridCoordToConfiguration(coord)
current_pose = self.robot.GetTransform()
checking_pose = current_pose.copy()
env = self.robot.GetEnv()
checking_pose[0:3,3] = numpy.array([config[0], config[1], 0.0])
angle = config[2]
rotation = numpy.array([[numpy.cos(angle), -numpy.sin(angle),0.0],[numpy.sin(angle),numpy.cos(angle),0.0],[0.0,0.0,1.0]])
checking_pose[0:3,0:3] = rotation
with env:
self.robot.SetTransform(checking_pose)
isCollision = env.CheckCollision(self.robot)
with env:
self.robot.SetTransform(current_pose)
return isCollision
def BoundaryCheck(self, config):
#config = self.discrete_env.GridCoordToConfiguration(coord)
if config[0] < self.lower_limits[0] or config[1] < self.lower_limits[1] or config[2] < self.lower_limits[2]:
return False
if config[0] > self.upper_limits[0] or config[1] > self.upper_limits[1] or config[2] > self.upper_limits[2]:
return False
return True
def ComputeDistance(self, start_id, end_id):
dist = 0
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(end_id)
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
dist_xy = numpy.linalg.norm(start_config[0:2]-end_config[0:2])
dist_w = numpy.linalg.norm(start_config[2]-end_config[2])
dist = dist_xy + 2*dist_w
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(goal_id)
dist = numpy.linalg.norm(start_config[0:2]-end_config[0:2])
orientation = numpy.linalg.norm(start_config[-1]-end_config[-1])
print "========================================="
#print "start_config = " + str(start_config)
#print "end_config = " + str(end_config)
print "dist cost = " + str(dist)
print "orientation cost = " + str(orientation)
cost = 10*dist + orientation
#cost = self.ComputeDistance(start_id,goal_id)
return cost
class HerbEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits, self.upper_limits = self.robot.GetActiveDOFLimits()
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
# account for the fact that snapping to the middle of the grid cell may put us over our
# upper limit
upper_coord = numpy.array([x - 1 for x in self.discrete_env.num_cells])
upper_config = self.discrete_env.GridCoordToConfiguration(upper_coord)
for idx in range(len(upper_config)):
self.discrete_env.num_cells[idx] -= 1
# add a table and move the robot into place
table = self.robot.GetEnv().ReadKinBodyXMLFile(
'models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = numpy.array([[0, 0, -1, 0.7], [-1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 0, 1]])
table.SetTransform(table_pose)
# set the camera
camera_pose = numpy.array(
[[0.3259757, 0.31990565, -0.88960678, 2.84039211],
[0.94516159, -0.0901412, 0.31391738, -0.87847549],
[0.02023372, -0.9431516, -0.33174637, 1.61502194],
[0., 0., 0., 1.]])
self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
def GetSuccessors(self, node_id):
successors = []
config = self.discrete_env.NodeIdToGridCoord(node_id)
for i in range(0, self.discrete_env.dimension):
config[i] += 1
if config[i] < self.discrete_env.num_cells[i]:
successors.append(self.discrete_env.GridCoordToNodeId(config))
config[i] -= 2
if config[i] >= 0:
successors.append(self.discrete_env.GridCoordToNodeId(config))
config[i] += 1
return successors
def GetValidSuccessors(self, node_id):
successors = self.GetSuccessors(self, node_id)
successors = [
x for x in successors
if self.ComputeDistance(node_id, x) != float("inf")
]
return successors
def ComputeDistanceBetweenIds(self, start_id, end_id):
epsilon = 0.01
start = self.discrete_env.NodeIdToConfiguration(start_id)
end = self.discrete_env.NodeIdToConfiguration(end_id)
with self.robot:
cfg = self.robot.GetActiveDOFValues()
vec = end - start
veclen = numpy.linalg.norm(vec)
vec = vec / veclen
travel = 0
while travel < veclen:
val = start + vec * travel
self.robot.SetActiveDOFValues(val)
if self.robot.GetEnv().CheckCollision(
self.robot) or self.robot.CheckSelfCollision():
return float("inf")
travel += epsilon
return self.ComputeHeuristicCost(start_id, end_id)
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
start = numpy.array(self.discrete_env.NodeIdToConfiguration(start_id))
end = numpy.array(self.discrete_env.NodeIdToConfiguration(goal_id))
return numpy.linalg.norm(end - start)
def SetGoalParameters(self, goal_config, p=0.05):
self.goal_config = goal_config
self.p = p
def GenerateRandomConfiguration(self):
config = [0] * len(self.robot.GetActiveDOFIndices())
lower_limits, upper_limits = self.robot.GetActiveDOFLimits()
import numpy
lower_limits = numpy.array(lower_limits)
upper_limits = numpy.array(upper_limits)
# Generate random configuration
choice = numpy.random.rand(1)
if choice < self.p:
config = self.goal_config
else:
COLLISION = True
while COLLISION:
config = numpy.random.rand(
len(self.robot.GetActiveDOFIndices())) * (
upper_limits - lower_limits) + lower_limits
# Check if it is collision free
with self.robot:
robot_pos = self.robot.GetActiveDOFValues()
robot_pos = config
self.robot.SetActiveDOFValues(robot_pos)
if (self.robot.GetEnv().CheckCollision(self.robot)
or self.robot.CheckSelfCollision()) == False:
COLLISION = False
return self.discrete_env.NodeIdToConfiguration(
self.discrete_env.ConfigurationToNodeId(config))
def ComputeDistance(self, start_config, end_config):
return numpy.linalg.norm(end_config - start_config)
def Extend(self, start_config, end_config):
epsilon = .01
dist = self.ComputeDistance(start_config, end_config)
numSteps = math.ceil(dist / epsilon)
step = (end_config - start_config) / numSteps
best_config = None
i = 1
while i <= numSteps:
cur_config = start_config + step * i
# Check if it is collision free
with self.robot:
robot_pos = self.robot.GetActiveDOFValues()
robot_pos = cur_config
self.robot.SetActiveDOFValues(robot_pos)
## should we also check self.robot.CheckSelfCollision?
if self.robot.GetEnv().CheckCollision(self.robot):
return best_config
#update variables
best_config = cur_config
i += 1
return end_config
def ShortenPath(self, path, timeout=5.0):
#print('starting path shortening')
start_time = time.time()
current_time = 0
i = 0
totalDistance = 0
#TODO: Calculate current path distance
for i in range(0, len(path) - 1):
curDistance = self.ComputeDistance(path[i], path[i + 1])
totalDistance += curDistance
print(totalDistance)
i = 0
while current_time < timeout and i == 0:
#print("NOW IN THE FUNCTION LOOP")
pathLength = len(path)
p1index = random.randint(0, pathLength - 2)
p2index = p1index
while p2index == p1index or abs(p2index - p1index) <= 1:
p2index = random.randint(0, pathLength - 2)
if p1index > p2index:
p1index, p2index = p2index, p1index
p1a = numpy.array(path[p1index])
p1b = numpy.array(path[p1index + 1])
p2a = numpy.array(path[p2index])
p2b = numpy.array(path[p2index + 1])
#choose random interpolation between points
p1Vec = (p1b - p1a)
p2Vec = (p2b - p2a)
p1 = p1a + (p1Vec * random.random())
p2 = p2a + (p2Vec * random.random())
#TODO use extend to move to the two random points
extender = self.Extend(p1, p2)
if extender is not None:
if self.ComputeDistance(extender, p2) < .01:
#TODO get new path
#print('shortening path now')
badIndeces = [p1index + 1, p2index]
if (badIndeces[1] - badIndeces[0]) != 0:
path[badIndeces[0]:badIndeces[1]] = []
else:
path[badIndeces[0]] = []
path[badIndeces[0]] = p1
path.insert(badIndeces[0] + 1, p2)
#print('new path created')
# repeate until timeout
current_time = time.time() - start_time
i = 0
totalDistance = 0
#TODO: Calculate current path distance
for i in range(0, len(path) - 1):
curDistance = self.ComputeDistance(path[i], path[i + 1])
totalDistance += curDistance
print(totalDistance)
return path
class HerbEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits, self.upper_limits = self.robot.GetActiveDOFLimits()
self.resolution = resolution
self.discrete_env = DiscreteEnvironment(self.resolution,
self.lower_limits,
self.upper_limits)
# account for the fact that snapping to the middle of the grid cell may put us over our
# upper limit
upper_coord = [x - 1 for x in self.discrete_env.num_cells]
upper_config = self.discrete_env.GridCoordToConfiguration(upper_coord)
for idx in range(len(upper_config)):
self.discrete_env.num_cells[idx] -= 1
# add a table and move the robot into place
table = self.robot.GetEnv().ReadKinBodyXMLFile(
'models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = np.array([[0, 0, -1, 0.7], [-1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 0, 1]])
table.SetTransform(table_pose)
# set the camera
camera_pose = np.array(
[[0.3259757, 0.31990565, -0.88960678, 2.84039211],
[0.94516159, -0.0901412, 0.31391738, -0.87847549],
[0.02023372, -0.9431516, -0.33174637, 1.61502194],
[0., 0., 0., 1.]])
self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
def checkSucc(self, config):
self.env = self.robot.GetEnv()
robot_pose = self.robot.GetTransform()
table = self.robot.GetEnv().GetBodies()[1]
config = config.tolist()
self.robot.SetActiveDOFValues(config)
if self.env.CheckCollision(self.robot, table):
return False
for i in range(self.discrete_env.dimension):
if not (self.lower_limits[i] <= config[i] <= self.upper_limits[i]):
return False
return True
def GetSuccessors(self, node_id):
successors = []
successors_config = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
config = self.discrete_env.NodeIdToConfiguration(node_id)
for i in range(2 * self.discrete_env.dimension):
prim = np.zeros(self.discrete_env.dimension)
prim[i / 2] = self.resolution
if np.mod(i, 2):
succ = np.asarray(config) + prim
else:
succ = np.asarray(config) - prim
if self.checkSucc(succ):
successors_config.append(succ)
else:
continue
successors = [
self.discrete_env.ConfigurationToNodeId(x)
for x in successors_config
]
return successors
def ComputeDistance(self, start_id, end_id):
dist = 0
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(end_id)
dist = np.linalg.norm(end_config - start_config)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
# Keeping it as Euclidean distance for now
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(goal_id)
cost = np.linalg.norm(end_config - start_config)
return cost
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits = [-5., -5.]
self.upper_limits = [5., 5.]
self.boundary_limits = [[-5., -5.], [5., 5.]]
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
# add an obstacle
table = self.robot.GetEnv().ReadKinBodyXMLFile(
'models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = numpy.array([[0, 0, -1, 1.5], [-1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 0, 1]])
table.SetTransform(table_pose)
def GetSuccessors(self, node_id):
successors = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
coord = numpy.array(self.discrete_env.NodeIdToGridCoord(node_id))
#print config
idx = 0
for i in range(len(coord)):
if coord[i] + 1 < self.discrete_env.num_cells[i]:
this_coord = numpy.copy(coord)
this_coord[i] += 1
this_id = self.discrete_env.GridCoordToNodeId(this_coord)
this_config = self.discrete_env.NodeIdToConfiguration(this_id)
if self.no_collision(this_config):
successors.append(this_id)
if (coord[i] - 1) >= 0:
this_coord = numpy.copy(coord)
this_coord[i] -= 1
this_id = self.discrete_env.GridCoordToNodeId(this_coord)
this_config = self.discrete_env.NodeIdToConfiguration(this_id)
if self.no_collision(this_config):
successors.append(this_id)
#print "number of successors = ",len(successors)
return successors
def ComputeDistance(self, start_id, end_id):
dist = numpy.linalg.norm(
numpy.array(self.discrete_env.NodeIdToConfiguration(start_id)) -
numpy.array(self.discrete_env.NodeIdToConfiguration(end_id)))
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by te two node ids
return dist
def ComputeDistance_continue(self, start_id, end_id):
dist = numpy.linalg.norm(numpy.array(start_id) - numpy.array(end_id))
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by te two node ids
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
# cost = self.ComputeDistance(start_id,goal_id)
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
goal_config = self.discrete_env.NodeIdToConfiguration(goal_id)
cost = 0
for i in range(self.discrete_env.dimension):
cost = cost + abs(start_config[i] - goal_config[i])
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
return cost
def InitializePlot(self, goal_config):
self.fig = pl.figure()
pl.xlim([self.lower_limits[0], self.upper_limits[0]])
pl.ylim([self.lower_limits[1], self.upper_limits[1]])
pl.plot(goal_config[0], goal_config[1], 'gx')
# Show all obstacles in environment
for b in self.robot.GetEnv().GetBodies():
if b.GetName() == self.robot.GetName():
continue
bb = b.ComputeAABB()
pl.plot([
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] - bb.extents()[0]
], [
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] - bb.extents()[1]
], 'r')
pl.ion()
pl.show()
def PlotEdge(self, sconfig, econfig):
pl.plot([sconfig[0], econfig[0]], [sconfig[1], econfig[1]],
'k.-',
linewidth=2.5)
pl.draw()
#help function for checking collision
def no_collision(self, n_config):
with self.robot:
position = self.robot.GetTransform()
#change the current position valuse wiht n_config
position[0][3] = n_config[0]
position[1][3] = n_config[1]
#move robot to new position
self.robot.SetTransform(position)
flag = self.robot.GetEnv().CheckCollision(self.robot)
flag = not flag
return flag
#help function
def ComputePathLength(self, path):
length = 0
for milestone in range(1, len(path)):
dist = numpy.linalg.norm(
numpy.array(path[milestone - 1]) -
numpy.array(path[milestone]))
length += dist
return length
def Extend(self, start_config, end_config):
with self.robot:
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#The function should return
#either None or a configuration such that the linear interpolation between the start configuration and
#this configuration is collision free and remains inside the environment boundaries.
#self.boundary_limits = [[-5., -5.], [5., 5.]]
collision = False
outside = False
lower_limits, upper_limits = self.boundary_limits
resolution = 1000
move_dir = [
float(end_config[0] - start_config[0]) / float(resolution),
float(end_config[1] - start_config[1]) / float(resolution)
]
position_unchange = self.robot.GetTransform()
for i in range(resolution + 1):
#get the robot position
position = self.robot.GetTransform()
position[0][3] = start_config[0] + i * move_dir[0]
position[1][3] = start_config[1] + i * move_dir[1]
self.robot.SetTransform(position)
if (self.robot.GetEnv().CheckCollision(self.robot)):
collision = True
#print position
#print "Detected collision!! Will return last safe step"
#print
if (upper_limits[0] < position[0][3]
or position[0][3] < lower_limits[0]
or upper_limits[1] < position[1][3]
or position[1][3] < lower_limits[1]):
outside = True
#print position
#print "Detected outside of bound!! Will return last safe step"
self.robot.SetTransform(position_unchange)
#check the collision == not touch the table && inside the boundary
if (collision or outside):
if (i >= 1):
valid_config = [0, 0]
valid_config[0] = start_config[0] + (i -
1) * move_dir[0]
valid_config[1] = start_config[1] + (i -
1) * move_dir[1]
return valid_config
else:
return None
#No interpolate with collision and outside
return end_config
#return NULL
def GenerateRandomConfiguration(self):
config = [0] * 2
lower_limits, upper_limits = self.boundary_limits
#
# TODO: Generate and return a random configuration
#
#Peter: just random config in the limitations
config[0] = lower_limits[0] + (
upper_limits[0] - lower_limits[0]) * numpy.random.random_sample()
config[1] = lower_limits[1] + (
upper_limits[1] - lower_limits[1]) * numpy.random.random_sample()
return numpy.array(config)
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits = [-5., -5.]
self.upper_limits = [5., 5.]
#HRRT
self.boundary_limits = [[-5., -5.], [5., 5.]]
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits, self.upper_limits)
# add an obstacle
table = self.robot.GetEnv().ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = np.array([[ 0, 0, -1, 1.5],
[-1, 0, 0, 0],
[ 0, 1, 0, 0],
[ 0, 0, 0, 1]])
table.SetTransform(table_pose)
self.p = 0.0
def GetCollision(self, config):
pose = np.array([[ 1., 0, 0, 0],
[ 0, 1., 0, 0],
[ 0, 0, 1., 0],
[ 0, 0, 0, 1.]])
pose[:2,3] = config
self.robot.SetTransform(pose)
collision = self.robot.GetEnv().CheckCollision(self.robot)
return collision
def GetSuccessors(self, node_id):
successors = []
node_config = self.discrete_env.NodeIdToConfiguration(node_id)
neighbors_config = ec.neighbors(node_config, self.discrete_env, self.GetCollision)
for config in neighbors_config:
successors.append(self.discrete_env.ConfigurationToNodeId(config))
return successors
def ComputeDistance(self, start_id, end_id):
start_coord = self.discrete_env.NodeIdToGridCoord(start_id)
end_coord = self.discrete_env.NodeIdToGridCoord(end_id)
dist = ec.cost(start_coord, end_coord)
dist = float(dist)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
start_coord = self.discrete_env.NodeIdToGridCoord(start_id)
goal_coord = self.discrete_env.NodeIdToGridCoord(goal_id)
cost = ec.cost(start_coord, goal_coord)
cost = float(cost)
return cost
def InitializePlot(self, goal_config):
self.fig = pl.figure()
pl.xlim([self.lower_limits[0], self.upper_limits[0]])
pl.ylim([self.lower_limits[1], self.upper_limits[1]])
pl.plot(goal_config[0], goal_config[1], 'gx')
# Show all obstacles in environment
for b in self.robot.GetEnv().GetBodies():
if b.GetName() == self.robot.GetName():
continue
bb = b.ComputeAABB()
pl.plot([bb.pos()[0] - bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] - bb.extents()[0]],
[bb.pos()[1] - bb.extents()[1],
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] - bb.extents()[1]], 'r')
pl.ion()
pl.show()
def PlotEdge(self, sconfig, econfig, color = 'k--'):
pl.plot([sconfig[0], econfig[0]],
[sconfig[1], econfig[1]],
color, linewidth=2.5)
pl.draw()
#
#
# H-RRT METHODS
#
#
###################################################
def SetGoalParameters(self, goal_config, p = 0.2):
self.goal_config = goal_config
self.p = p
def GenerateRandomConfiguration(self):
config = [0] * 2;
lower_limits, upper_limits = self.boundary_limits
#
# TODO: Generate and return a random configuration
#
s = self.robot.GetTransform()
while True:
config = np.multiply(np.subtract(upper_limits, lower_limits), np.random.random_sample((len(config),))) + np.array(lower_limits)
snew = self.robot.GetTransform()
snew[0][3] = config[0]
snew[1][3] = config[1]
self.robot.SetTransform(snew)
#Check if
if(not(self.robot.GetEnv().CheckCollision(self.robot.GetEnv().GetBodies()[0], self.robot.GetEnv().GetBodies()[1]))):
self.robot.SetTransform(s)
return np.array(config)
# DIFFERENT DISTANCE
def HRRTComputeDistance(self, start_config, end_config):
#
# TODO: Implement a function which computes the distance between
# two configurations
#
start_config_temp = np.array(start_config)
end_config_temp = np.array(end_config)
return np.linalg.norm(start_config_temp - end_config_temp)
pass
def Extend(self, start_config, end_config, epsilon):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
s = self.robot.GetTransform()
num_steps = 100
# epsilon = 0.001
dist = self.HRRTComputeDistance(start_config, end_config)
step_size = dist/num_steps
direction = (end_config - start_config)/dist
config = start_config + step_size*direction
lower_limits, upper_limits = np.array(self.boundary_limits)
lower_limits = lower_limits.tolist()
upper_limits = upper_limits.tolist()
steps = 1
# print self.robot.GetEnv().GetBodies()
while True:
snew = self.robot.GetTransform()
snew[0][3] = config[0]
snew[1][3] = config[1]
self.robot.SetTransform(snew)
#Check if the first step is out of limits, return None
if((steps==1) and ((config.tolist() < lower_limits) or (config.tolist() > upper_limits))):
self.robot.SetTransform(s)
# print "limits crossed"
return None
#Check if the first step collides then return None
elif((steps==1) and (self.robot.GetEnv().CheckCollision(self.robot.GetEnv().GetBodies()[0], self.robot.GetEnv().GetBodies()[1]))):
self.robot.SetTransform(s)
# print "collision"
return None
#If config is out of limits, return previous step
elif((config.tolist() < lower_limits) or (config.tolist() > upper_limits)):
self.robot.SetTransform(s)
# print "previous step"
return config - step_size*direction
#If collision occured later, return previous step
elif((self.robot.GetEnv().CheckCollision(self.robot.GetEnv().GetBodies()[0], self.robot.GetEnv().GetBodies()[1]))):
self.robot.SetTransform(s)
# print "previous step"
return config - step_size*direction
if np.all((self.HRRTComputeDistance(config, end_config) < epsilon)):
self.robot.SetTransform(s)
# print "jai mata di"
return config
config += step_size*direction
steps += 1
def ShortenPath(self, path, epsilon):
#
# TODO: Implement a function which performs path shortening
# on the given path. Terminate the shortening after the
# given timout (in seconds).
#
delta = 1
current_time = time.clock()
distance_initial = 0;
for i in range(len(path)-1):
distance_initial += self.HRRTComputeDistance(path[i],path[i+1])
print distance_initial, "initial distance"
while(not(delta > 5)):
leng = len(path)
g = random.randint(1, leng)
h = random.randint(1, leng-2)
while(g >= h):
g = random.randint(1, leng)
h = random.randint(1, leng-2)
##### two points ###
first_vertex = ((np.array(path[g]) + np.array(path[g-1]))/2).tolist()
second_vertex = ((np.array(path[h]) + np.array(path[h+1]))/2).tolist()
config = self.Extend(np.asarray(first_vertex), np.asarray(second_vertex), epsilon)
if(config != None):
#print vertex, vertex.dtype, local_path[ul], local_path[ul].dtype
#print config, np.asarray(second_vertex), "hey"
if (self.HRRTComputeDistance(config, np.asarray(second_vertex)) < 0.00001):
#if all(map(lambda v: v in config, np.asarray(second_vertex))):
path[g] = first_vertex
path[h] = second_vertex
for remove_ind in range(g+1, h):
path.pop(g+1)
delta = time.clock() - current_time
distance_final = 0;
print "Final vertices:", len(path)
for i in range(len(path)-1):
distance_final += self.HRRTComputeDistance(path[i],path[i+1])
for points in xrange(len(path)-1):
self.PlotEdge(path[points], path[points+1], 'g')
print "shortened distance:", distance_final
return path
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits = [-5., -5.]
self.upper_limits = [5., 5.]
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits, self.upper_limits)
# add an obstacle
self.table = self.robot.GetEnv().ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(self.table)
table_pose = numpy.array([[ 0, 0, -1, 1.5],
[-1, 0, 0, 0],
[ 0, 1, 0, 0],
[ 0, 0, 0, 1]])
self.table.SetTransform(table_pose)
self.p = 0.4
def GetSuccessors(self, node_id):
successors = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
# Implement using an eight connected world
# Function to determine if the configuration is in bounds
def in_bounds(config):
return ((config[0] >= self.lower_limits[0] and config[0] <= self.upper_limits[0])) and (config[1] >= self.lower_limits[1] and config[1] <= self.upper_limits[1])
# Function to determine if the robot is in collision
def collision_check(x, y):
transform_t = numpy.eye(4)
transform_t[0][3] = x
transform_t[1][3] = y
self.robot.SetTransform(transform_t)
collision = self.robot.GetEnv().CheckCollision(self.robot, self.table)
self.robot.SetTransform(numpy.eye(4))
return collision
def extend_collision_check(start_config, end_config):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
steps = self.ComputeDistance(start_config, end_config)*25 # Get 25 times the number of unit values in the distance space
x = numpy.linspace(start_config[0], end_config[0], num=steps) # Get the interpolated x values
y = numpy.linspace(start_config[1], end_config[1], num=steps) # Get the interpolated y values
# Check the interpolated path for collisions
for i in xrange(0, len(x)):
if(collision_check(x[i], y[i]) == True): # If in collision
return True
# If no part of the path is in collision
return False
# Function to determine if the perterbed config is within the grid
def valid_id(id, start_config):
end_config = self.discrete_env.NodeIdToConfiguration(id)
#if(extend_collision_check(start_config, end_config)):
if(collision_check(end_config[0], end_config[1])):
return False
return True # if it doesn't go out of bounds
config = self.discrete_env.NodeIdToConfiguration(node_id)
direction = [-1, 0, 1]
perterbed_config = [0] * 2
for x_dir in direction:
for y_dir in direction:
# Get perturbed configuration
perterbed_config[0] = config[0] + x_dir * self.discrete_env.resolution
perterbed_config[1] = config[1] + y_dir * self.discrete_env.resolution
# Check to see if the perturbed configuration is in bounds
if(in_bounds(perterbed_config)):
# Get perturbed node id
perterbed_id = self.discrete_env.ConfigurationToNodeId(perterbed_config)
if(perterbed_id != node_id):
if(valid_id(perterbed_id, config)):
# Return the robot to old config
transform_t = numpy.eye(4)
transform_t[0][3] = config[0]
transform_t[1][3] = config[1]
self.robot.SetTransform(transform_t)
# Append the successor
successors.append(perterbed_id)
return successors
def ComputeDistance(self, start_id, end_id):
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
# Using Euclidean Distance
start_config = numpy.array(self.discrete_env.NodeIdToConfiguration(start_id))
end_config = numpy.array(self.discrete_env.NodeIdToConfiguration(end_id))
return numpy.linalg.norm(start_config - end_config, 2)
def ComputeHeuristicCost(self, start_id, goal_id):
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
# Use manhattan distance for grid world
start_config = numpy.array(self.discrete_env.NodeIdToConfiguration(start_id))
goal_config = numpy.array(self.discrete_env.NodeIdToConfiguration(goal_id))
return numpy.linalg.norm(start_config - goal_config, 1)
def GenerateRandomConfiguration(self):
config = [0] * 2;
lower_limits = self.lower_limits
upper_limits = self.upper_limits
#
# TODO: Generate and return a random configuration
#
# lower_limits = [-5, -5]
# upper_limits = [5, 5]
# config = [0, 0]
while(True):
rand_x = numpy.random.uniform(lower_limits[0], upper_limits[0])
rand_y = numpy.random.uniform(lower_limits[1], upper_limits[1])
robo = self.robot # Copy the robot
new_pose = numpy.array([[1, 0, 0, rand_x],
[0, 1, 0, rand_y],
[0, 0, 1, 0 ],
[0, 0, 0, 1 ]])
robo.SetTransform(new_pose)
if(self.robot.GetEnv().CheckCollision(robo, self.table) == False):
config = numpy.array([rand_x, rand_y])
return numpy.array(config)
def Extend(self, start_config, end_config,):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
steps = self.ComputeDistance(self.discrete_env.ConfigurationToNodeId(start_config), self.discrete_env.ConfigurationToNodeId(end_config))*25 # Get 25 times the number of unit values in the distance space
x = numpy.linspace(start_config[0], end_config[0], num=steps) # Get the interpolated x values
y = numpy.linspace(start_config[1], end_config[1], num=steps) # Get the interpolated y values
# Check the interpolated path for collisions
for i in xrange(0, len(x)):
if(self.collision_check(x[i], y[i]) == True):
if(i == 0):
return None
return numpy.vstack((x[0:i], y[0:i])).transpose() # Send path up until collision
# If no part of the path is in collision
return numpy.vstack((x,y)).transpose() # Send whole path
def collision_check(self, x, y):
transform_t = numpy.eye(4)
transform_t[0][3] = x
transform_t[1][3] = y
self.robot.SetTransform(transform_t)
collision = self.robot.GetEnv().CheckCollision(self.robot, self.table)
self.robot.SetTransform(numpy.eye(4))
return collision
def ShortenPath(self, path, timeout=5.0):
#
# TODO: Implement a function which performs path shortening
# on the given path. Terminate the shortening after the
# given timout (in seconds).
#
#make copy so we don't overwrite the original path
short_path = list(path)
init_time = time.time()
while time.time() - init_time < timeout:
start = random.randint(0, len(short_path)-3)
end = random.randint(start+1, len(short_path)-1)
extend = self.Extend(short_path[start], short_path[end])
if(extend == None or len(extend) == 0):
continue
else:
if compare(extend[len(extend)-1,:], short_path[end]):
short_path[start+1:end] = []
return numpy.array(short_path)
def InitializePlot(self, goal_config):
self.fig = pl.figure()
pl.xlim([self.lower_limits[0], self.upper_limits[0]])
pl.ylim([self.lower_limits[1], self.upper_limits[1]])
pl.plot(goal_config[0], goal_config[1], 'gx')
# Show all obstacles in environment
for b in self.robot.GetEnv().GetBodies():
if b.GetName() == self.robot.GetName():
continue
bb = b.ComputeAABB()
pl.plot([bb.pos()[0] - bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] - bb.extents()[0]],
[bb.pos()[1] - bb.extents()[1],
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] - bb.extents()[1]], 'r')
pl.ion()
pl.show()
def PlotEdge(self, sconfig, econfig):
pl.plot([sconfig[0], econfig[0]],
[sconfig[1], econfig[1]],
'k.-', linewidth=2.5)
pl.draw()
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.herb = herb
self.robot = herb.robot
self.boundary_limits = [[-5., -5., -numpy.pi], [5., 5., numpy.pi]]
lower_limits, upper_limits = self.boundary_limits
self.discrete_env = DiscreteEnvironment(resolution, lower_limits, upper_limits)
self.resolution = resolution
self.ConstructActions()
def GenerateFootprintFromControl(self, start_config, control, stepsize=0.01):
# Extract the elements of the control
ul = control.ul
ur = control.ur
dt = control.dt
# Initialize the footprint
config = start_config.copy()
footprint = [numpy.array([0., 0., config[2]])]
timecount = 0.0
while timecount < dt:
# Generate the velocities based on the forward model
xdot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.cos(config[2])
ydot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.sin(config[2])
tdot = self.herb.wheel_radius * (ul - ur) / self.herb.wheel_distance
# Feed forward the velocities
if timecount + stepsize > dt:
stepsize = dt - timecount
config = config + stepsize*numpy.array([xdot, ydot, tdot])
if config[2] > numpy.pi:
config[2] -= 2.*numpy.pi
if config[2] < -numpy.pi:
config[2] += 2.*numpy.pi
footprint_config = config.copy()
footprint_config[:2] -= start_config[:2]
footprint.append(footprint_config)
timecount += stepsize
# Add one more config that snaps the last point in the footprint to the center of the cell
nid = self.discrete_env.ConfigurationToNodeId(config)
snapped_config = self.discrete_env.NodeIdToConfiguration(nid)
snapped_config[:2] -= start_config[:2]
footprint.append(snapped_config)
return footprint
def PlotActionFootprints(self, idx):
actions = self.actions[idx]
fig = pl.figure()
lower_limits, upper_limits = self.boundary_limits
pl.xlim([lower_limits[0], upper_limits[0]])
pl.ylim([lower_limits[1], upper_limits[1]])
print('Numactions: ' ,len(actions), len(self.actions))
for action in actions:
xpoints = [config[0] for config in action.footprint]
ypoints = [config[1] for config in action.footprint]
pl.plot(xpoints, ypoints, 'k')
# title = 'Controls =' + str(action.control) +', Orientation = ' + str(idx)
# pl.title(title)
# fig.savefig(str(action.control)+'_Idx_'+str(idx) + '.png')
pl.ion()
pl.show()
def ConstructActions(self):
# Actions is a dictionary that maps orientation of the robot to
# an action set
self.actions = dict()
wc = [0., 0., 0.]
grid_coordinate = self.discrete_env.ConfigurationToGridCoord(wc)
# Iterate through each possible starting orientation
for idx in range(int(self.discrete_env.num_cells[2]+1)):
self.actions[idx] = []
grid_coordinate[2] = idx
start_config = self.discrete_env.GridCoordToConfiguration(grid_coordinate)
#create a list named control to store a set controls
controls = [Control(1.0,1.0,.5), Control(1.0,-1.0,.5), Control(-1.0,-1.0,.5), Control(-1.0,1.0,.5)]
for control in controls:
self.actions[idx].append(Action(control, self.GenerateFootprintFromControl(start_config, control, 0.01)))
# for idx in range(len(self.actions)):
# self.PlotActionFootprints(idx)
# raw_input('')
def GetSuccessors(self, node_id):
successors = []
config = self.discrete_env.NodeIdToConfiguration(node_id)
coord = self.discrete_env.NodeIdToGridCoord(node_id)
avail_actions = self.actions[coord[2]]
# print '....................'
# print avail_actions
# print '....................'
for i in range(len(avail_actions)):
final_config = [config[0] + avail_actions[i].footprint[-1][0], config[1] + avail_actions[i].footprint[-1][1], avail_actions[i].footprint[-1][2]]
if not self.RobotIsInCollisionAt(final_config):
true_footprint = []
for j in range(len(avail_actions[i].footprint)):
true_config = [config[0] + avail_actions[i].footprint[j][0], config[1] + avail_actions[i].footprint[j][1], avail_actions[i].footprint[j][2]]
true_footprint.append(true_config)
successor_control = avail_actions[i].control
final_config = true_footprint[-1]
successor_id = self.discrete_env.ConfigurationToNodeId(final_config)
#succ_config = self.discrete_env.NodeIdToConfiguration(successor_id)
successors.append([successor_id, Action(successor_control, numpy.array(true_footprint))])
return successors
def RobotIsInCollisionAt(self, point=None):
"""
Call self.RobotIsInCollisionAt() to check collision in current state
self.RobotIsInCollisionAt(np2darray) to check at another point
"""
lower_limits, upper_limits = self.boundary_limits
lower_limits, upper_limits = self.boundary_limits
if point is None:
return self.robot.GetEnv().CheckCollision(self.robot)
# If checking collision in point other than current state, move robot
# to that point, check collision, then move it back.
current_state = self.robot.GetTransform()
# print '-----------------------------------------'
# print current_state
check_state = numpy.copy(current_state)
# print '-----------------------------------------'
# print point
check_state[:3, 3] = numpy.array([point[0], point[1], 0.0])
check_state[:3, :3] = numpy.array([[numpy.cos(point[2]), -numpy.sin(point[2]),0.0],[numpy.sin(point[2]),numpy.cos(point[2]),0.0],[0.0,0.0,1.0]])
self.robot.SetTransform(check_state)
collision = False
for body in self.robot.GetEnv().GetBodies()[1:]:
if self.robot.GetEnv().CheckCollision(self.robot, body):
collision = True
print "in collision!"
# if self.robot.GetEnv().CheckCollision(self.robot):
# collision = True
# print "in collision!"
in_collision = collision or ((point < lower_limits).any() or (point > upper_limits).any())
self.robot.SetTransform(current_state) # move robot back to current state
return in_collision
# Apply loc to current transform and return new transform
def ApplyMotion(self, loc):
new_transform = self.robot.GetTransform()
for i in range(len(loc[0:2])):
new_transform[i][3] = loc[i]
return new_transform
# Check if new locn has collisions
def CheckCollision(self, locn):
collisions = []
with self.robot.GetEnv():
# Apply new_locn to current robot transform
orig_transform = self.robot.GetTransform()
new_transform = self.ApplyMotion(locn)
self.robot.SetTransform(new_transform)
# Check for collision
bodies = self.robot.GetEnv().GetBodies()[1:]
collisions = map(lambda body: self.robot.GetEnv().CheckCollision(self.robot, body), bodies)
# Set robot back to original transform
self.robot.SetTransform(orig_transform)
return reduce(lambda x1, x2: x1 or x2, collisions)
def ComputeDistance(self, start_id, end_id):
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(end_id)
return numpy.linalg.norm(numpy.array(start_config[:len(start_config)-1]) - numpy.array(end_config[:len(end_config)-1]))
def ComputeHeuristicCost(self, start_id, goal_id):
return self.ComputeDistance(start_id,goal_id)
def InitializePlot(self, goal_config):
self.fig = pl.figure()
lower_limits, upper_limits = self.boundary_limits
pl.xlim([lower_limits[0], upper_limits[0]])
pl.ylim([lower_limits[1], upper_limits[1]])
pl.plot(goal_config[0], goal_config[1], 'gx')
# Show all obstacles in environment
for b in self.robot.GetEnv().GetBodies():
if b.GetName() == self.robot.GetName():
continue
bb = b.ComputeAABB()
pl.plot([bb.pos()[0] - bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] - bb.extents()[0]],
[bb.pos()[1] - bb.extents()[1],
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] - bb.extents()[1]], 'r')
pl.ion()
pl.show()
def PlotEdge(self, sconfig, econfig):
pl.plot([sconfig[0], econfig[0]],
[sconfig[1], econfig[1]],
'k.-', linewidth=2.5)
pl.draw()
class HerbEnvironment(object):
def __init__(self, herb, resolution):
self.herb = herb
self.robot = herb.robot
self.lower_limits, self.upper_limits = self.robot.GetActiveDOFLimits()
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
# account for the fact that snapping to the middle of the grid cell may put us over our
# upper limit
#
#Peter: we have deal with the close to upper limit in DiscreteEnviroment.py/GridCoordToConfiguration
#so this line will cause confusion
#upper_coord = [x - 1 for x in self.discrete_env.num_cells]
#upper_config = self.discrete_env.GridCoordToConfiguration(upper_coord)
#for idx in range(len(upper_config)):
# self.discrete_env.num_cells[idx] -= 1
#Peter:
# add a table and move the robot into place
#table = self.robot.GetEnv().ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
#self.robot.GetEnv().Add(table)
#print self.lower_limits
#print self.upper_limits
#table_pose = numpy.array([[ 0, 0, -1, 0.7],
# [-1, 0, 0, 0],
# [ 0, 1, 0, 0],
# [ 0, 0, 0, 1]])
#table.SetTransform(table_pose)
# set the camera
camera_pose = numpy.array(
[[0.3259757, 0.31990565, -0.88960678, 2.84039211],
[0.94516159, -0.0901412, 0.31391738, -0.87847549],
[0.02023372, -0.9431516, -0.33174637, 1.61502194],
[0., 0., 0., 1.]])
self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
def GetSuccessors(self, node_id):
successors = []
node_coord = numpy.array(self.discrete_env.NodeIdToGridCoord(node_id))
#for each direction there are two type neighbor
for idx in range(self.discrete_env.dimension):
#positive direction
if ((node_coord[idx] + 1) <=
(self.discrete_env.num_cells[idx] - 1)):
n_node_coord = numpy.copy(node_coord)
n_node_coord[idx] += 1
n_node_config = self.discrete_env.GridCoordToConfiguration(
n_node_coord)
if (self.no_collision(n_node_config)):
n_node_id = self.discrete_env.GridCoordToNodeId(
n_node_coord)
successors.append(n_node_id)
#negation direction
if ((node_coord[idx] - 1) >= 0):
n_node_coord = numpy.copy(node_coord)
n_node_coord[idx] -= 1
n_node_config = self.discrete_env.GridCoordToConfiguration(
n_node_coord)
if (self.no_collision(n_node_config)):
n_node_id = self.discrete_env.GridCoordToNodeId(
n_node_coord)
successors.append(n_node_id)
return successors
def ComputeDistance(self, start_id, end_id):
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(end_id)
#calculate distance
dist = numpy.linalg.norm(
numpy.array(start_config) - numpy.array(end_config))
return dist
def ComputeDistance_continue(self, start_id, end_id):
dist = numpy.linalg.norm(numpy.array(start_id) - numpy.array(end_id))
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by te two node ids
return dist
def SetGoalParameters(self, goal_config, p=0.2):
self.goal_config = goal_config
self.p = p
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
cost = self.ComputeDistance(start_id, goal_id)
return cost
def CheckCollision(self, config):
self.robot.SetActiveDOFValues(config.squeeze())
obstacles = self.robot.GetEnv().GetBodies()
collision = self.robot.GetEnv().CheckCollision(
self.robot, obstacles[1]) or self.robot.GetEnv().CheckCollision(
self.robot, obstacles[2]) or self.robot.CheckSelfCollision()
return collision
def no_collision(self, config):
self.robot.SetActiveDOFValues(config.squeeze())
obstacles = self.robot.GetEnv().GetBodies()
collision = self.robot.GetEnv().CheckCollision(
self.robot, obstacles[1]) or self.robot.GetEnv().CheckCollision(
self.robot, obstacles[2]) or self.robot.CheckSelfCollision()
return not collision
def ComputePathLength(self, path):
path_length = 0
for milestone in range(1, len(path)):
dist = numpy.linalg.norm(
numpy.array(path[milestone - 1]) -
numpy.array(path[milestone]))
path_length = path_length + dist
return path_length
def GenerateRandomConfiguration(self):
new_config = gp.generate(self.lower_limits, self.upper_limits,
self.goal_config, self.p, self.CheckCollision)
return new_config
"""def GenerateRandomConfiguration(self):
config = [0] * len(self.robot.GetActiveDOFIndices())
lower_limits, upper_limits = self.robot.GetActiveDOFLimits()
collisionFlag = True
while collisionFlag is True:
for dof in range(len(self.robot.GetActiveDOFIndices())):
config[dof] = lower_limits[dof] + (upper_limits[dof] - lower_limits[dof]) * numpy.random.random_sample()
if self.CheckCollision(numpy.array(config)) is False and self.robot.CheckSelfCollision() is False:
collisionFlag = False
#else:
#print "Self Collision detected in random configuration"
#else:
#print "Collision Detected in random configuration"
return numpy.array(config)
"""
def Extend(self, start_config, end_config, epsilon=0.1):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
new_config = gp.extend(start_config, end_config, self.CheckCollision,
epsilon)
return new_config
class HerbEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits, self.upper_limits = self.robot.GetActiveDOFLimits()
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
self.resolution = resolution
# account for the fact that snapping to the middle of the grid cell may put us over our
# upper limit
upper_coord = [x - 1 for x in self.discrete_env.num_cells]
upper_config = self.discrete_env.GridCoordToConfiguration(upper_coord)
for idx in range(len(upper_config)):
self.discrete_env.num_cells[idx] -= 1
# add a table and move the robot into place
table = self.robot.GetEnv().ReadKinBodyXMLFile(
'models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = numpy.array([[0, 0, -1, 0.7], [-1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 0, 1]])
table.SetTransform(table_pose)
# set the camera
camera_pose = numpy.array(
[[0.3259757, 0.31990565, -0.88960678, 2.84039211],
[0.94516159, -0.0901412, 0.31391738, -0.87847549],
[0.02023372, -0.9431516, -0.33174637, 1.61502194],
[0., 0., 0., 1.]])
self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
def GetSuccessors(self, node_id):
successors = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
coord = self.discrete_env.NodeIdToGridCoord(node_id)
## this only changes one joint at a time, considering the +/- 1 coord, with the other coords remaining constant
neighbors = []
for idx, dof in enumerate(coord):
cfg1 = coord.copy()
cfg2 = coord.copy()
cfg1[idx] = cfg1[idx] + 1
cfg2[idx] = cfg2[idx] - 1
neighbors.append(cfg1)
neighbors.append(cfg2)
bodies = self.robot.GetEnv().GetBodies()
orig_config = self.robot.GetActiveDOFValues()
# print neighbors
for ncoord in neighbors:
if numpy.any(ncoord < 0) or numpy.any(
ncoord >= (self.discrete_env.num_cells)):
#print "...invalid neighbor"
continue
config = self.discrete_env.GridCoordToConfiguration(ncoord)
with self.robot.GetEnv():
self.robot.SetActiveDOFValues(config)
collision = (self.robot.CheckSelfCollision()
or self.robot.GetEnv().CheckCollision(
bodies[1], bodies[0]))
# self.robot.SetActiveDOFValues(orig_config)
if not collision:
successors.append(
self.discrete_env.GridCoordToNodeId(ncoord))
# print successors
else:
#print "collision"
self.robot.SetActiveDOFValues(orig_config)
return successors
def ComputeDistance(self, start_id, end_id):
dist = 0
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(end_id)
# start_coord = self.discrete_env.NodeIdToGridCoord(start_id)
# end_coord = self.discrete_env.NodeIdToGridCoord(end_id)
# dist = sum(abs(start_config-end_config))
# dist = numpy.linalg.norm(start_config-end_config,1)
dist = numpy.linalg.norm(start_config - end_config)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
## Heuristic = Euclidean Distance between two configs
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(goal_id)
cost = numpy.linalg.norm(start_config - end_config)
return cost
def GenerateRandomConfiguration(self):
config = [0] * len(self.robot.GetActiveDOFIndices())
#
# TODO: Generate and return a random configuration
#
# Brad: Generate and return a random, collision-free, configuration
lower_limits, upper_limits = self.robot.GetActiveDOFLimits()
collisionFlag = True
while collisionFlag is True:
for dof in range(len(self.robot.GetActiveDOFIndices())):
config[dof] = lower_limits[dof] + (
upper_limits[dof] -
lower_limits[dof]) * numpy.random.random_sample()
#CHECK: Lock environment?
self.robot.SetActiveDOFValues(numpy.array(config))
if (self.robot.GetEnv().CheckCollision(self.robot)) is False:
if (self.robot.CheckSelfCollision()) is False:
collisionFlag = False
#else:
#print "Self Collision detected in random configuration"
#else:
#print "Collision Detected in random configuration"
return numpy.array(config)
def Extend(self, start_config, end_config):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
# Brad: Extend from start to end configuration and return sooner if collision or limits exceeded
lower_limits, upper_limits = self.robot.GetActiveDOFLimits()
resolution = 100
#Calculate incremental configuration changes
config_inc = [0] * len(self.robot.GetActiveDOFIndices())
for dof in range(len(self.robot.GetActiveDOFIndices())):
config_inc[dof] = (end_config[dof] -
start_config[dof]) / float(resolution)
#Set initial config state to None to return if start_config violates conditions
config = None
#Move from start_config to end_config
for step in range(resolution + 1):
prev_config = config
config = [0] * len(self.robot.GetActiveDOFIndices())
for dof in range(len(self.robot.GetActiveDOFIndices())):
#Calculate new config
config[dof] = start_config[dof] + config_inc[dof] * float(step)
#Check joint limits
if config[dof] > upper_limits[dof]:
print "Upper joint limit exceeded"
return prev_config
if config[dof] < lower_limits[dof]:
print "Lower joint limit exceeded"
return prev_config
#Set config and check for collision
#CHECK: Lock environment?
self.robot.SetActiveDOFValues(numpy.array(config))
if (self.robot.GetEnv().CheckCollision(self.robot)) is True:
#print "Collision Detected in extend"
return prev_config
if (self.robot.CheckSelfCollision()) is True:
#print "Self Collision Detected in extend"
return prev_config
return end_config
def ShortenPath(self, path, timeout=5.0):
#
# TODO: Implement a function which performs path shortening
# on the given path. Terminate the shortening after the
# given timout (in seconds).
#
#Brad
#print "Start shortening"
prev_path = list(path)
prev_len = self.ComputePathLength(path)
print "Current path length is %f" % prev_len
elapsed_time = 0
start_time = time.time()
while elapsed_time < timeout:
end_time = time.time()
elapsed_time = end_time - start_time
#print path_short
if len(prev_path) < 3:
print "Shortened path length is %f" % self.ComputePathLength(
prev_path)
return prev_path
else:
curr_path = list(path)
for attempt in range(len(path) * len(path)):
#print attempt
init_idx = numpy.random.randint(0, len(curr_path) - 2)
end_idx = numpy.random.randint(init_idx + 2,
len(curr_path))
init_ms = curr_path[init_idx]
end_ms = curr_path[end_idx]
result_ms = self.Extend(init_ms, end_ms)
if numpy.array_equal(end_ms, result_ms):
for elem in range(init_idx + 1, end_idx):
del curr_path[init_idx + 1]
curr_len = self.ComputePathLength(curr_path)
if curr_len < prev_len:
prev_path = curr_path
prev_len = curr_len
print "Shortened path length is %f" % self.ComputePathLength(prev_path)
return prev_path
def ComputePathLength(self, path):
#Helper function to compute path length
length = 0
for milestone in range(1, len(path)):
#print path[milestone-1], path[milestone]
distance = self.ComputeDistanceC(numpy.array(path[milestone - 1]),
numpy.array(path[milestone]))
#print distance
length += distance
return length
def ComputeDistanceC(self, start_config, end_config):
#
# TODO: Implement a function which computes the distance between
# two configurations
#
# Brad: Compute the distance between two configurations as the L2 norm
distance = numpy.linalg.norm(end_config - start_config)
return distance
def ComputeHeuristicC(self, start_config, end_config):
distance = numpy.linalg.norm(end_config - start_config)
return distance
def ShortenPath(self, path, timeout=5.0):
#
# TODO: Implement a function which performs path shortening
# on the given path. Terminate the shortening after the
# given timout (in seconds).
#
#Brad
#print "Start shortening"
prev_path = list(path)
prev_len = self.ComputePathLength(path)
print "Current path length is %f" % prev_len
elapsed_time = 0
start_time = time.time()
while elapsed_time < timeout:
end_time = time.time()
elapsed_time = end_time - start_time
#print path_short
if len(prev_path) < 3:
print "Shortened path length is %f" % self.ComputePathLength(
prev_path)
return prev_path
else:
curr_path = list(path)
for attempt in range(len(path) * len(path)):
#print attempt
init_idx = numpy.random.randint(0, len(curr_path) - 2)
end_idx = numpy.random.randint(init_idx + 2,
len(curr_path))
init_ms = curr_path[init_idx]
end_ms = curr_path[end_idx]
result_ms = self.Extend(init_ms, end_ms)
if numpy.array_equal(end_ms, result_ms):
for elem in range(init_idx + 1, end_idx):
del curr_path[init_idx + 1]
curr_len = self.ComputePathLength(curr_path)
if curr_len < prev_len:
prev_path = curr_path
prev_len = curr_len
print "Shortened path length is %f" % self.ComputePathLength(prev_path)
return prev_path
class HerbEnvironment(object):
def __init__(self, herb, table, resolution):
self.robot = herb.robot
self.lower_limits, self.upper_limits = self.robot.GetActiveDOFLimits()
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits, self.upper_limits)
# account for the fact that snapping to the middle of the grid cell may put us over our
# upper limit
upper_coord = [x - 1 for x in self.discrete_env.num_cells]
upper_config = self.discrete_env.GridCoordToConfiguration(upper_coord)
for idx in range(len(upper_config)):
self.discrete_env.num_cells[idx] -= 1
# add a table and move the robot into place
# self.table = self.robot.GetEnv().ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
# self.robot.GetEnv().Add(self.table)
# table_pose = numpy.array([[ 0, 0, -1, 0.7],
# [-1, 0, 0, 0],
# [ 0, 1, 0, 0],
# [ 0, 0, 0, 1]])
# self.table.SetTransform(table_pose)
# set the camera
camera_pose = numpy.array([[ 0.3259757 , 0.31990565, -0.88960678, 2.84039211],
[ 0.94516159, -0.0901412 , 0.31391738, -0.87847549],
[ 0.02023372, -0.9431516 , -0.33174637, 1.61502194],
[ 0. , 0. , 0. , 1. ]])
self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
self.table = table
self.p = 0
def GetSuccessors(self, node_id):
successors = []
# This function looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
env = self.discrete_env
coords = env.NodeIdToGridCoord(node_id)
# print("coordinates for node id ("+str(node_id)+") = "+str(coords))
# Iterate over dimensions, adding and subtracting one grid per dimension
for x in xrange(0,self.discrete_env.dimension):
# Slice copy to not alter coords
newCoord = coords[:]
# Subtraction neighbor
newCoord[x] = coords[x] - 1
newID = env.GridCoordToNodeId(newCoord)
if (self.CheckCollisions(newID) == False and self.InBounds(newCoord) == True):
successors.append(newID)
# Addition Neighbor
newCoord[x] = coords[x] + 1
newID = env.GridCoordToNodeId(newCoord)
if (self.CheckCollisions(newID) == False and self.InBounds(newCoord) == True):
successors.append(newID)
return successors
def CheckCollisions(self, node_id):
config = self.discrete_env.NodeIdToConfiguration(node_id)
# Put robot into configuration
self.robot.SetActiveDOFValues(config)
# Check collision with table
if self.robot.GetEnv().CheckCollision(self.robot, self.table) == True or self.robot.CheckSelfCollision() == True:
return True
else:
return False
def InBounds(self, coord):
config = self.discrete_env.GridCoordToConfiguration(coord)
for x in xrange(0, self.discrete_env.dimension):
# Check limits of space for each dimension
if not(config[x] < self.upper_limits[x]-0.0005 and config[x] > self.lower_limits[x]+0.0005):
return False
return True
def ComputeDistance(self, start_id, end_id):
# computes the distance between the configurations given
# by the two node ids
start_grid = self.discrete_env.NodeIdToGridCoord(start_id)
end_grid = self.discrete_env.NodeIdToGridCoord(end_id)
dist = 0
for x in xrange(0, self.discrete_env.dimension):
dist = dist + (abs(end_grid[x] - start_grid[x]) * self.discrete_env.resolution)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = self.ComputeDistance(start_id, goal_id)
return cost
def ShortenPath(self, path, timeout=5.0):
#
# TODO: Implement a function which performs path shortening
# on the given path. Terminate the shortening after the
# given timout (in seconds).
#
t0 = time.time()
idx = 0
print "Shortening Path (original length: %d" % len(path)
while idx < len(path)-1 and time.time() - t0 < timeout:
# Check backwrds from goal
for ridx in xrange(len(path)-1, idx, -1):
if self.Extend(path[idx], path[ridx]) != None:
dist_ab = self.ComputeSomeDistance(path[idx], path[ridx])
dist_path_slice = self.ComputePathSliceLength(path, idx, ridx)
# If distance between two points is less than distance along path, slice out inbetween
if dist_ab < dist_path_slice:
# Remove all inbetween if not next to each other
# And done
if (ridx - idx+1 > 0):
path[idx+1:ridx] = []
break
idx += 1
print "Shorter Path (length: %d" % len(path)
return path
def SetGoalParameters(self, goal_config, p = 0.2):
self.goal_config = goal_config
self.p = p
def GenerateRandomConfiguration(self):
if (numpy.random.random() < self.p):
return numpy.array(self.goal_config)
config = [0] * len(self.robot.GetActiveDOFIndices())
lower_limits, upper_limits = self.robot.GetActiveDOFLimits()
for i in xrange(len(config)):
config[i] = lower_limits[i] + numpy.random.random()*(upper_limits[i] - lower_limits[i]);
#
# TODO: Generate and return a random configuration
#
return numpy.array(config)
def ComputeSomeDistance(self, start_config, end_config):
#
# TODO: Implement a function which computes the distance between
# two configurations
#
return self.getDistance(end_config-start_config)
def getDistance(self, offset):
return numpy.sqrt(sum(offset**2))
def Extend(self, start_config, end_config):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
dist = self.ComputeSomeDistance(start_config, end_config)
MAX_STEP_SIZE = 0.05
# If two points are so close vs step size, it can make it
if (dist < MAX_STEP_SIZE):
return end_config
num_steps = round(dist/MAX_STEP_SIZE)
path = numpy.array(map(lambda x: numpy.linspace(x[0], x[1], num_steps), zip(start_config,end_config)))
#IPython.embed()
for p in path.transpose():
# print(p)
if self.checkConfigurationCollision(p):
return None
# Return last element in path
return path.transpose()[-1]
def ComputePathSliceLength(self, path, a_idx, b_idx):
dist_path_slice = 0
for x in xrange(a_idx,b_idx):
dist_path_slice += self.ComputeSomeDistance(numpy.array(path[x]), numpy.array(path[x+1]))
return dist_path_slice
def checkConfigurationCollision(self, config):
# This could be extended to N bodies check using loops, quadtrees etc.
# But this has been solved in openrave etc. so I'm just hardcoding it here.
# IPython.embed()
self.robot.SetJointValues(config, self.robot.GetActiveDOFIndices())
return self.robot.GetEnv().CheckCollision(self.robot, self.table) or self.robot.CheckSelfCollision()
class HerbEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits, self.upper_limits = self.robot.GetActiveDOFLimits()
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
# account for the fact that snapping to the middle of the grid cell may put us over our
# upper limit
upper_coord = [x - 1 for x in self.discrete_env.num_cells]
upper_config = self.discrete_env.GridCoordToConfiguration(upper_coord)
for idx in range(len(upper_config)):
self.discrete_env.num_cells[idx] -= 1
# add a table and move the robot into place
table = self.robot.GetEnv().ReadKinBodyXMLFile(
'models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = numpy.array([[0, 0, -1, 0.7], [-1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 0, 1]])
table.SetTransform(table_pose)
# set the camera
camera_pose = numpy.array(
[[0.3259757, 0.31990565, -0.88960678, 2.84039211],
[0.94516159, -0.0901412, 0.31391738, -0.87847549],
[0.02023372, -0.9431516, -0.33174637, 1.61502194],
[0., 0., 0., 1.]])
self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
self.p = 0.0
def GetCollision(self, config):
self.robot.SetActiveDOFValues(config)
rigidBody = self.robot.GetEnv().GetBodies()
bodyNum = len(rigidBody)
if bodyNum == 2:
check = self.robot.GetEnv().CheckCollision(rigidBody[0],
rigidBody[1])
elif bodyNum == 3:
check = self.robot.GetEnv().CheckCollision(rigidBody[0],
rigidBody[1],
rigidBody[2])
if check is False and self.robot.CheckSelfCollision() is False:
collision = False
else:
collision = True
return collision
def GetSuccessors(self, node_id):
successors = []
node_config = self.discrete_env.NodeIdToConfiguration(node_id)
neighbors_config = ec.neighbors(node_config, self.discrete_env,
self.GetCollision)
for config in neighbors_config:
successors.append(self.discrete_env.ConfigurationToNodeId(config))
return successors
def ComputeDistance(self, start_id, end_id):
start_coord = self.discrete_env.NodeIdToGridCoord(start_id)
end_coord = self.discrete_env.NodeIdToGridCoord(end_id)
dist = ec.cost(start_coord, end_coord)
dis = float(dist)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
start_coord = self.discrete_env.NodeIdToGridCoord(start_id)
goal_coord = self.discrete_env.NodeIdToGridCoord(goal_id)
cost = ec.cost(start_coord, goal_coord)
cost = float(cost)
return cost
#
#HERB ENVIRONMENT HRRT METHODS
#
#
#
def SetGoalParameters(self, goal_config, p=0.2):
self.goal_config = goal_config
self.p = p
def HRRTComputeDistance(self, start_config, end_config):
start_config_temp = numpy.array(start_config)
end_config_temp = numpy.array(end_config)
return numpy.linalg.norm(start_config_temp - end_config_temp)
pass
def GenerateRandomConfiguration(self):
config = [0] * len(self.robot.GetActiveDOFIndices())
#
# TODO: Generate and return a random configuration
#
s = self.robot.GetActiveDOFValues()
lower_limits, upper_limits = self.robot.GetActiveDOFLimits()
while True:
config = numpy.multiply(
numpy.subtract(upper_limits, lower_limits),
numpy.random.random_sample(
(len(config), ))) + numpy.array(lower_limits)
self.robot.SetActiveDOFValues(config)
if (not ((self.robot.GetEnv().CheckCollision(
self.robot,
self.robot.GetEnv().GetKinBody('conference_table'))))):
self.robot.SetActiveDOFValues(s)
return numpy.array(config)
def Extend(self, start_config, end_config, epsilon):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
s = self.robot.GetActiveDOFValues()
num_steps = 10
# epsilon = 0.01
dist = self.HRRTComputeDistance(start_config, end_config)
step_size = dist / num_steps
direction = (end_config - start_config) / dist
config = start_config + step_size * direction
lower_limits, upper_limits = numpy.array(
self.robot.GetActiveDOFLimits())
lower_limits = lower_limits.tolist()
upper_limits = upper_limits.tolist()
steps = 1
while True:
self.robot.SetActiveDOFValues(config)
#Check if the first step is out of limits, return None
if ((steps == 1) and ((config.tolist() < lower_limits) or
(config.tolist() > upper_limits))):
self.robot.SetActiveDOFValues(s)
# print "none"
return None
#Check if the first step collides then return None
elif ((steps == 1) and (self.robot.GetEnv().CheckCollision(
self.robot.GetEnv().GetBodies()[0],
self.robot.GetEnv().GetBodies()[1])
or self.robot.CheckSelfCollision())):
self.robot.SetActiveDOFValues(s)
# print "none"
return None
#If config is out of limits, return previous step
elif ((config.tolist() < lower_limits)
or (config.tolist() > upper_limits)):
self.robot.SetActiveDOFValues(s)
# print 'out of limits'
return config - step_size * direction
#If collision occured later, return previous step
elif ((self.robot.GetEnv().CheckCollision(
self.robot.GetEnv().GetBodies()[0],
self.robot.GetEnv().GetBodies()[1])
or self.robot.CheckSelfCollision())):
self.robot.SetActiveDOFValues(s)
# print 'return previous step'
return config - step_size * direction
if numpy.all((numpy.subtract(config, end_config) < epsilon)):
self.robot.SetActiveDOFValues(s)
# print 'end config'
return end_config
config += step_size * direction
steps += 1
pass
def ShortenPath(self, path, timeout=5.0):
#
# TODO: Implement a function which performs path shortening
# on the given path. Terminate the shortening after the
# given timout (in seconds).
#
delta = 1
current_time = time.clock()
while (not (delta > 5)):
leng = len(path)
g = random.randint(1, leng)
h = random.randint(1, leng - 2)
while (g >= h):
g = random.randint(1, leng)
h = random.randint(1, leng - 2)
##### two points ######
first_vertex = ((numpy.array(path[g]) + numpy.array(path[g - 1])) /
2).tolist()
second_vertex = (
(numpy.array(path[h]) + numpy.array(path[h + 1])) /
2).tolist()
config = self.Extend(numpy.asarray(first_vertex),
numpy.asarray(second_vertex))
if (config != None):
#print vertex, vertex.dtype, local_path[ul], local_path[ul].dtype
if all(map(lambda v: v in config,
numpy.asarray(second_vertex))):
path[g] = first_vertex
path[h] = second_vertex
for remove_ind in range(g + 1, h):
path.pop(g + 1)
delta = time.clock() - current_time
distance_final = 0
print "final vertices", len(path)
for i in range(len(path) - 1):
distance_final += self.HRRTComputeDistance(path[i], path[i + 1])
print "shortened distance", distance_final
return path
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits = [-5., -5.]
self.upper_limits = [5., 5.]
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
# add an obstacle
table = self.robot.GetEnv().ReadKinBodyXMLFile(
'models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = np.array([[0, 0, -1, 1.5], [-1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 0, 1]])
table.SetTransform(table_pose)
def GetSuccessors(self, node_id):
successors = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
#get grid coordinates from node_id
x, y = self.discrete_env.NodeIdToGridCoord(node_id)
#Check that grid coordinates are within boundries
xlim, ylim = self.discrete_env.num_cells
if (node_id < 0 or node_id > xlim * ylim - 1):
return None
#If neighboing node is withing bounds, add to successors
temp = [[x - 1, y], [x + 1, y], [x, y - 1], [x, y + 1]]
for s in temp:
#Check that robot is within the environment
if s[0] >= 0 and s[0] < xlim and s[1] >= 0 and s[1] < ylim:
#Collision checking: Update robot transform
x, y = self.discrete_env.GridCoordToConfiguration([s[0], s[1]])
transform = np.array([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, 0],
[0, 0, 0, 1]])
self.robot.SetTransform(transform)
#Collision checking: Check collisions
if self.robot.GetEnv().CheckCollision(self.robot) == False:
successors.append(self.discrete_env.GridCoordToNodeId(s))
return successors
def ComputeDistance(self, start_id, end_id):
dist = 0
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
s_config = np.asarray(
self.discrete_env.NodeIdToConfiguration(start_id))
e_config = np.asarray(self.discrete_env.NodeIdToConfiguration(end_id))
dist = LA.norm(e_config - s_config)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
s_config = np.asarray(
self.discrete_env.NodeIdToConfiguration(start_id))
g_config = np.asarray(self.discrete_env.NodeIdToConfiguration(goal_id))
cost = LA.norm(g_config - s_config)
return cost
def InitializePlot(self, goal_config):
self.fig = pl.figure()
pl.xlim([self.lower_limits[0], self.upper_limits[0]])
pl.ylim([self.lower_limits[1], self.upper_limits[1]])
pl.plot(goal_config[0], goal_config[1], 'gx')
# Show all obstacles in environment
for b in self.robot.GetEnv().GetBodies():
if b.GetName() == self.robot.GetName():
continue
bb = b.ComputeAABB()
pl.plot([
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] - bb.extents()[0]
], [
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] - bb.extents()[1]
], 'r')
pl.ion()
pl.show()
def PlotEdge(self, sconfig, econfig):
pl.plot([sconfig[0], econfig[0]], [sconfig[1], econfig[1]],
'k.-',
linewidth=2.5)
pl.draw()
# # test
# env = openravepy.Environment()
# robot = SimpleRobot(env)
# se = SimpleEnvironment(robot, 1.)
# # 1
# print se.GetSuccessors(99)
# #2
# print se.ComputeDistance(0, 9)
# #3
# print se.ComputeHeuristicCost(0, 9)
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.herb = herb
self.robot = herb.robot
self.boundary_limits = [[-5., -5., -numpy.pi], [5., 5., numpy.pi]]
self.lower_limits, self.upper_limits = self.boundary_limits
print self.lower_limits
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
self.resolution = resolution
self.ConstructActions()
def GenerateFootprintFromControl(self,
start_config,
control,
stepsize=0.01):
# Extract the elements of the control
ul = control.ul
ur = control.ur
dt = control.dt
# Initialize the footprint
config = start_config.copy()
footprint = [numpy.array([0., 0., config[2]])]
timecount = 0.0
while timecount < dt:
# Generate the velocities based on the forward model
xdot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.cos(
config[2])
ydot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.sin(
config[2])
tdot = self.herb.wheel_radius * (ul -
ur) / self.herb.wheel_distance
# Feed forward the velocities
if timecount + stepsize > dt:
stepsize = dt - timecount
config = config + stepsize * numpy.array([xdot, ydot, tdot])
if config[2] > numpy.pi:
config[2] -= 2. * numpy.pi
if config[2] < -numpy.pi:
config[2] += 2. * numpy.pi
footprint_config = config.copy()
footprint_config[:2] -= start_config[:2]
footprint.append(footprint_config)
timecount += stepsize
# Add one more config that snaps the last point in the footprint to the center of the cell
nid = self.discrete_env.ConfigurationToNodeId(config)
snapped_config = self.discrete_env.NodeIdToConfiguration(nid)
snapped_config[:2] -= start_config[:2]
footprint.append(snapped_config)
return footprint
def PlotActionFootprints(self, idx):
actions = self.actions[idx]
fig = pl.figure()
lower_limits, upper_limits = self.boundary_limits
pl.xlim([lower_limits[0], upper_limits[0]])
pl.ylim([lower_limits[1], upper_limits[1]])
for action in actions:
xpoints = [config[0] for config in action.footprint]
ypoints = [config[1] for config in action.footprint]
pl.plot(xpoints, ypoints, 'k')
pl.ion()
pl.show()
def ConstructActions(self):
# Actions is a dictionary that maps orientation of the robot to
# an action set
self.actions = dict()
wc = [0., 0., 0.]
grid_coordinate = self.discrete_env.ConfigurationToGridCoord(wc)
# Iterate through each possible starting orientation
for idx in range(int(self.discrete_env.num_cells[2])):
self.actions[idx] = []
grid_coordinate[2] = idx
start_config = self.discrete_env.GridCoordToConfiguration(
grid_coordinate)
#print start_config
# TODO: Here you will construct a set of actionson
# to be used during the planning process
#
# Since an action is composed of only 3 variables (left, right, and duration),
# It is not unreasonable to do a comprehensive action generation.
omega_range = 1
# min/max velocity
resolution = 0.1
n_pts = int(omega_range * 2 / resolution)
omega = numpy.linspace(-omega_range, omega_range, n_pts)
duration = 1 # Can make this a range for even more options
# Generate all combinations of left and right wheel velocities
for om_1 in omega:
for om_2 in omega:
ctrl = Control(om_1, om_2, duration)
footprint = self.GenerateFootprintFromControl(
start_config, ctrl, stepsize=0.01)
this_action = Action(ctrl, footprint)
self.actions[idx].append(this_action)
def GetSuccessors(self, node_id):
successors = {}
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids and controls that represent the neighboring
# nodes
currentConfiguration = self.discrete_env.NodeIdToConfiguration(node_id)
currentCoord = self.discrete_env.NodeIdToGridCoord(node_id)
#print currentConfiguration
#print currentCoord
#print currentCoord[2]
for action in self.actions[currentCoord[2]]:
successorNodeid = self.discrete_env.ConfigurationToNodeId(
currentConfiguration +
action.footprint[len(action.footprint) - 1])
successors[successorNodeid] = action
#print successors
return successors
def ComputeDistance(self, start_id, end_id):
dist = 0
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
start_config = numpy.array(
self.discrete_env.NodeIdToConfiguration(start_id))
end_config = numpy.array(
self.discrete_env.NodeIdToConfiguration(end_id))
dist = numpy.linalg.norm(start_config - end_config)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
# Heuristic Cost in uncertain for this case with three resolution,
# I will update this part later
start_coord = self.discrete_env.NodeIdToGridCoord(start_id)
goal_coord = self.discrete_env.NodeIdToGridCoord(goal_id)
cost = 0
for i in range(len(start_coord)):
cost = cost + abs(goal_coord[i] -
start_coord[i]) * self.discrete_env.resolution[i]
#cost = cost*self.discrete_env.resolution
return cost
def PrintActions(self):
# for key in self.actions.keys():
key = 0
# Just choose one key for sake of example
for action in self.actions[key]:
c = action.control
print "(%.2f %.2f) %.2f s" % (c.ul, c.ur, c.dt)
def PlotEdge2(self, sconfig, econfig, color, size):
pl.plot([sconfig[0], econfig[0]], [sconfig[1], econfig[1]],
color,
linewidth=size)
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits = [-5., -5.]
self.upper_limits = [5., 5.]
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits, self.upper_limits)
# add an obstacle
table = self.robot.GetEnv().ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = numpy.array([[ 0, 0, -1, 1.5],
[-1, 0, 0, 0],
[ 0, 1, 0, 0],
[ 0, 0, 0, 1]])
table.SetTransform(table_pose)
self.goal_config = None
self.p = 0
def GetSuccessors(self, node_id):
successors = []
config = self.discrete_env.NodeIdToGridCoord(node_id)
for i in range(0,self.discrete_env.dimension):
config[i] += 1
if config[i] < self.discrete_env.num_cells[i]:
successors.append(self.discrete_env.GridCoordToNodeId(config))
config[i] -= 2
if config[i] >= 0:
successors.append(self.discrete_env.GridCoordToNodeId(config))
config[i] += 1
return successors
def GetValidSuccessors(self, node_id):
successors = self.GetSuccessors(node_id)
successors = [x for x in successors if self.ComputeDistanceBetweenIds(node_id,x) != float("inf")]
return successors
def ComputeDistanceBetweenIds(self, start_id, end_id):
epsilon = 0.01
start = self.discrete_env.NodeIdToConfiguration(start_id)
end = self.discrete_env.NodeIdToConfiguration(end_id)
with self.robot:
tform = self.robot.GetTransform()
vec = end-start
veclen = numpy.linalg.norm(vec)
vec = vec/veclen
travel = 0
while travel < veclen:
val = start + vec*travel
tform[0:2,3] = val
self.robot.SetTransform(tform)
if self.robot.GetEnv().CheckCollision(self.robot) == True:
return float("inf")
travel += epsilon
return self.ComputeHeuristicCost(start_id, end_id)
def ComputeHeuristicCost(self, start_id, goal_id):
start = numpy.array(self.discrete_env.NodeIdToConfiguration(start_id))
end = numpy.array(self.discrete_env.NodeIdToConfiguration(goal_id))
return numpy.linalg.norm(end-start)
def InitializePlot(self, goal_config):
self.fig = pl.figure()
pl.xlim([self.lower_limits[0], self.upper_limits[0]])
pl.ylim([self.lower_limits[1], self.upper_limits[1]])
pl.plot(goal_config[0], goal_config[1], 'gx')
# Show all obstacles in environment
for b in self.robot.GetEnv().GetBodies():
if b.GetName() == self.robot.GetName():
continue
bb = b.ComputeAABB()
pl.plot([bb.pos()[0] - bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] - bb.extents()[0]],
[bb.pos()[1] - bb.extents()[1],
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] - bb.extents()[1]], 'r')
pl.ion()
pl.show()
def PlotEdge(self, sconfig, econfig):
pl.plot([sconfig[0], econfig[0]],
[sconfig[1], econfig[1]],
'k.-', linewidth=2.5)
pl.draw()
def SetGoalParameters(self, goal_config, p = 0.05):
self.goal_config = goal_config
self.p = p
def GenerateRandomConfiguration(self):
config = [0] * 2;
import numpy
lower_limits = numpy.array(self.lower_limits)
upper_limits = numpy.array(self.upper_limits)
choice = numpy.random.rand(1)
if choice < self.p:
config = self.goal_config
else:
COLLISION = True
while COLLISION:
config = numpy.random.rand(2)*(upper_limits - lower_limits) + lower_limits
# Check if it is collision free
with self.robot:
robot_pos = self.robot.GetTransform()
robot_pos[0:2,3] = config
self.robot.SetTransform(robot_pos)
if self.robot.GetEnv().CheckCollision(self.robot) == False:
COLLISION = False
return numpy.array(config)
def ComputeDistance(self, start_config, end_config):
return numpy.linalg.norm(end_config - start_config)
def Extend(self, start_config, end_config):
#number of steps
epsilon = .01
dist = self.ComputeDistance(start_config, end_config)
numSteps = math.ceil(dist / epsilon)
step = (end_config - start_config) / numSteps
best_config = None
i = 1
while i <= numSteps:
cur_config = start_config+step*i
# Check if it is collision free
with self.robot:
robot_pos = self.robot.GetTransform()
robot_pos[0:2,3] = cur_config
self.robot.SetTransform(robot_pos)
if self.robot.GetEnv().CheckCollision(self.robot) == True:
return best_config
#update variables
best_config = cur_config
i += 1
return end_config
def ShortenPath(self, path, timeout=5.0):
print('starting path shortening')
start_time = time.time()
current_time = 0
p1 = [0,0]
p2 = [0,0]
i = 0
while current_time < timeout and i == 0:
# print("NOW IN THE FUNCTION LOOP")
badIndeces = []
pathLength = len(path)
p1index = random.randint(0,pathLength-2)
p2index = p1index
while p2index == p1index or abs(p2index-p1index) <= 1:
p2index = random.randint(0,pathLength-2)
if p1index > p2index:
p1index, p2index = p2index,p1index
# print(p1index)
# print(p2index)
# print(pathLength)
p1a = numpy.array(path[p1index])
p1b = numpy.array(path[p1index+1])
p2a = numpy.array(path[p2index])
p2b = numpy.array(path[p2index+1])
#choose random interpolation between points
p1Vec = (p1b-p1a)
p2Vec = (p2b-p2a)
p1 = p1a + (p1Vec*random.random())
p2 = p2a + (p2Vec*random.random())
#TODO use extend to move to the two random points
extender = self.Extend(p1,p2)
if extender is not None:
if self.ComputeDistance(extender,p2) < .01:
#TODO get new path
# print('shortening path now')
# print(len(path))
badIndeces = [p1index+1,p2index]
# print(len(path[badIndeces[0]:badIndeces[1]]))
if (badIndeces[1]-badIndeces[0]) != 0:
path[badIndeces[0]:badIndeces[1]] = []
else:
path[badIndeces[0]] = []
# print len(path)
path[badIndeces[0]] = p1
path.insert(badIndeces[0]+1,p2)
# print('new path created')
# print(path)
#TODO repeate until timeout
current_time = time.time()-start_time
i = 0
return path
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits = [-5., -5.]
self.upper_limits = [5., 5.]
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits, self.upper_limits)
self.env1 = self.robot.GetEnv()
# add an obstacle
table = self.robot.GetEnv().ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = numpy.array([[ 0, 0, -1, 1.5],
[-1, 0, 0, 0],
[ 0, 1, 0, 0],
[ 0, 0, 0, 1]])
table.SetTransform(table_pose)
def GetSuccessors(self, node_id):
successors = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
node=self.discrete_env.NodeIdToGridCoord(node_id)
for i in range(0,numpy.size(node,0)):
node_add = list(node)
node_add[i] = node_add[i]+1
node_add_id = self.discrete_env.GridCoordToNodeId(node_add)
if (node_add_id !=-1 and self.IsInLimits(node_add_id)==True and self.IsInCollision(node_add_id)!=True):
successors.append(node_add_id)
for j in range(0,numpy.size(node,0)):
node_minus = list(node)
node_minus[j] = node_minus[j]-1
node_minus_id = self.discrete_env.GridCoordToNodeId(node_minus)
if (node_minus_id !=-1 and self.IsInLimits(node_minus_id)==True and self.IsInCollision(node_minus_id)!=True):
successors.append(node_minus_id)
return successors
def ComputeDistance(self, start_id, end_id):
dist = 0
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
start_grid = self.discrete_env.NodeIdToGridCoord(start_id)
end_grid = self.discrete_env.NodeIdToGridCoord(end_id)
#dist = numpy.linalg.norm(start_config-end_config)
dist = scipy.spatial.distance.cityblock(start_grid,end_grid)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
start_grid = self.discrete_env.NodeIdToGridCoord(start_id)
goal_grid = self.discrete_env.NodeIdToGridCoord(goal_id)
cost = scipy.spatial.distance.cityblock(start_grid,goal_grid)
return cost
def InitializePlot(self, goal_config):
self.fig = pl.figure()
pl.xlim([self.lower_limits[0], self.upper_limits[0]])
pl.ylim([self.lower_limits[1], self.upper_limits[1]])
pl.plot(goal_config[0], goal_config[1], 'gx')
# Show all obstacles in environment
for b in self.robot.GetEnv().GetBodies():
if b.GetName() == self.robot.GetName():
continue
bb = b.ComputeAABB()
pl.plot([bb.pos()[0] - bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] - bb.extents()[0]],
[bb.pos()[1] - bb.extents()[1],
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] - bb.extents()[1]], 'r')
pl.ion()
pl.show()
def PlotEdge(self, sconfig, econfig):
pl.plot([sconfig[0], econfig[0]],
[sconfig[1], econfig[1]],
'k.-', linewidth=2.5)
pl.draw()
def PlotAll(self,plan):
lines=[]
for i in range(0,numpy.size(plan,0)-1):
start = plan[i]
end = plan[i+1]
temp = [start,end]
pl.plot([start[0], end[0]],
[start[1], end[1]],
'k.-', linewidth=2.5)
pl.draw()
def IsInCollision(self, node_id):
orig_config = self.robot.GetTransform()
location = self.discrete_env.NodeIdToConfiguration(node_id)
config =orig_config
config[:2,3]=location
env = self.robot.GetEnv()
with env:
self.robot.SetTransform(config)
collision = env.CheckCollision(self.robot)
with env:
self.robot.SetTransform(orig_config)
return collision
def IsInLimits(self, node_id):
config = self.discrete_env.NodeIdToConfiguration(node_id)
if config == -1:
return False
for idx in range(len(config)):
if config[idx] < self.lower_limits[idx] or config[idx] > self.upper_limits[idx]:
return False
return True
def SetGoalParameters(self, goal_config, p = 0.2):
self.goal_config = goal_config
self.p = p
def GenerateRandomConfiguration(self):
config = [0] * 2;
#
# TODO: Generate and return a random configuration
#
config[0] = numpy.random.uniform(low=self.lower_limits[0], high=self.upper_limits[0]);
config[1] = numpy.random.uniform(low=self.lower_limits[1], high=self.upper_limits[1]);
return numpy.array(config)
def Extend(self, start_config, end_config):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
origTransform = self.robot.GetTransform()
steps = 10;
xSteps = numpy.linspace(start_config[0], end_config[0], (steps + 1));
ySteps = numpy.linspace(start_config[1], end_config[1], (steps + 1));
for i in range(steps + 1):
transform = self.robot.GetTransform()
transform[0, 3] = xSteps[i]
transform[1, 3] = ySteps[i]
self.robot.SetTransform(transform);
for body in self.robot.GetEnv().GetBodies():
if (body.GetName() != self.robot.GetName() and
self.robot.GetEnv().CheckCollision(body, self.robot)):
#self.robot.SetTransform(origTransform);
if (i == 0):
return None;
else:
return [xSteps[i-1], ySteps[i-1]]
#self.robot.SetTransform(origTransform)
return end_config
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits = [-5., -5.]
self.upper_limits = [5., 5.]
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits, self.upper_limits)
# add an obstacle
table = self.robot.GetEnv().ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = numpy.array([[ 0, 0, -1, 1.5],
[-1, 0, 0, 0],
[ 0, 1, 0, 0],
[ 0, 0, 0, 1]])
table.SetTransform(table_pose)
self.env = self.robot.GetEnv()
self.resolution = resolution
def CheckConfigCollision(self, config):
t = self.robot.GetTransform()
t[:2,3] = numpy.array(config)
self.robot.SetTransform(t)
return self.env.CheckCollision(self.robot)
def GetSuccessors(self, node_id):
successors = []
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
coord = self.discrete_env.NodeIdToGridCoord(node_id)
current_config = self.discrete_env.NodeIdToConfiguration(node_id)
for idx in range(self.discrete_env.dimension):
if coord[idx] - 1 >= 0:
neighbor_coord = coord[:]
neighbor_coord[idx] -= 1
neighbor_config = self.discrete_env.GridCoordToConfiguration(neighbor_coord)
if not self.CheckConfigCollision(neighbor_config):
successors.append(self.discrete_env.GridCoordToNodeId(neighbor_coord))
if coord[idx] + 1 <= self.discrete_env.num_cells[idx] - 1:
neighbor_coord = coord[:]
neighbor_coord[idx] += 1
neighbor_config = self.discrete_env.GridCoordToConfiguration(neighbor_coord)
if not self.CheckConfigCollision(neighbor_config):
successors.append(self.discrete_env.GridCoordToNodeId(neighbor_coord))
return successors
def ComputeDistance(self, start_id, end_id):
# computes the distance between the configurations given
# by the two node ids
start_config = numpy.array(self.discrete_env.NodeIdToGridCoord(start_id))
end_config = numpy.array(self.discrete_env.NodeIdToGridCoord(end_id))
dist = numpy.linalg.norm(end_config-start_config)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
# computes the heuristic cost between the configurations
# given by the two node ids
cost = self.ComputeDistance(start_id, goal_id)
return cost
def InitializePlot(self, goal_config):
self.fig = pl.figure()
pl.xlim([self.lower_limits[0], self.upper_limits[0]])
pl.ylim([self.lower_limits[1], self.upper_limits[1]])
pl.plot(goal_config[0], goal_config[1], 'gx')
# Show all obstacles in environment
for b in self.robot.GetEnv().GetBodies():
if b.GetName() == self.robot.GetName():
continue
bb = b.ComputeAABB()
pl.plot([bb.pos()[0] - bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] - bb.extents()[0]],
[bb.pos()[1] - bb.extents()[1],
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] - bb.extents()[1]], 'r')
pl.ion()
pl.show()
def PlotEdge(self, sconfig, econfig):
pl.plot([sconfig[0], econfig[0]],
[sconfig[1], econfig[1]],
'k.-', linewidth=2.5)
pl.draw()
class HerbEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.manip = herb.manip
self.lower_limits, self.upper_limits = self.robot.GetActiveDOFLimits()
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
# account for the fact that snapping to the middle of the grid cell may put us over our
# upper limit
upper_coord = [x - 1 for x in self.discrete_env.num_cells]
upper_config = self.discrete_env.GridCoordToConfiguration(upper_coord)
for idx in range(len(upper_config)):
self.discrete_env.num_cells[idx] -= 1
# add a table and move the robot into place
table = self.robot.GetEnv().ReadKinBodyXMLFile(
'models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = numpy.array([[0, 0, -1, 0.7], [-1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 0, 1]])
table.SetTransform(table_pose)
# set the camera
camera_pose = numpy.array(
[[0.3259757, 0.31990565, -0.88960678, 2.84039211],
[0.94516159, -0.0901412, 0.31391738, -0.87847549],
[0.02023372, -0.9431516, -0.33174637, 1.61502194],
[0., 0., 0., 1.]])
self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
self.env = self.robot.GetEnv()
self.resolution = resolution
def CheckConfigCollision(self, config):
# t = self.robot.GetTransform()
# t[:2,3] = numpy.array(config)
# self.robot.SetTransform(t)
self.robot.SetActiveDOFs(self.manip.GetArmIndices())
self.robot.SetActiveDOFValues(config)
return self.env.CheckCollision(self.robot)
def GetSuccessors(self, node_id):
successors = []
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
coord = self.discrete_env.NodeIdToGridCoord(node_id)
current_config = self.discrete_env.NodeIdToConfiguration(node_id)
for idx in range(self.discrete_env.dimension):
if coord[idx] - 1 >= 0:
neighbor_coord = coord[:]
neighbor_coord[idx] -= 1
neighbor_config = self.discrete_env.GridCoordToConfiguration(
neighbor_coord)
if not self.CheckConfigCollision(neighbor_config):
successors.append(
self.discrete_env.GridCoordToNodeId(neighbor_coord))
if coord[idx] + 1 <= self.discrete_env.num_cells[idx] - 1:
neighbor_coord = coord[:]
neighbor_coord[idx] += 1
neighbor_config = self.discrete_env.GridCoordToConfiguration(
neighbor_coord)
if not self.CheckConfigCollision(neighbor_config):
successors.append(
self.discrete_env.GridCoordToNodeId(neighbor_coord))
return successors
def ComputeDistance(self, start_id, end_id):
# computes the distance between the configurations given
# by the two node ids
start_config = numpy.array(
self.discrete_env.NodeIdToGridCoord(start_id))
end_config = numpy.array(self.discrete_env.NodeIdToGridCoord(end_id))
dist = numpy.linalg.norm(end_config - start_config)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
# computes the heuristic cost between the configurations
# given by the two node ids
cost = self.ComputeDistance(start_id, goal_id)
return cost
class RandomEnvironment(object):
def __init__(self, robot, resolution, start_config, goal_config):
self.robot = robot.robot
self.resolution = resolution
#self.lower_limits, self.upper_limits = self.robot.GetActiveDOFLimits()
self.lower_limits = [-5, -5]
self.upper_limits = [5., 5.]
self.start_config = start_config
self.goal_config = goal_config
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
# account for the fact that snapping to the middle of the grid cell may put us over our
# upper limit
upper_coord = [x - 1 for x in self.discrete_env.num_cells]
upper_config = self.discrete_env.GridCoordToConfiguration(upper_coord)
for idx in range(len(upper_config)):
self.discrete_env.num_cells[idx] -= 1
# set the camera
camera_pose = numpy.array(
[[0.3259757, 0.31990565, -0.88960678, 2.84039211],
[0.94516159, -0.0901412, 0.31391738, -0.87847549],
[0.02023372, -0.9431516, -0.33174637, 1.61502194],
[0., 0., 0., 1.]])
#self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
self.p = 0.4
#maxIter = max number of iterations to try to place an obstacle without collisions
def genNewEnvironment(self,
numObjects,
xAxisIndex=0,
yAxisIndex=1,
save=True,
filename='',
maxIter=100):
self.obstacles = [None] * numObjects
count = numObjects
for i in range(0, numObjects):
# Read in obstacle file
self.obstacles[i] = (self.robot.GetEnv().ReadKinBodyXMLFile(
'models/objects/obs.kinbody.xml'))
#Add obstacle to the environment
self.robot.GetEnv().AddKinBody(self.obstacles[i], True)
#Check if it is in collision
collision = True
iter = 0
while collision:
iter += 1
#Choose random location within the environment
rand = numpy.random.rand(3, 1)
obsX = rand[0]
obsY = rand[1]
rot = numpy.round(rand[2] / .25) * numpy.pi / 2
obsX = (self.upper_limits[xAxisIndex] -
self.lower_limits[xAxisIndex]
) * obsX + self.lower_limits[xAxisIndex]
obsY = (self.upper_limits[yAxisIndex] -
self.lower_limits[yAxisIndex]
) * obsY + self.lower_limits[yAxisIndex]
#Generate Transform Matrix
pose = numpy.array([[numpy.cos(rot),
numpy.sin(rot), 0, obsX],
[-numpy.sin(rot),
numpy.cos(rot), 0, obsY], [0, 0, 1, 0],
[0, 0, 0, 1]])
#Apply Transform
self.obstacles[i].SetTransform(pose)
#Check for collision
collision = self.robot.GetEnv().CheckCollision(
self.obstacles[i])
#Check that obstacle isn't covering the start or goal position
bb = self.obstacles[i].ComputeAABB()
pos = bb.pos()
extents = bb.extents()
L = pos[0] - extents[0]
R = pos[0] + extents[0]
B = pos[1] - extents[1]
T = pos[1] + extents[1]
if self.start_config[0] > L and self.start_config[0] < R:
if self.start_config[1] > B and self.start_config[1] < T:
collision = True
continue
if self.goal_config[0] > L and self.goal_config[0] < R:
if self.goal_config[1] > B and self.goal_config[1] < T:
collision = True
continue
if iter >= maxIter:
rem = self.robot.GetEnv.Remove(self.obstacles[i])
count -= 1
print "Environment created with " + str(count) + " obstacles..."
#Save environment
if save:
if not filename:
now = datetime.datetime.now()
filename = now.strftime("environments/%m%d%Y_%H%M%S")
#Save environment
self.robot.GetEnv().Save(filename)
self.env_filename = filename
def GetSuccessors(self, node_id):
# - ----------------------4 CONNECTED VERSION ------------------------
successors = []
coord = [0] * self.discrete_env.dimension
new_coord = [0] * self.discrete_env.dimension
coord = self.discrete_env.NodeIdToGridCoord(node_id)
for i in range(self.discrete_env.dimension):
new_coord = list(coord)
new_coord[i] = coord[i] - 1
new_config = self.discrete_env.GridCoordToConfiguration(new_coord)
flag = True
for j in range(len(new_config)):
if (new_config[j] > self.upper_limits[j]
or new_config[j] < self.lower_limits[j]):
flag = False
if flag == True and not self.collision_check(
self.discrete_env.GridCoordToConfiguration(new_coord)):
successors.append(
self.discrete_env.GridCoordToNodeId(new_coord))
for i in range(self.discrete_env.dimension):
new_coord = list(coord)
new_coord[i] = coord[i] + 1
new_config = self.discrete_env.GridCoordToConfiguration(new_coord)
flag = True
for j in range(len(new_config)):
if (new_config[j] > self.upper_limits[j]
or new_config[j] < self.lower_limits[j]):
flag = False
if flag == True and not self.collision_check(
self.discrete_env.GridCoordToConfiguration(new_coord)):
successors.append(
self.discrete_env.GridCoordToNodeId(new_coord))
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
return successors
#----------------------------8 CONNECTED VERSION -----------------------------------
# successors = []
# coord = [0]*2
# new_coord = [0]*2
#
# coord = self.discrete_env.NodeIdToGridCoord(node_id)
# steps = [[0,1],[0,-1],[-1,0],[1,0],[1,1],[1,-1],[-1,1],[-1,-1]]
# #print node_id
# #print coord
# #print self.discrete_env.GridCoordToNodeId([-0.05,0.05])
# #print self.discrete_env.GridCoordToConfiguration(coord)
# for step in steps:
# new_coord = [coord[0] + step[0] ,coord[1] + step[1]]
# new_config = self.discrete_env.GridCoordToConfiguration(new_coord)
# #print new_config
# flag = True
# for j in range(len(new_config)):
# if (new_config[j] > self.upper_limits[j] or new_config[j] < self.lower_limits[j]):
# flag = False
#
# if flag == True and not self.collision_check(self.discrete_env.GridCoordToConfiguration(new_coord)):
# successors.append(self.discrete_env.GridCoordToNodeId(new_coord))
# return successors
#print successors
def ComputeDistance(self, start_id, end_id):
dist = 0
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(end_id)
dist = numpy.linalg.norm(
numpy.array(start_config) - numpy.array(end_config))
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
goal_config = self.discrete_env.NodeIdToConfiguration(goal_id)
cost = numpy.linalg.norm(
numpy.array(start_config) - numpy.array(goal_config))
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
return cost
def InitializePlot(self, updatePeriod=.1):
self.fig = pl.figure()
pl.xlim([self.lower_limits[0], self.upper_limits[0]])
pl.ylim([self.lower_limits[1], self.upper_limits[1]])
pl.plot(self.start_config[0], self.start_config[1], 'gx')
pl.plot(self.goal_config[0], self.goal_config[1], 'rx')
# Show all obstacles in environment
for b in self.robot.GetEnv().GetBodies():
if b.GetName() == self.robot.GetName():
continue
bb = b.ComputeAABB()
pl.plot([
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] - bb.extents()[0]
], [
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] - bb.extents()[1]
], 'r')
self.updatePeriod = updatePeriod
self.updateTime = time.time() + self.updatePeriod
pl.ion()
pl.show()
def savePlot(self, filename):
pl.savefig(filename)
def updatePlotAnnotation(self, text):
self.fig.suptitle(text)
def PlotPoint(self, config):
pl.plot(config[0], config[1], 'b*')
if time.time() > self.updateTime:
pl.draw()
self.updateTime = self.updatePeriod + time.time()
def PlotEdge(self, sconfig, econfig):
pl.plot([sconfig[0], econfig[0]], [sconfig[1], econfig[1]],
'k.-',
linewidth=2.5)
if time.time() > self.updateTime:
pl.draw()
self.updateTime = self.updatePeriod + time.time()
def PlotRedEdge(self, sconfig, econfig):
pl.plot([sconfig[0], econfig[0]], [sconfig[1], econfig[1]],
'r.-',
linewidth=2.5)
if time.time() > self.updateTime:
pl.draw()
self.updateTime = self.updatePeriod + time.time()
def collision_check(self, config):
robot_pose = numpy.array([[1, 0, 0, config[0]], [0, 1, 0, config[1]],
[0, 0, 1, 0], [0, 0, 0, 1]])
self.robot.SetTransform(robot_pose)
return self.robot.GetEnv().CheckCollision(self.robot)
class HerbEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.resolution = resolution
self.lower_limits, self.upper_limits = self.robot.GetActiveDOFLimits()
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
# account for the fact that snapping to the middle of the grid cell may put us over our
# upper limit
upper_coord = [x - 1 for x in self.discrete_env.num_cells]
upper_config = self.discrete_env.GridCoordToConfiguration(upper_coord)
for idx in range(len(upper_config)):
self.discrete_env.num_cells[idx] -= 1
# add a table and move the robot into place
self.table = self.robot.GetEnv().ReadKinBodyXMLFile(
'models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(self.table)
table_pose = numpy.array([[0, 0, -1, 0.7], [-1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 0, 1]])
self.table.SetTransform(table_pose)
# set the camera
camera_pose = numpy.array(
[[0.3259757, 0.31990565, -0.88960678, 2.84039211],
[0.94516159, -0.0901412, 0.31391738, -0.87847549],
[0.02023372, -0.9431516, -0.33174637, 1.61502194],
[0., 0., 0., 1.]])
self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
self.p = 0.4
def GetSuccessors(self, node_id):
successors = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
# Implement using an eight connected world
# Function to determine if the robot is in collision
def collision_check(config):
self.robot.SetDOFValues(config, self.robot.GetActiveDOFIndices())
return self.robot.GetEnv().CheckCollision(
self.robot, self.table) or self.robot.CheckSelfCollision()
def extend_collision_check(start_config, end_config):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
steps = self.ComputeDistance(
start_config, end_config
) * 10 # Get 10 times the number of unit values in the distance space
path = [
numpy.linspace(start_config[i], end_config[i], steps)
for i in range(0, len(self.robot.GetActiveDOFIndices()))
]
path = numpy.array(path)
for i in xrange(0, len(path[0, :])):
config = numpy.array([[path[0, i]], [path[1, i]], [path[2, i]],
[path[3, i]], [path[4, i]], [path[5, i]],
[path[6, i]]])
if (collision_check(config)):
return True
return False
# Function to determine if the perterbed config is within the grid
def valid_id(id, start_config):
end_config = self.discrete_env.NodeIdToConfiguration(id)
if (extend_collision_check(start_config, end_config)):
return False
for i in xrange(0, len(self.lower_limits)):
if (end_config[i] > self.upper_limits[i]
or end_config[i] < self.lower_limits[i]):
return False # if it goes out of bounds
return True # if it doesn't go out of bounds
coord = self.discrete_env.NodeIdToGridCoord(node_id)
config = self.discrete_env.NodeIdToConfiguration(node_id)
# Return nothing if the id is in collision or out of bounds
if collision_check(config):
return []
for i in xrange(0, len(self.lower_limits)):
if (config[i] > self.upper_limits[i]
or config[i] < self.lower_limits[i]):
return []
# Perterb the configuration
count = 0
for i in xrange(0, len(coord)):
perterbed_coord1 = list(coord)
perterbed_coord2 = list(coord)
perterbed_coord1[i] = perterbed_coord1[i] + 1
perterbed_coord2[i] = perterbed_coord2[i] - 1
perterbed_id1 = self.discrete_env.GridCoordToNodeId(
perterbed_coord1)
perterbed_id2 = self.discrete_env.GridCoordToNodeId(
perterbed_coord2)
if (perterbed_id1 != node_id):
if (valid_id(perterbed_id1, config)):
# Return the robot to old config
self.robot.SetDOFValues(config,
self.robot.GetActiveDOFIndices(),
True)
# Append the successor
successors.append(perterbed_id1)
if (perterbed_id2 != node_id):
if (valid_id(perterbed_id2, config)):
# Return the robot to old config
self.robot.SetDOFValues(config,
self.robot.GetActiveDOFIndices(),
True)
# Append the successor
successors.append(perterbed_id2)
return successors
def ComputeDistance(self, start_id, end_id):
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
# Use Euclidean distance
start_config = numpy.array(
self.discrete_env.NodeIdToConfiguration(start_id))
end_config = numpy.array(
self.discrete_env.NodeIdToConfiguration(end_id))
return numpy.linalg.norm(end_config - start_config, 2)
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
# Use Euclidean distance
start_config = numpy.array(
self.discrete_env.NodeIdToConfiguration(start_id))
goal_config = numpy.array(
self.discrete_env.NodeIdToConfiguration(goal_id))
weights = [5, 5, 5, 5, 1, 1, 1]
for i in xrange(0, len(start_config)):
#cost += weights[i] * numpy.linalg.norm(goal_config[i] - start_config[i], 2)
cost += weights[i] * (goal_config[i] - start_config[i])**2
return cost**0.5
def SetGoalParameters(self, goal_config, p=0.3):
self.goal_config = goal_config
self.p = p
def collision_check(self, config):
self.robot.SetDOFValues(config, self.robot.GetActiveDOFIndices(), True)
return self.robot.GetEnv().CheckCollision(
self.robot, self.table) or self.robot.CheckSelfCollision()
def GenerateRandomConfiguration(self):
config = [0] * len(self.robot.GetActiveDOFIndices())
#
# TODO: Generate and return a random configuration
#
lower_limits, upper_limits = self.robot.GetActiveDOFLimits()
while (True):
config = [
random.uniform(lower_limits[i], upper_limits[i])
for i in range(0, len(lower_limits))
]
if (self.collision_check(config)):
continue
else:
return numpy.array(config)
def Extend(self, start_config, end_config):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
steps = self.ComputeDistance(
self.discrete_env.ConfigurationToNodeId(start_config),
self.discrete_env.ConfigurationToNodeId(end_config)
) * 25 # Get 25 times the number of unit values in the distance space
path = [
numpy.linspace(start_config[i], end_config[i], steps)
for i in range(0, len(self.robot.GetActiveDOFIndices()))
]
path = numpy.array(path)
if (len(path[0, :])) < 10:
return None
for i in xrange(0, len(path[0, :])):
config = numpy.array([[path[0, i]], [path[1, i]], [path[2, i]],
[path[3, i]], [path[4, i]], [path[5, i]],
[path[6, i]]])
if (self.collision_check(config)):
if (i < 5):
return None
else:
if (self.ComputeDistance(
self.discrete_env.ConfigurationToNodeId(
path[:, int(i / 2)]),
self.discrete_env.ConfigurationToNodeId(config))
) > 0.5:
return numpy.array(path[:, 0:int(i / 2)]).transpose()
else:
return None
# j=i
# while (j>10):
# safe = self.ComputeDistance(self.discrete_env.ConfigurationToNodeId(path[:,j]), self.discrete_env.ConfigurationToNodeId(config))
# if safe > 0.3:
# return numpy.array(path[:, 0:j]).transpose()
# j-=1
# return None
return path.transpose()
def ShortenPath(self, path, timeout=5.0):
#
# TODO: Implement a function which performs path shortening
# on the given path. Terminate the shortening after the
# given timout (in seconds).
#
short_path = list(path)
init_time = time.time()
while time.time() - init_time < timeout:
start = random.randint(0, len(short_path) - 3)
end = random.randint(start + 1, len(short_path) - 1)
extend = self.Extend(short_path[start], short_path[end])
if (extend == None or len(extend) == 0):
continue
else:
if compare(extend[len(extend) - 1, :], short_path[end]):
short_path[start + 1:end] = []
return numpy.array(short_path)
class HerbEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits, self.upper_limits = self.robot.GetActiveDOFLimits()
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits, self.upper_limits)
# account for the fact that snapping to the middle of the grid cell may put us over our
# upper limit
self.upper_coord = [x - 1 for x in self.discrete_env.num_cells]
self.upper_config = self.discrete_env.GridCoordToConfiguration(self.upper_coord)
for idx in range(len(self.upper_config)):
self.discrete_env.num_cells[idx] -= 1
print (self.lower_limits)
print (self.upper_limits)
# add a table and move the robot into place
table = self.robot.GetEnv().ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = np.array([[ 0, 0, -1, 0.7],
[-1, 0, 0, 0],
[ 0, 1, 0, 0],
[ 0, 0, 0, 1]])
table.SetTransform(table_pose)
# set the camera
camera_pose = np.array([[ 0.3259757 , 0.31990565, -0.88960678, 2.84039211],
[ 0.94516159, -0.0901412 , 0.31391738, -0.87847549],
[ 0.02023372, -0.9431516 , -0.33174637, 1.61502194],
[ 0. , 0. , 0. , 1. ]])
self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
def GetSuccessors(self, node_id):
successors = []
temp = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
herb_coord = self.discrete_env.NodeIdToGridCoord(node_id) # 7D
limits = self.discrete_env.num_cells
# all possibilities
for i in range (0, len(herb_coord)):
temp_coord = np.asarray(herb_coord)
temp_coord[i] += 1
temp.append(temp_coord.tolist())
temp_coord = np.asarray(herb_coord)
temp_coord[i] -= 1
temp.append(temp_coord.tolist())
# upper
# [ 0.52359878 -1.97222205 -2.74016693 -0.87266463 -4.79965544 -1.57079633 -3.00196631]
# [ 5.75958653 1.97222205 2.74016693 3.14159265 1.30899694 1.57079633 3.00196631]
# filter
for coord in temp:
flag = True
# check in continuous space
config = self.discrete_env.GridCoordToConfiguration(coord)
for idx, con_num in enumerate(config):
if con_num < self.lower_limits[idx] or con_num > self.upper_limits[idx]:
flag = False
# check each number for negatives and out of range
for i in range (0, len(coord)):
number = coord[i]
# pdb.set_trace()
if number < 0 or number >= limits[i]: # must be one less due to indexing
flag = False
with self.robot:
# test = [4.4505895925855405, -1.6904760469316509, 0.27401669256310957, 1.8871372537188689, -0.98174770424681013, 0.78539816339744806, -1.7511470161676443]
# self.robot.SetActiveDOFValues(test)
self.robot.SetActiveDOFValues(self.discrete_env.GridCoordToConfiguration(coord))
if (self.robot.GetEnv().CheckCollision(self.robot) or \
self.robot.CheckSelfCollision()):
# print 'collision'
flag = False
# pdb.set_trace()
if flag:
successors.append(self.discrete_env.GridCoordToNodeId(coord))
# print successors
# pdb.set_trace()
return successors
def ComputeDistance(self, start_id, end_id):
dist = 0
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
s_config = np.asarray(self.discrete_env.NodeIdToConfiguration(start_id))
e_config = np.asarray(self.discrete_env.NodeIdToConfiguration(end_id))
dist = LA.norm(e_config-s_config)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
s_config = np.asarray(self.discrete_env.NodeIdToConfiguration(start_id))
g_config = np.asarray(self.discrete_env.NodeIdToConfiguration(goal_id))
cost = LA.norm(g_config-s_config)
# cost = 100*LA.norm(g_config-s_config)
return cost
# # test
# env = openravepy.Environment()
# robot = HerbRobot(env, 'right')
# se = HerbEnvironment(robot, 0.1)
# se.GetSuccessors(1)
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits = [-5., -5.]
self.upper_limits = [5., 5.]
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits, self.upper_limits)
# add an obstacle
table = self.robot.GetEnv().ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = np.array([[0, 0, -1, 1.5],
[-1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 0, 1]])
table.SetTransform(table_pose)
# Create the offset, we will use this to calculate neighbors later.
self.offset = np.array([[-1, 0], [0, -1], [1, 0], [0, 1]])
self.config = [0] * 2
self.p = 0.0
self.step = 2
def Crashed(self, end_config):
# Create a 4x4 identity matrix that will be used as a transformation matrix.
# Define the displacement transformation according to the end configuration
newTransform = np.eye(4)
newTransform[0,3] = end_config[0]
newTransform[1,3] = end_config[1]
robot = self.robot
robot.SetTransform(newTransform)
crashed = ((robot.GetEnv().CheckCollision(robot)) or robot.CheckSelfCollision())
if crashed:
# print "crashed!!!!!!!!!!"
return True
else:
return False
def GetSuccessors(self, node_id):
successors = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
grid_coord = self.discrete_env.NodeIdToGridCoord2(node_id)
node_id_list = []
for i in [-1, 1]:
for j in range(self.discrete_env.dimension):
# print("center is {}".format(grid_coord))
center_coord = copy.deepcopy(grid_coord)
center_coord[j]= center_coord[j] + i
near_grid_coord = center_coord
near_grid_config = self.discrete_env.GridCoordToConfiguration(near_grid_coord)
lower_coords = np.zeros(self.discrete_env.dimension).tolist()
dimension = self.discrete_env.dimension
if all(near_grid_coord[i] >= lower_coords[i] for i in range(0,dimension)):
if all(near_grid_coord[i] <= self.discrete_env.num_cells[i] for i in range(0,dimension)):
if any(n < 0 for n in near_grid_coord.tolist()):
print("near coord <0 : {}".format(near_grid_coord))
if self.Crashed(near_grid_config):
print("Crashing!!")
else:
near_node_id = self.discrete_env.GridCoordToNodeId2(near_grid_coord)
node_id_list.append(near_node_id)
successors = node_id_list
return successors
# return successors
def ComputeDistance(self, start_id, end_id):
dist = 0
if self.discrete_env.IdinRange(start_id) and self.discrete_env.IdinRange(end_id):
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
# Start coordinate
start = self.discrete_env.NodeIdToConfiguration(start_id)
# End coordinate
end = self.discrete_env.NodeIdToConfiguration(end_id)
# Calculate euclidean distance between start and end coordinates
dist = np.linalg.norm(start_coord - end_coord)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
# Not sure!!!!!!!!!!!!!!!!!!!!
cost = self.ComputeDistance(start_id, goal_id)
return cost
def InitializePlot(self, goal_config):
self.fig = pl.figure()
pl.xlim([self.lower_limits[0], self.upper_limits[0]])
pl.ylim([self.lower_limits[1], self.upper_limits[1]])
pl.plot(goal_config[0], goal_config[1], 'gx')
# Show all obstacles in environment
for b in self.robot.GetEnv().GetBodies():
if b.GetName() == self.robot.GetName():
continue
bb = b.ComputeAABB()
pl.plot([bb.pos()[0] - bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] - bb.extents()[0]],
[bb.pos()[1] - bb.extents()[1],
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] - bb.extents()[1]], 'r')
pl.ion()
pl.show()
def PlotEdge(self, sconfig, econfig):
pl.plot([sconfig[0], econfig[0]],
[sconfig[1], econfig[1]],
'k.-', linewidth=2.5)
pl.draw()
def ShortenPath(self, path, timeout=5.0):
#
# TODO: Implement a function which performs path shortening
# on the given path. Terminate the shortening after the
# given timout (in seconds).
#
start_time = time.time()
while (True):
# Randomly select 2 points from the list of points on the shortest path
len_path = len(path)-1
q1_, q2_ = np.random.choice(len_path, 2, replace = False)
print("q1_:{}, q2:{}".format(q1_, q2_))
q1, q2 = path[q1_], path[q2_]
'''
We now need to see if we are able to connect the points together and create a
new sample point between the 2. Let us call this sample point qi. The formula
for qi is given by:
qi = (q2 - q1)*i + q1 where i is a float that lies in the range [0,1]
'''
# Default condition is connect = 2,
# If points can't be connected for shortening, connect = 0. If points can be connected for shortening, connect = 1
connect = 2
for i in np.linspace(0.0, 1.0, 3000):
qi = (q2 - q1) * i + q1
self.robot.SetActiveDOFValues(qi)
# If a collision is imminent, then you must not consider shortening from these 2 points
if (self.robot.CheckSelfCollision() or self.robot.GetEnv().CheckCollision(self.robot)):
connect = 0
else:
# If no collision is imminent, go for it
connect = 1
if (connect == 1):
rng = np.arange(q1_ + 1, q2_)
# for j in range(q1_ + 1, q2_, 1):
# print("q1_+1:{}, q2_:{}, size of path:{}".format(q1_+1, q2_, path.shape))
path = np.delete(path, rng, 0)
print("size of path".format(path.size))
current_time = time.time()
if (current_time - start_time >= timeout):
print("exceed timeout")
break
print("shorten path: All step dist: {},and vertices number {} ".format(np.sum(self.step*len(path)), len(path)))
return path
def SetGoalParameters(self, goal_config, p = 0.2):
goal_tf = np.eye(4,dtype = float)
goal_tf[0,3] = goal_config[0]
goal_tf[1,3] = goal_config[1]
self.goal_config = goal_tf
self.p = p
def GenerateRandomConfiguration(self):
# config = [0] * 2;
lower_limits, upper_limits = self.boundary_limits
#
# TODO: Generate and return a random configuration
#
# self.config = numpy.random.uniform(lower_limits, upper_limits)
print("Simple environment")
config = np.eye(4, dtype = float)
config[0,3] = random.uniform(lower_limits[0],upper_limits[1])
config[1,3] = random.uniform(lower_limits[0],upper_limits[1])
# print("config shape {}".format(numpy.shape(self.config)))
self.robot.SetTransform(config)
if not self.robot.GetEnv().CheckCollision(self.robot) and not self.robot.CheckSelfCollision():
return config
else:
print("---------collision")
self.GenerateRandomConfiguration()
return config
def ComputeDistance(self, start_config, end_config):
#
# TODO: Implement a function which computes the distance between
# two configurations
# get x, y from the tf, find the distance and return
dx = start_config[0,3] - end_config[0,3]
dy = start_config[1,3] - end_config[1,3]
dist = np.sqrt(np.power(dx,2)+np.power(dy,2))
return dist
def Extend(self, start_config, end_config):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
# qi, qr, qc are 4*4 ndarray
step_dist = self.step
qi = start_config
qr = end_config
q_start = qi
qi_qr_dist = self.ComputeDistance(qi,qr)
step_ratio = step_dist/qi_qr_dist
qi_qc_vec = (qr - qi) * step_ratio
qc = qi + qi_qc_vec
qi_qc_dist = self.ComputeDistance(qi,qc)
qi_qc_vec_unit = qi_qc_vec/ 20
qi_qc_vec_unit_dist = self.ComputeDistance(qi,qi+qi_qc_vec_unit)
current_dist = self.ComputeDistance(qi,qc)
extend_tf = np.eye(4,dtype = float)
extend_tf[0,3] = 0
extend_tf[1,3] = 0
if qi_qc_dist > qi_qr_dist:
qc = qr
# print("*********************")
# print("current_dist {}, qi_qc_vec_unit_dist{}: ".format(current_dist,qi_qc_vec_unit_dist))
while(current_dist > qi_qc_vec_unit_dist):
current_dist = self.ComputeDistance(qi,qc)
# print("current dist {}".format(current_dist))
qi = qi + qi_qc_vec_unit
extend_tf[0,3] = qi[0,3]
extend_tf[1,3] = qi[1,3]
self.robot.SetTransform(extend_tf)
if not self.robot.GetEnv().CheckCollision(self.robot) and not self.robot.CheckSelfCollision():
# print("No Collision")
pass
else:
print("Collision")
extend_tf[0,3] = q_start[0,3]
extend_tf[1,3] = q_start[1,3]
return q_start
extend_tf[0,3] = qi[0,3]
extend_tf[1,3] = qi[1,3]
return qi
class HerbEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits, self.upper_limits = self.robot.GetActiveDOFLimits()
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
# account for the fact that snapping to the middle of the grid cell may put us over our
# upper limit
upper_coord = [x - 1 for x in self.discrete_env.num_cells]
upper_config = self.discrete_env.GridCoordToConfiguration(upper_coord)
for idx in range(len(upper_config)):
self.discrete_env.num_cells[idx] -= 1
# add a table and move the robot into place
table = self.robot.GetEnv().ReadKinBodyXMLFile(
'models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = numpy.array([[0, 0, -1, 0.7], [-1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 0, 1]])
table.SetTransform(table_pose)
self.table = table
# set the camera
camera_pose = numpy.array(
[[0.3259757, 0.31990565, -0.88960678, 2.84039211],
[0.94516159, -0.0901412, 0.31391738, -0.87847549],
[0.02023372, -0.9431516, -0.33174637, 1.61502194],
[0., 0., 0., 1.]])
self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
self.p = 0.2
def GetSuccessors(self, node_id):
successors = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
coord = self.discrete_env.NodeIdToGridCoord(node_id)
if not self.BoundaryCheck(coord) or self.CollisionCheck(coord):
#print "Failed to find successors"
return successors
successors = self.NeighborCheck(coord)
return successors
def CollisionCheck(self, coord):
config = self.discrete_env.GridCoordToConfiguration(coord)
env = self.robot.GetEnv()
jvalues = self.robot.GetDOFValues()
jvalues[0:7] = config #
with env:
self.robot.SetDOFValues(jvalues)
isCollision = env.CheckCollision(self.robot)
return isCollision
def BoundaryCheck(self, coord):
config = self.discrete_env.GridCoordToConfiguration(coord)
product = True
for i in range(0, len(config)):
product * (config[i] > self.lower_limits[i]
and config[i] < self.upper_limits[i])
#print "boundary check: " + str(product)
return product
def NeighborCheck(self, coord):
neighbors = []
for i in range(0, len(coord)):
new_coord = list(coord)
new_coord[i] = new_coord[i] + 1
#print new_coord, coord
neighbors.append(new_coord)
new_coord = list(coord)
new_coord[i] -= 1
neighbors.append(new_coord)
successors = []
for i in xrange(len(neighbors)):
if not self.CollisionCheck(neighbors[i]) and self.BoundaryCheck(
neighbors[i]):
#print "appending successor"
successors.append(
self.discrete_env.GridCoordToNodeId(neighbors[i]))
return successors
def ComputeDistance(self, start_id, end_id):
dist = 0
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(end_id)
#print "start_config: " + str(start_config)
#print "end_config: " + str(end_config)
total = 0
for i in range(0, len(start_config)):
dx = start_config[i] - end_config[i]
total += dx**2
dist = math.sqrt(total)
#print "dist: " + str(dist)
return dist
def ComputeConfigDistance(self, start_config, end_config):
#
# TODO: Implement a function which computes the distance between
# two configurations
#
dist = numpy.linalg.norm(start_config - end_config)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
cost = self.ComputeDistance(start_id, goal_id)
return cost
#RRT function
def SetGoalParameters(self, goal_config, p=0.2):
self.goal_config = goal_config
self.p = p
def GenerateRandomConfiguration(self):
config = [0] * len(self.robot.GetActiveDOFIndices())
#print self.robot.GetActiveDOFIndices()
legalConfig = False
while (not legalConfig):
# Generate random config
#config=numpy.random.rand(len(self.robot.GetActiveDOFIndices()))
#config=config - .5
#config = 4 * config
# Set joints to random config
lower, upper = self.robot.GetActiveDOFLimits()
for i in range(7):
config[i] = numpy.random.uniform(lower[i], upper[i])
#pdb.set_trace()
self.robot.SetDOFValues(config,
self.robot.GetActiveDOFIndices(),
checklimits=1)
# Check for collisions
# If legal exit loop
selfC = self.robot.CheckSelfCollision()
env = self.robot.GetEnv()
envC = env.CheckCollision(self.robot, self.table)
# print "Self C: " + str(selfC) + " env C: " + str(envC)
if ((not selfC) and (not envC)):
legalConfig = True
return numpy.array(config)
def ExtendN(self, start_config, end_config):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
increment_length = 0.01
step_size = 0.3
current_position = numpy.array([0, 0, 0, 0, 0, 0, 0])
dist = self.ComputeDistance(start_config, end_config)
unit_vector = (end_config - start_config) / dist
increment_dist = unit_vector * increment_length
interpolate_num = int(dist / increment_length)
for i in range(interpolate_num):
config = start_config + increment_dist * (i + 1)
#pdb.set_trace()
self.robot.SetDOFValues(config,
self.robot.GetActiveDOFIndices(),
checklimits=1)
selfC = self.robot.CheckSelfCollision()
env = self.robot.GetEnv()
envC = env.CheckCollision(self.robot, self.table)
if ((selfC) or (envC)):
#One can either move until obstacle (CONNECT), or just discard it (EXTEND)
#return current_position - increment_dist
self.robot.SetDOFValues(start_config,
self.robot.GetActiveDOFIndices(),
checklimits=1)
return None
#if numpy.linalg.norm(current_position - start_config)> step_size:
# return start_config + step_size*unit_vector
#else:
return end_config
def Extend(self, start_config, end_config):
#
# TODO: Implement a function which attempts to extend from
# a start configuration to a goal configuration
#
increment_length = 0.01
step_size = 0.3
current_position = numpy.array([0, 0, 0, 0, 0, 0, 0])
dist = self.ComputeConfigDistance(start_config, end_config)
unit_vector = (end_config - start_config) / dist
increment_dist = unit_vector * increment_length
interpolate_num = int(dist / increment_length)
for i in range(interpolate_num):
config = start_config + increment_dist * (i + 1)
#pdb.set_trace()
self.robot.SetDOFValues(config,
self.robot.GetActiveDOFIndices(),
checklimits=1)
selfC = self.robot.CheckSelfCollision()
env = self.robot.GetEnv()
envC = env.CheckCollision(self.robot, self.table)
if ((selfC) or (envC)):
#One can either move until obstacle (CONNECT), or just discard it (EXTEND)
#return current_position - increment_dist
self.robot.SetDOFValues(start_config,
self.robot.GetActiveDOFIndices(),
checklimits=1)
return None
#if numpy.linalg.norm(current_position - start_config)> step_size:
# return start_config + step_size*unit_vector
#else:
return end_config
def ShortenPath(self, path, timeout=5.0):
start = time.time()
while (time.time() - start) < 50:
increment_length = 0.1
goal_config = path[-1]
l = len(path)
new_path = []
new_path.append(path[0])
for i in range(l):
end_config = path[i + 1]
start_config = path[i]
dist = self.ComputeConfigDistance(start_config, end_config)
unit_vector = (end_config - start_config) / dist
increment_dist = unit_vector * increment_length
interpolate_num = int(dist / increment_length)
new_path.append(path[i])
for j in range(interpolate_num):
current_position = start_config + increment_dist * (j + 1)
check = self.ExtendN(current_position, goal_config)
if check != None:
#pdb.set_trace()
new_path.append(current_position)
new_path.append(goal_config)
break
else:
continue
break
return new_path
return path
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.robot = herb.robot
self.lower_limits = [-5., -5.]
self.upper_limits = [5., 5.]
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
self.resolution = resolution
# add an obstacle
table = self.robot.GetEnv().ReadKinBodyXMLFile(
'models/objects/table.kinbody.xml')
self.robot.GetEnv().Add(table)
table_pose = numpy.array([[0, 0, -1, 1.5], [-1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 0, 1]])
table.SetTransform(table_pose)
def GetSuccessors(self, node_id):
successors = []
tentative_successors = []
robot_env = self.robot.GetEnv()
conf_table = robot_env.GetKinBody('conference_table')
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
this_node_coord = self.discrete_env.NodeIdToGridCoord(node_id)
x = this_node_coord[0]
y = this_node_coord[1]
tentative_successors = [[x - 1, y - 1], [x, y - 1], [x + 1, y - 1],
[x - 1, y], [x + 1, y], [x - 1, y + 1],
[x, y + 1], [x + 1, y + 1]]
for i in range(len(tentative_successors)):
if tentative_successors[i][0] >= 0 and tentative_successors[i][
0] < self.discrete_env.num_cells[
0] and tentative_successors[i][
1] >= 0 and tentative_successors[i][
1] < self.discrete_env.num_cells[1]:
tf = self.robot.GetTransform()
tf_robot = copy.copy(tf)
go_to = self.discrete_env.GridCoordToConfiguration(
tentative_successors[i])
tf[0][3] = go_to[0]
tf[1][3] = go_to[1]
self.robot.SetTransform(tf)
if not robot_env.CheckCollision(self.robot, conf_table):
successors.append(
self.discrete_env.GridCoordToNodeId(
tentative_successors[i]))
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids that represent the neighboring
# nodes
# try dummy way in 2D first, 7D.....We will see
#total_idx = self.discrete_env.num_cells[0]*self.discrete_env.num_cells[1] - 1
self.robot.SetTransform(tf_robot)
'''# left neighbor
left_node_coord = numpy.add(this_node_coord,[-1,0])
left_node_id = self.discrete_env.GridCoordToNodeId(left_node_coord)
# right neighbor
right_node_coord = numpy.add(this_node_coord,[1,0])
right_node_id = self.discrete_env.GridCoordToNodeId(right_node_coord)
# up neighbor
up_node_coord = numpy.add(this_node_coord,[0, -1])
up_node_id = self.discrete_env.GridCoordToNodeId(up_node_coord)
# down neighbor
down_node_coord = numpy.add(this_node_coord,[0, 1])
down_node_id = self.discrete_env.GridCoordToNodeId(down_node_coord)'''
return successors
def ComputeDistance(self, start_id, end_id):
dist = 0
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
start_config = numpy.array(
self.discrete_env.NodeIdToConfiguration(start_id))
end_config = numpy.array(
self.discrete_env.NodeIdToConfiguration(end_id))
dist = numpy.linalg.norm((end_config - start_config))
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
start_grid = self.discrete_env.NodeIdToGridCoord(start_id)
goal_grid = self.discrete_env.NodeIdToGridCoord(goal_id)
# start_grid=self.discrete_env.NodeIdToConfiguration(start_id)
# goal_grid=self.discrete_env.NodeIdToConfiguration(goal_id)
cost = abs(start_grid[0] - goal_grid[0]) + abs(start_grid[1] -
goal_grid[1])
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
return cost
def InitializePlot(self, goal_config):
self.fig = pl.figure()
pl.xlim([self.lower_limits[0], self.upper_limits[0]])
pl.ylim([self.lower_limits[1], self.upper_limits[1]])
pl.plot(goal_config[0], goal_config[1], 'gx')
# Show all obstacles in environment
for b in self.robot.GetEnv().GetBodies():
if b.GetName() == self.robot.GetName():
continue
bb = b.ComputeAABB()
pl.plot([
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] + bb.extents()[0],
bb.pos()[0] - bb.extents()[0],
bb.pos()[0] - bb.extents()[0]
], [
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] - bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] + bb.extents()[1],
bb.pos()[1] - bb.extents()[1]
], 'r')
pl.ion()
pl.show()
def PlotEdge(self, sconfig, econfig):
pl.plot([sconfig[0], econfig[0]], [sconfig[1], econfig[1]],
'k.-',
linewidth=2.5)
pl.draw()
def IsValid(self, coord):
upper_bound_coord = self.discrete_env.ConfigurationToGridCoord([5, 5])
lower_bound_coord = self.discrete_env.ConfigurationToGridCoord(
[-5, -5])
if coord[0] > upper_bound_coord[0]:
return False
if coord[0] < lower_bound_coord[0]:
return False
if coord[1] > upper_bound_coord[1]:
return False
if coord[1] < lower_bound_coord[1]:
return False
return True
class SimpleEnvironment(object):
def __init__(self, herb, table, resolution):
self.herb = herb
self.robot = herb.robot
self.boundary_limits = [[-5., -5., -numpy.pi], [5., 5., numpy.pi]]
lower_limits, upper_limits = self.boundary_limits
self.discrete_env = DiscreteEnvironment(resolution, lower_limits, upper_limits)
self.table = table
self.resolution = resolution
self.ConstructActions()
def GenerateFootprintFromControl(self, start_config, control, stepsize=0.01):
# Extract the elements of the control
ul = control.ul
ur = control.ur
dt = control.dt
# Initialize the footprint
config = start_config.copy()
footprint = [numpy.array([0., 0., config[2]])]
timecount = 0.0
while timecount < dt:
# Generate the velocities based on the forward model
xdot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.cos(config[2])
ydot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.sin(config[2])
tdot = self.herb.wheel_radius * (ul - ur) / self.herb.wheel_distance
# Feed forward the velocities
if timecount + stepsize > dt:
stepsize = dt - timecount
config = config + stepsize*numpy.array([xdot, ydot, tdot])
if config[2] > numpy.pi:
# print (str(start_config[2])+" Went over... old = "+str(config[2]))
config[2] -= 2.*numpy.pi
# print ("Went over... new = "+str(config[2]))
if config[2] < -numpy.pi:
# print (str(start_config[2])+" Went under... cur = "+str(config[2]))
config[2] += 2.*numpy.pi
# print ("Went under... new = "+str(config[2]))
footprint_config = config.copy()
footprint_config[:2] -= start_config[:2]
footprint.append(footprint_config)
timecount += stepsize
# Add one more config that snaps the last point in the footprint to the center of the cell
nid = self.discrete_env.ConfigurationToNodeId(config)
snapped_config = self.discrete_env.NodeIdToConfiguration(nid)
snapped_config[:2] -= start_config[:2]
footprint.append(snapped_config)
start_id = self.discrete_env.ConfigurationToNodeId(start_config)
if (nid == start_id):
return None
return footprint
def PlotActionFootprints(self, idx):
actions = self.actions[idx]
fig = pl.figure()
lower_limits, upper_limits = self.boundary_limits
pl.xlim([lower_limits[0], upper_limits[0]])
pl.ylim([lower_limits[1], upper_limits[1]])
for action in actions:
xpoints = [config[0] for config in action.footprint]
ypoints = [config[1] for config in action.footprint]
pl.plot(xpoints, ypoints, 'k')
pl.ion()
pl.show()
def ConstructActions(self):
# Actions is a dictionary that maps orientation of the robot to
# an action set
self.actions = dict()
wc = [0., 0., 0.]
grid_coordinate = self.discrete_env.ConfigurationToGridCoord(wc)
# Iterate through each possible starting orientation
print("ConstructActions")
for idx in range(int(self.discrete_env.num_cells[2])):
self.actions[idx] = []
grid_coordinate[2] = idx
start_config = numpy.array(self.discrete_env.GridCoordToConfiguration(grid_coordinate))
alreadyAdded = dict()
actionSet = list()
for ul in numpy.arange(-1, 1, 0.2):
for ur in numpy.arange(-1, 1, 0.2):
for dt in numpy.arange(0.2, 1, 0.2):
control = Control(ul, ur, dt)
footprint = self.GenerateFootprintFromControl(start_config, control, stepsize = 0.05)
# newID = self.discrete_env.ConfigurationToNodeId(footprint[len(footprint)-1])
# if (addedStuff)
if footprint != None:
nid = self.discrete_env.ConfigurationToNodeId(footprint[len(footprint)-1])
if (alreadyAdded.get(nid) == None):
# print(footprint[len(footprint)-3:])
actionSet.append(Action(control, footprint))
alreadyAdded[nid] = True
self.actions[idx] = actionSet
print("number of actions for config: "+str(start_config)+" = "+str(len(actionSet)))
# TODO: Here you will construct a set of actions
# to be used during the planning process
# self.PlotActionFootprints(idx)
return
def GetSuccessors(self, node_id):
successors = list()
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids and controls that represent the neighboring
# nodes
grid_coord = self.discrete_env.NodeIdToGridCoord(node_id)
startConfig = self.discrete_env.GridCoordToConfiguration(grid_coord)
theta_cord = grid_coord[2]
validTrajectory = True
with self.herb.env:
for x in self.actions[theta_cord]:
# x = the action in the self.actions variable
for idx in range(len(x.footprint)):
currentPosition = startConfig + x.footprint[idx]
if (self.Collides(currentPosition)):
validTrajectory = False
# print("collision...");
break
if validTrajectory == True:
successors.append(x)
return successors
def Collides(self, config):
x = config[0]
y = config[1]
theta = config[2]
transform = matrixFromAxisAngle([0,0,theta])
transform[0][3] = x
transform[1][3] = y
# Assigns Transform to Robot and Checks Collision
with self.herb.env:
self.robot.SetTransform(transform)
# time.sleep(0.01)
if self.herb.env.CheckCollision(self.robot,self.table) == True:
return True
else:
return False
def ComputeDistance(self, start_id, end_id):
# This is a function that
# computes the distance between the configurations given
# by the two node ids
start = self.discrete_env.NodeIdToConfiguration(start_id)
end = self.discrete_env.NodeIdToConfiguration(end_id)
# print("Current id is: "+str(start_id)+" Goal id is: "+str(end_id))
# print("Current is: "+str(start)+" Goal is: "+str(end))
# Manhattan distance
# dist = math.sqrt((end[0] - start[0]) * (end[0] - start[0]) + (end[1] - start[1]) * (end[1] - start[1]) + (end[2] - start[2]) * (end[2] - start[2]))
dist = math.sqrt((end[0] - start[0])*(end[0] - start[0]) * self.resolution[0] \
+ (end[1] - start[1]) * (end[1] - start[1]) * self.resolution[1] \
+ (end[2] - start[2]) * (end[2] - start[2]) * self.resolution[2])
# dist = abs(end[0] - start[0]) + abs(end[1] - start[1]) + abs(end[2] - start[2])
return dist
def ComputeHeuristicCost(self, start_id, end_id):
# start = self.discrete_env.NodeIdToConfiguration(start_id)
# end = self.discrete_env.NodeIdToConfiguration(goal_id)
# dist = 0
# dist = dist + (end[0] - start[0])
# dist = dist + (end[1] - start[1])
start = self.discrete_env.NodeIdToConfiguration(start_id)
end = self.discrete_env.NodeIdToConfiguration(end_id)
dist = math.sqrt((end[0] - start[0])*(end[0] - start[0]) * self.resolution[0] \
+ (end[1] - start[1]) * (end[1] - start[1]) * self.resolution[1] \
+ (end[2] - start[2]) * (end[2] - start[2]) * self.resolution[2] * 0.2)
return dist
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.herb = herb
self.robot = herb.robot
self.boundary_limits = [[-5., -5., -np.pi], [5., 5., np.pi]]
lower_limits, upper_limits = self.boundary_limits
self.discrete_env = DiscreteEnvironment(resolution, lower_limits, upper_limits)
self.resolution = resolution
self.ConstructActions()
def GenerateFootprintFromControl(self, start_config, control, stepsize=0.01):
# Extract the elements of the control
ul = control.ul
ur = control.ur
dt = control.dt
# Initialize the footprint
config = start_config.copy()
footprint = [np.array([0., 0., config[2]])]
timecount = 0.0
while timecount < dt:
# Generate the velocities based on the forward model
xdot = 0.5 * self.herb.wheel_radius * (ul + ur) * np.cos(config[2])
ydot = 0.5 * self.herb.wheel_radius * (ul + ur) * np.sin(config[2])
tdot = self.herb.wheel_radius * (ul - ur) / self.herb.wheel_distance
#Feed forward the velocities
# print(xdot, ydot)
if timecount + stepsize > dt:
stepsize = dt - timecount
config = config + stepsize*np.array([xdot, ydot, tdot]).T
# print(config)
if config[2] > np.pi:
config[2] -= 2.*np.pi
if config[2] < -np.pi:
config[2] += 2.*np.pi
footprint_config = config.copy()
footprint_config[:2] -= start_config[:2]
footprint.append(footprint_config)
timecount += stepsize
# Add one more config that snaps the last point in the footprint to the center of the cell
nid = self.discrete_env.ConfigurationToNodeId(config)
snapped_config = self.discrete_env.NodeIdToConfiguration(nid)
snapped_config[:2] -= start_config[:2]
footprint.append(snapped_config)
return footprint
def PlotActionFootprints(self, idx):
actions = self.actions[idx]
fig = pl.figure()
lower_limits, upper_limits = self.boundary_limits
pl.xlim([lower_limits[0], upper_limits[0]])
pl.ylim([lower_limits[1], upper_limits[1]])
for action in actions:
print(idx*self.resolution[2]+lower_limits[2])
xpoints = [config[0] for config in action.footprint]
ypoints = [config[1] for config in action.footprint]
print(action.control.ul, action.control.ur)
print(xpoints, ypoints)
pl.plot(xpoints, ypoints, 'k')
pl.ion()
pl.show()
def ConstructActions(self):
# Actions is a dictionary that maps orientation of the robot to
# an action set
self.actions = dict()
wc = [0., 0., 0.]
grid_coordinate = self.discrete_env.ConfigurationToGridCoord(wc)
# self.discrete_env.num_cells shape = [40,40,8]
# Iterate through each possible starting orientation
for idx in range(int(self.discrete_env.num_cells[2])):
self.actions[idx] = []
grid_coordinate[2] = idx
start_config = self.discrete_env.GridCoordToConfiguration(grid_coordinate)
print(start_config)
# TODO: Here you will construct a set of actions
# to be used during the planning process
w_left = np.array([-1,-1,1,1])
w_right = np.array([-1,1,-1,1])
duration = 1
for i in range(w_left.size):
control = Control(w_left[i],w_right[i],duration)
footprint = self.GenerateFootprintFromControl(start_config,control)
self.actions[idx].append(Action(control,footprint))
def GetSuccessors(self, node_id):
successors = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids and controls that represent the neighboring
# nodes
lower_limits, upper_limits = self.boundary_limits
cur_config = self.discrete_env.NodeIdToConfiguration(node_id)
orig_coord = self.discrete_env.NodeIdToGridCoord(node_id)
# print("shape")
# print(len(self.actions))
# there are 8 orientations around the robot, choose the current orientation
cur_orient_actions = self.actions[orig_coord[2]]
# Given the orientation, find the corresponding control and footprints, add the footprint to the orignal config to get the new config
for action in cur_orient_actions:
new_config = np.copy(cur_config)
valid_action = True
for footprint in action.footprint:
new_config[0] = cur_config[0] + footprint[0]
new_config[1] = cur_config[1] + footprint[1]
new_config[2] = footprint[2]
# check weather the new_config is within the boundary
if new_config[0] >= upper_limits[0] or new_config[0] <= lower_limits[0]:
valid_action = False
break
if new_config[1] >= upper_limits[1] or new_config[1] <= lower_limits[1]:
valid_action = False
break
if self.CheckCollision(new_config):
valid_action = False
break
if valid_action:
successors.append((self.discrete_env.ConfigurationToNodeId(new_config),action))
return successors
def ComputeDistance(self, start_id, end_id):
dist = 0
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(end_id)
dist = np.linalg.norm(start_config-end_config)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
heurist_cost = self.ComputeDistance(start_id, goal_id)
return heurist_cost
def CheckCollision(self, config):
collide = False
with self.robot:
robot_tf = np.identity(4)
robot_tf[0, 3] = config[0]
robot_tf[1, 3] = config[1]
self.robot.SetTransform(robot_tf)
collide = self.robot.GetEnv().CheckCollision(self.robot)
return collide
class SimpleEnvironment(object):
def __init__(self, herb, resolution):
self.herb = herb
self.robot = herb.robot
self.boundary_limits = [[-5., -5., -numpy.pi], [5., 5., numpy.pi]]
self.lower_limits, self.upper_limits = self.boundary_limits
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits, self.upper_limits)
self.resolution = resolution
self.ConstructActions()
self.saved_transform = None
# self.PlotActionFootprints(0)
def GenerateFootprintFromControl(self, start_config, control, stepsize=0.01):
# Extract the elements of the control
ul = control.ul
ur = control.ur
dt = control.dt
# Initialize the footprint
config = start_config.copy()
footprint = [numpy.array([0., 0., config[2]])]
timecount = 0.0
while timecount < dt:
# Generate the velocities based on the forward model
xdot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.cos(config[2])
ydot = 0.5 * self.herb.wheel_radius * (ul + ur) * numpy.sin(config[2])
tdot = self.herb.wheel_radius * (ul - ur) / self.herb.wheel_distance
# Feed forward the velocities
if timecount + stepsize > dt:
stepsize = dt - timecount
config = config + stepsize*numpy.array([xdot, ydot, tdot])
if config[2] > numpy.pi:
config[2] -= 2.*numpy.pi
if config[2] < -numpy.pi:
config[2] += 2.*numpy.pi
footprint_config = config.copy()
footprint_config[:2] -= start_config[:2]
footprint.append(footprint_config)
timecount += stepsize
# Add one more config that snaps the last point in the footprint to the center of the cell
nid = self.discrete_env.ConfigurationToNodeId(config)
snapped_config = self.discrete_env.NodeIdToConfiguration(nid)
snapped_config[:2] -= start_config[:2]
footprint.append(snapped_config)
return footprint
def PlotActionFootprints(self, idx):
actions = self.actions[idx]
fig = pl.figure()
lower_limits, upper_limits = self.boundary_limits
pl.xlim([lower_limits[0], upper_limits[0]])
pl.ylim([lower_limits[1], upper_limits[1]])
for action in actions:
xpoints = [config[0] for config in action.footprint]
ypoints = [config[1] for config in action.footprint]
pl.plot(xpoints, ypoints, 'k')
pl.ion()
pl.show()
def SaveTransform(self):
self.saved_transform = self.robot.GetTransform()
def LoadTransform(self):
with self.robot.GetEnv():
self.robot.SetTransform(self.saved_transform)
def ConstructActions(self):
# Actions is a dictionary that maps orientation of the robot to
# an action set
self.actions = dict()
wc = [0., 0., 0.]
grid_coordinate = self.discrete_env.ConfigurationToGridCoord(wc)
# print 'construct actions'
# Iterate through each possible starting orientation
for idx in range(int(self.discrete_env.num_cells[2])):
self.actions[idx] = []
grid_coordinate[2] = idx
start_config = self.discrete_env.GridCoordToConfiguration(grid_coordinate)
# TODO: Here you will construct a set of actions
# to be used during the planning process
#
omega_rs = numpy.linspace(-1, 1, 5)
omega_ls = numpy.linspace(-1, 1, 5)
duration_list = numpy.linspace(0,3,8)
for o_l in omega_ls:
for o_r in omega_rs:
for d in duration_list:
control = Control(o_l, o_r, d)
footprint = self.GenerateFootprintFromControl(start_config, control)
action = Action(control, footprint)
self.actions[idx].append(action)
# print idx
# omega_r = 1 if start_config[2] < numpy.pi/2 else -1 # TODO set based on the value? Kp?
# omega_l = -omega_r
# tdot = self.herb.wheel_radius * (omega_l - omega_r) / self.herb.wheel_distance
def CheckCollision(self, config):
H = numpy.identity(4)
angle = config[2]
H[0:2,0:2] = numpy.array([[numpy.cos(angle), -numpy.sin(angle)], [numpy.sin(angle), numpy.cos(angle)]])
H[0:2, 3] = [config[0], config[1]]
with self.robot.GetEnv():
self.robot.SetTransform(H)
ret = self.robot.GetEnv().CheckCollision(self.robot)
return ret
def GetSuccessors(self, node_id):
successors = []
# TODO: Here you will implement a function that looks
# up the configuration associated with the particular node_id
# and return a list of node_ids and controls that represent the neighboring
# nodes
gridcoord = self.discrete_env.NodeIdToGridCoord(node_id)
config = self.discrete_env.GridCoordToConfiguration(gridcoord)
lower_limits, upper_limits = self.boundary_limits
actions = self.actions[gridcoord[2]]
for action in actions:
fp = action.footprint[-1]
new_config = numpy.add(config, fp)
new_config[2] = fp[2]
if (lower_limits <= new_config).all() and (new_config<= upper_limits).all() and not self.CheckCollision(new_config):
Nid = self.discrete_env.ConfigurationToNodeId(new_config)
successors.append((Nid, action))
return successors
def ComputeDistance(self, start_id, end_id):
dist = 0
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(end_id)
# TODO: Here you will implement a function that
# computes the distance between the configurations given
# by the two node ids
dist = numpy.linalg.norm(start_config-end_config)
return dist
def ComputeHeuristicCost(self, start_id, goal_id):
# cost = 0
# TODO: Here you will implement a function that
# computes the heuristic cost between the configurations
# given by the two node ids
start_config = self.discrete_env.NodeIdToConfiguration(start_id)
end_config = self.discrete_env.NodeIdToConfiguration(goal_id)
xy_dist = numpy.linalg.norm(start_config[0:2]- end_config[0:2])
return xy_dist;