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.env = self.robot.GetEnv()
# 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.5
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
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 __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
def __init__(self, herb, resolution):
self.herb = herb
self.robot = herb.robot
self.env = self.robot.GetEnv()
self.table = self.env.GetBodies()[1] # table
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 __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 __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 __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 = np.array([[0, 0, -1, 1.5], [-1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 0, 1]])
self.table.SetTransform(table_pose)
offset = np.zeros(len(self.discrete_env.num_cells))
offset[0] = 1
self.offsets = set(itertools.permutations(offset))
self.draw_counter = 0
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
self.space_measure = 1 # for real numbers
self.unit_ball_measure = numpy.pi # for 2D space with a radius of 1
self.C = numpy.array([[1, 0], [0, 1]])
self.dimension = len(self.lower_limits)
def __init__(self, resolution, dimension):
graphFile = 'graphs/' + ` dimension ` + 'D.graphml'
self.graph = nx.read_graphml(graphFile)
# You will use this graph to query the discrete world it embeds in the space
self.lower_limits = numpy.zeros(dimension)
self.upper_limits = numpy.ones(dimension)
self.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
obstacleFile = 'obstacles/' + ` dimension ` + 'D.txt'
obstacles = []
with open(obstacleFile) as file:
for line in file:
new_obstacle = [float(k) for k in line.split()]
obstacles.append(new_obstacle)
self.obstacles = numpy.array(obstacles)
self.space_dim = dimension
self.step_size = 0.01
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 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.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)
self.env = self.robot.GetEnv()
# 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.5
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
return successors
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
def ComputeDistance(self, start_config, end_config):
#
# TODO: Implement a function which computes the distance between
# two configurations
#
S0 = start_config[0]
S1 = start_config[1]
S2 = start_config[2]
S3 = start_config[3]
S4 = start_config[4]
S5 = start_config[5]
S6 = start_config[6]
E0 = end_config[0]
E1 = end_config[1]
E2 = end_config[2]
E3 = end_config[3]
E4 = end_config[4]
E5 = end_config[5]
E6 = end_config[6]
dist = numpy.sqrt((S0-E0)**2 + (S1-E1)**2 + (S2-E2)**2 + (S3-E3)**2 + (S4-E4)**2 + (S5-E5)**2 + (S6-E6)**2)
return dist
def Collides(self, point):
# Show all obstacles in environment
#if(env.CheckCollision(robot1,robot2)
Deg0 = point[0]
Deg1 = point[1]
Deg2 = point[2]
Deg3 = point[3]
Deg4 = point[4]
Deg5 = point[5]
Deg6 = point[6]
activeDOFs = self.robot.GetActiveDOFValues()
activeDOFs[0] = Deg0
activeDOFs[1] = Deg1
activeDOFs[2] = Deg2
activeDOFs[3] = Deg3
activeDOFs[4] = Deg4
activeDOFs[5] = Deg5
activeDOFs[6] = Deg6
self.robot.SetActiveDOFValues(activeDOFs)
if self.env.CheckCollision(self.robot,self.table) == True or self.robot.CheckSelfCollision() == True:
return True
else:
return False
def ComputePathLength(self, plan):
dist = 0
for i in range(0,len(plan)-1):
x = self.ComputeDistance(plan[i],plan[i+1])
dist = dist + x
return dist
def GenerateRandomConfiguration(self):
config = [0] * len(self.robot.GetActiveDOFIndices())
lower_limits, upper_limits = self.robot.GetActiveDOFLimits()
for i in range(len(config)):
config[i] = lower_limits[i]+(upper_limits[i]-lower_limits[i])*numpy.random.random()
return numpy.array(config)
def Extend(self, start_config, end_config):
numsteps = 100.0
dimensions = 7
steps = numpy.zeros((dimensions, int(numsteps)))
for i in range(dimensions):
diff = end_config[i] - start_config[i]
this_dim_steps = range(1, int(numsteps)+1)
steps[i] = [x * (diff / numsteps) + start_config[i] for x in this_dim_steps]
joints = self.robot.GetActiveDOFIndices()
original_values = self.robot.GetDOFValues()
# steps[i][j], refers to the i-th dimension and the j-th step
env = self.robot.GetEnv()
for j in range(int(numsteps)):
values = steps[:,j]
self.robot.SetActiveDOFValues(values)
with self.robot.GetEnv():
self.robot.SetActiveDOFValues(values)
#print values
for b in self.robot.GetEnv().GetBodies():
q = self.robot.GetEnv().CheckCollision(b, self.robot)
#print q
if q:
#print 'collision!'
self.robot.SetActiveDOFValues(original_values)
with self.robot.GetEnv():
self.robot.SetActiveDOFValues(original_values)
return None
self.robot.SetActiveDOFValues(values)
with self.robot.GetEnv():
self.robot.SetActiveDOFValues(values)
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).
#
now = 0
while now < timeout:
now = copy.deepcopy(time.clock() - now)
g = copy.deepcopy(path[-1])
i = 0
while path[i+1].all() != g.all():
if len(path) > 3:
if self.Extend(path[i],path[i+2]) != None:
del path[i+1]
i = i + 1
else:
break
if len(path) - i < 2:
break
now = copy.deepcopy(time.clock() - now)
return path
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 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.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 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.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 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.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 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.discrete_env = DiscreteEnvironment(resolution, self.lower_limits,
self.upper_limits)
self.step_size = 0.05
# 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.gridCoord_to_configuration(
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 get_successors(self, node_id):
# TODO: Here you will implement a function that returns all the
# neighbouring configurations' node IDs of the configuration
# represented by the input `node_id`
successors = []
return successors
def state_validity_checker(self, node_id):
# TODO: Here you will implement a function which
# checks the collision status of a state represented
# by the the node ID given
status = 0
return status
def edge_validity_checker(self, start_id, goal_id):
# TODO: Here you will implement a function which
# check the collision status of an edge between the
# states represented by the node IDs given using
# using `step_size` as the resolution factor.
status = 0
return status
def compute_distance(self, start_id, end_id):
# TODO: Here you will implement a function which computes the
# distance between two states given their node IDs.
dist = 0
return dist
def get_heuristic(self, start_id, goal_id):
# TODO: Here you will implement a function to return the
# heuristic cost of a state given its node ID and goal ID
heuristic = 0
return heuristic
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 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 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
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)
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 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)
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
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 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)