def checkCollision(world, robot, q):
robot.setConfig(q)
collisionChecker = collide.WorldCollider(world)
coll = collisionChecker.robotObjectCollisions(world.robot(0))
for i, j in coll:
return True
return False
def __init__(self,world):
vis.GLPluginInterface.__init__(self)
self.world = world
self.collider = collide.WorldCollider(world)
self.quit = False
#adds an action to the window's menu
def doquit():
self.quit = True
self.add_action(doquit,"Quit",'Ctrl+q',"Quit the program")
def Planning(self):
self.nodeList = [self.start]
robo = self.robot
collisionChecker = collide.WorldCollider(self.world)
while True:
# Random Sampling
if random.randint(0, 100) > self.goalSampleRate:
rnd = [
random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand)
]
else:
rnd = [self.end.x, self.end.y]
# Find nearest node
nind = self.GetNearestListIndex(self.nodeList, rnd)
# print(nind)
# expand tree
nearestNode = self.nodeList[nind]
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
newNode = copy.deepcopy(nearestNode)
newNode = VDP2(newNode.x, newNode.y, theta, self.expandDis)
newNode.parent = nind
robo.setConfig(newNode)
collRT2 = collisionChecker.robotObjectCollisions(self.world.robo)
for i, j in collRT2:
collisionFlag = True
continue
self.nodeList.append(newNode)
# check goal
dx = newNode.x - self.end.x
dy = newNode.y - self.end.y
d = math.sqrt(dx * dx + dy * dy)
if d <= self.expandDis:
print("Goal!!")
break
#if animation:
#self.DrawGraph(rnd)
path = [[self.end.x, self.end.y]]
lastIndex = len(self.nodeList) - 1
while self.nodeList[lastIndex].parent is not None:
node = self.nodeList[lastIndex]
path.append([node.x, node.y])
lastIndex = node.parent
path.append([self.start.x, self.start.y])
return path
def registerCollisionFunction(world, name='collisionFree'):
collider = collide.WorldCollider(world)
robot = world.robot(0)
def collides(x):
robot.setConfig(x)
for c in collider.collisions():
return False
return True
registerFunctionFactory(name, lambda args: collides)
def gen_rrt(world, robot, q_init, num_samples):
# Used to check if a configuration results in collision in the world.
coll_check = collide.WorldCollider(world)
# First, build tree with initial configuration.
rrt = RRT(q_init)
# Now, for k iterations, try to expand the tree.
for k in range(0, num_samples):
q_rand = gen_rand_config(world)
rrt.extend_tree(q_rand, world, robot, coll_check)
return rrt
def __init__(self,
visWorld,
planningWorld,
name="IK Database visual tester"):
GLWidgetProgram.__init__(self, visWorld, name)
self.planningWorld = planningWorld
self.collider = collide.WorldCollider(visWorld)
self.ikdb = ManagedIKDatabase(planningWorld.robot(0))
self.ikWidgets = []
self.ikIndices = []
self.ikProblem = IKProblem()
self.ikProblem.setFeasibilityTest('collisionFree', None)
qmin, qmax = planningWorld.robot(0).getJointLimits()
self.ikProblem.setCostFunction('jointRangeCost_dynamic', [qmin, qmax])
self.drawDb = False
self.continuous = False
self.reSolve = False
self.currentConfig = self.world.robot(0).getConfig()
def WillCollideDuringClosure(hand, obj):
world = hand.world
robot = hand.robot
q_old = robot.getConfig()
sigma_old = hand.getCommand()
collision = collide.WorldCollider(world) #init
list_r_w_coll = [] # same as above. The output is generator so make a list to know how many collision we have been
for i in range(6):
hand.setCommand([i/5.0])
tau, q = hand.output(kinematic = True)
robot.setConfig(q)
r_w_coll = collision.robotTerrainCollisions(robot) # check collision robot-plane
for coll in r_w_coll:
list_r_w_coll.append(coll)
hand.setCommand(sigma_old)
robot.setConfig(q_old)
if len(list_r_w_coll) >0: #if the length is greater that zero, we have collision
return True
else:
return False
def CollisionTestInterpolate(world, robot, obj, w_P_h_goal, w_P_h_start = None):
q_old = robot.getConfig()
if not isinstance(w_P_h_goal, tuple):
w_T_h_goal = se3.from_homogeneous(w_P_h_goal) #o_P_h is end-effector in object frame
else:
w_T_h_goal = w_P_h_goal
if w_P_h_start is None:
w_T_h_start = get_moving_base_xform(robot)
else:
if not isinstance(w_P_h_start, tuple):
w_T_h_start = se3.from_homogeneous(w_P_h_start) # o_P_h is end-effector in object frame
else:
w_T_h_start = w_P_h_start
t_des = w_T_h_goal[1]
t_curr = w_T_h_start[1]
set_moving_base_xform(robot, w_T_h_goal[0], w_T_h_start[1])
step_size = 0.01
d = vectorops.distance(t_curr, t_des)
n_steps = int(math.ceil(d / step_size))
if n_steps == 0: # if o_T_h_current == o_T_h_desired
return CheckCollision(world, robot, obj)
collider = collide.WorldCollider(world)
for i in range(n_steps):
t_int = vectorops.interpolate(t_curr,t_des,float(i+1)/n_steps)
set_moving_base_xform(robot, w_T_h_goal[0], t_int)
if CheckCollision(world, robot, obj, collider):
robot.setConfig(q_old)
return True
robot.setConfig(q_old)
return False
def CheckCollision(world, robot, obj, collision = None):
"""
:param world: world object
:param robot: robot object
:param obj: object to check collision against
:collision: an optional WorldCollider
:return: True if in collision
"""
if collision is None:
collision = collide.WorldCollider(world) #init
r_o_coll = collision.robotObjectCollisions(robot,obj) #check collision robot-object. the output is generator
list_r_o_coll = [] # make a list to know how many collisions we have been
for coll in r_o_coll:
list_r_o_coll.append(coll)
r_w_coll = collision.robotTerrainCollisions(robot) # check collision robot-plane
list_r_w_coll = [] # same as above. The output is generator so make a list to know how many collision we have been
for coll in r_w_coll:
list_r_w_coll.append(coll)
if len(list_r_o_coll) > 0 or len(list_r_w_coll) >0: #if the length is greater that zero, we have collision
return True
else:
return False
robot.setAltitude(0.12)
#robot = sphero6DoF(world.robot(0), "sphero", vis)
## Display the world coordinate system
vis.add("WCS", [so3.identity(), [0, 0, 0]])
vis.setAttribute("WCS", "size", 24)
#print "Visualization items:"
#vis.listItems(indent=2)
#vis.autoFitCamera()
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
collisionFlag = False
collisionChecker = collide.WorldCollider(world)
## On-screen text display
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
vis.addText("textbottom", "WCS: X-axis Red, Y-axis Green, Z-axis Blue",
(20, -30))
print "Starting visualization window#..."
## Run the visualizer, which runs in a separate thread
vis.setWindowTitle("Visualization for kinematic simulation")
print("Starting.....")
source = [0, 0, 0, 0, 0, 0]
goal = [3, -3.5, 0, 0, 0, 0]
def create(self, start_pc, goal_pc):
"""
This method cretes the simulation
:param start_pc: robot's initial position coordinate
:param goal_pc: goal position coordinate
:return:
"""
print "Creating the Simulator"
object_dir = "/home/jeet/PycharmProjects/DeepQMotionPlanning/"
self.start_pc = start_pc
self.goal_pc = goal_pc
coordinates.setWorldModel(self.world)
getDoubleRoomDoor(self.world, 8, 8, 1)
builder = Builder(object_dir)
# Create a goal cube
n_objects = 1
width = 0.1
depth = 0.1
height = 0.1
x = goal_pc[0]
y = goal_pc[1]
z = goal_pc[2] / 2
thickness = 0.005
color = (0.2, 0.6, 0.3, 1.0)
builder.make_objects(self.world, n_objects, "goal", width, depth,
height, thickness, self.terrain_limit, color,
self.goal_pc)
# Create a obstacle cube
n_objects = 4
width = 0.2
depth = 0.2
height = 0.2
x = 2.3
y = 0.8
z = goal_pc[2] / 2
thickness = 0.001
color = (0.8, 0.2, 0.2, 1.0)
builder.make_objects(self.world, n_objects, "rigid", width, depth,
height, thickness, self.terrain_limit, color)
self.vis = vis
vis.add("world", self.world)
# Create the view port
vp = vis.getViewport()
vp.w, vp.h = 1200, 900
vp.x, vp.y = 0, 0
vis.setViewport(vp)
# Create the robot
self.robot = sphero6DoF(self.world.robot(0), "", None)
self.robot.setConfig(start_pc)
# Create the axis representation
# vis.add("WCS", [so3.identity(), [0, 0, 0]])
# vis.setAttribute("WCS", "size", 24)
# Add text messages component for collision check and robot position
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
vis.addText("textStep", "Steps: ")
vis.setAttribute("textStep", "size", 24)
vis.addText("textGoalDistance", "Goal Distance: ")
vis.setAttribute("textGoalDistance", "size", 24)
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
self.collision_checker = collide.WorldCollider(self.world)
vis.setWindowTitle("Simulator")
vis.show()
return vis, self.robot, self.world
def run_poking(config):
"""
this is poking api entrance.
"""
# init params
tableHeight = config.tableHeight
probeLength = config.probeLength
forceLimit = config.forceLimit
dt=config.dt #250Hz
moveStep=0.002*dt #2mm /s
shortServoTime=config.shortServoTime
longServoTime=config.longServoTime
IKErrorTolerence=config.IKErrorTolerence
maxDev=config.maxDev
EEZLimit=config.EEZLimit
intermediateConfig = config.intermediateConfig
probe_transform = config.probe_transform
point_probe = np.array([[0,0,0,1],
[1-probeLength,0,0,1],
[0,-1,0,1]]) # means the point in probe coordinate.
point_probe_to_local = np.dot(probe_transform, point_probe.T)
point_probe_to_local = point_probe_to_local[0:3,:].T
point_probe_to_local = point_probe_to_local.tolist()
print("[*]Debug: probe coodinate transform to EE:")
print(point_probe_to_local)
# init robot
world = WorldModel()
res = world.readFile(config.robot_model_path)
robot = world.robot(0)
ee_link=config.ee_link_number #UR5 model is 7.
link=robot.link(ee_link)
CONTROLLER = config.mode
collider = collide.WorldCollider(world)
print '---------------------model loaded -----------------------------'
# visualization
vis.add("world",world)
# create folder
data_folder = config.exp_path+'exp_'+str(config.exp_number)+'/'+config.probe_type
if not os.path.exists(data_folder):
os.mkdir(data_folder)
# begin loop
if config.probe_type == 'point':
run_poking_point_probe(config,tableHeight,probeLength,forceLimit,dt,moveStep,shortServoTime,longServoTime,
IKErrorTolerence,maxDev,EEZLimit,probe_transform,point_probe_to_local,world,res,robot,link,CONTROLLER,collider,intermediateConfig)
elif config.probe_type == 'line':
run_poking_line_probe(config,tableHeight,probeLength,forceLimit,dt,moveStep,shortServoTime,longServoTime,
IKErrorTolerence,maxDev,EEZLimit,probe_transform,point_probe_to_local,world,res,robot,link,CONTROLLER,collider,intermediateConfig)
elif config.probe_type == 'ellipse':
run_poking_ellipse_probe(config,tableHeight,probeLength,forceLimit,dt,moveStep,shortServoTime,longServoTime,
IKErrorTolerence,maxDev,EEZLimit,probe_transform,point_probe_to_local,world,res,robot,link,CONTROLLER,collider,intermediateConfig)
else:
print('[!]Probe type no exist')
def find_path(self, q_goal, world, robot):
# Extract collision detector for attempts at linking goal to RRT.
coll_check = collide.WorldCollider(world)
# First, try to connect q_goal to a nearby configuration.
# node_goal = self.adj_list[self.get_nearest_neighbor(q_goal)][0]
# The idea is to look in 8 directions around the q_goal to find nearest neighbors. This is in case an abundance
# of neighbors are not reachable from one side.
num_segs = 8
nearest_neighbors = self.get_nearest_neighbors_by_angle(q_goal, num_segs)
chosen_neighbor = []
# TODO: Try each of the neighbors, in order of closeness...
for neighbor_id in nearest_neighbors:
neighbor = self.adj_list[neighbor_id][0]
# First, rotate so the robot is facing towards q. Test for collision periodically.
# Angular interpolation
start_t = neighbor.t
# The direction pointing from q_near to q
goal_dir_t = math.atan2(q_goal[1] - neighbor.y, q_goal[0] - neighbor.x)
if goal_dir_t < 0:
goal_dir_t = goal_dir_t + 2 * math.pi
elif goal_dir_t >= 2 * math.pi:
goal_dir_t = goal_dir_t - 2 * math.pi
is_success = False
t_step = 10
# angle_diff = goal_dir_t - start_t
# angle_diff = abs((angle_diff + 180) % 360) - 180
angle_diff = math.atan2(math.sin(goal_dir_t - start_t), math.cos(goal_dir_t - start_t))
curr_t = start_t
rads_rotated = 0
has_collided = False
while rads_rotated < abs(angle_diff) and not has_collided:
rads_rotated = rads_rotated + t_step
curr_t = curr_t + np.sign(angle_diff) * t_step
if curr_t < 0:
curr_t = curr_t + 2 * math.pi
elif curr_t >= 2 * math.pi:
curr_t = curr_t - 2 * math.pi
# This is the last check we need, but don't overshoot it.
if rads_rotated > abs(angle_diff):
rads_rotated = abs(angle_diff)
curr_t = goal_dir_t
# Set current configuration in world.
robot.setConfig([neighbor.x, neighbor.y, curr_t])
# Check collision for current configuration.
obj_colls = coll_check.robotObjectCollisions(world.robot(0))
for i, j in obj_colls:
# There is collision with an obstacle! We can't add this to the tree.
# TODO: Possibly try the longer rotation if this one fails.
has_collided = True
return "TRAPPED"
# Translate in a straight line incrementally towards x,y of q. Test for collision periodically.
# 2D linear interpolation
# Compute (x,y) after traveling dist towards q(x,y)
x_diff = q_goal[0] - neighbor.x
y_diff = q_goal[1] - neighbor.y
mag = math.sqrt(x_diff ** 2 + y_diff ** 2)
norm = [x_diff / mag, y_diff / mag]
step_mag = .02
curr_mag = step_mag
curr_vec = [norm[0] * curr_mag, norm[1] * curr_mag]
# This is key difference. The desired mag here is the entire distance from neighbor to q_goal.
desired_mag = mag
has_collided = False
while curr_mag < desired_mag and not has_collided:
# Compute new (x,y)
curr_mag = curr_mag + step_mag
if curr_mag > desired_mag:
curr_mag = desired_mag
curr_vec = [norm[0] * curr_mag, norm[1] * curr_mag]
curr_x = curr_vec[0] + neighbor.x
curr_y = curr_vec[1] + neighbor.y
robot.setConfig([curr_x, curr_y, curr_t])
obj_colls = coll_check.robotObjectCollisions(world.robot(0))
for i, j in obj_colls:
# There is collision with an obstacle! We can't add this to the tree.
has_collided = True
return "TRAPPED"
chosen_neighbor = neighbor
break
# If we're here, the goal was successfully reached from a neighbor. Add goal to the tree.
node_goal = self.add_node([q_goal[0], q_goal[1], goal_dir_t], [chosen_neighbor.id])
# LEGACY
#node_goal = self.adj_list[self.get_nearest_neighbor(q_goal)][0]
prev_lookup = dict()
queue = [node_goal.id]
prev_lookup[node_goal.id] = -1
visited = []
while queue:
parent_node_id = queue.pop(0)
neighbor_ids = self.adj_list[parent_node_id][1:]
for neighbor_id in neighbor_ids:
if neighbor_id not in visited:
queue.append(neighbor_id)
node = self.adj_list[neighbor_id][0]
prev_lookup[node.id] = parent_node_id
visited.append(node.id)
if node is self.nodes[0]:
break
path = []
# Now, put together path.
curr_node_id = prev_lookup[self.nodes[0].id]
while curr_node_id is not -1:
curr_node = self.adj_list[curr_node_id][0]
path.append([curr_node.x, curr_node.y, curr_node.t])
curr_node_id = prev_lookup[curr_node_id]
#self.plot_path(path)
return path
def __init__(self,world):
vis.GLPluginInterface.__init__(self)
self.world = world
self.collider = collide.WorldCollider(world)
self.quit = False
def __init__(self, fn):
## Creates a world and loads all the items on the command line
self.world = WorldModel()
self.robotSystemName = 'O'
for f in fn:
print(f)
res = self.world.readFile(f)
if not res:
raise RuntimeError("Unable to load model " + fn)
self.showVis = False
coordinates.setWorldModel(self.world)
vis.lock()
bW.getDoubleRoomWindow(self.world, 8, 8, 1)
vis.unlock()
## Add the world to the visualizer
vis.add("world", self.world)
vp = vis.getViewport()
vp.w, vp.h = 1800, 800
vis.setViewport(vp)
self.robots = []
self.n = self.world.numRobots()
for i in range(self.n):
self.robots.append(
sphero6DoF(self.world.robot(i),
self.world.robot(i).getName()))
self.eps = 0.000001
self.sj = [[0, 0, 0], [0.2, 0, 0]]
self.sjStar = [[-0.070534, -0.097082, 0], [0, -0.06, 0],
[0.070534, -0.097082, 0], [0.057063, -0.018541, 0],
[0.11413, 0.037082, 0], [0.035267, 0.048541, 0],
[0, 0.12, 0], [-0.035267, 0.048541, 0],
[-0.11413, 0.037082, 0], [-0.057063, -0.018541, 0]]
self.sjL = [[0, 0, 0], [0, 0.2, 0], [0, 0.4, 0], [0, 0.6, 0],
[0, 0.8, 0], [0, 1, 0], [0.2, 0, 0], [0.4, 0, 0],
[0.6, 0, 0], [0.8, 0, 0]]
self.sj = self.sjL
self.xB = [-4, 4]
self.yB = [-4, 4]
self.zB = [0.02, 1]
self.rad = 0.04
self.currConfig = [0, 0, 1, 1, 0]
self.scMin = 1
self.scXMin = 1
self.scYMin = 2
self.sumDist = 0
if self.n > 1:
minSij = vectorops.norm(vectorops.sub(self.sj[0], self.sj[1]))
minSijX = math.fabs(self.sj[0][0] - self.sj[1][0])
minSijY = math.fabs(self.sj[0][1] - self.sj[1][1])
for i in range(self.n):
for j in range(self.n):
if i != j:
dist = vectorops.norm(
vectorops.sub(self.sj[i], self.sj[j]))
if dist < minSij:
minSij = dist
dist = math.fabs(self.sj[i][0] - self.sj[j][0])
if dist < minSijX:
minSijX = dist
dist = math.fabs(self.sj[i][1] - self.sj[j][1])
if dist < minSijY:
minSijY = dist
for i in range(self.n):
self.sumDist += vectorops.norm(self.sj[i])
if minSij > self.eps:
self.scMin = 2 * math.sqrt(2) * self.rad / minSij
if minSijX > self.eps:
self.scXMin = 2 * math.sqrt(2) * self.rad / minSijX
if minSijY > self.eps:
self.scYMin = 2 * math.sqrt(2) * self.rad / minSijY
self.scMax = max(2, self.scMin)
self.scB = [self.scMin, 2 * self.scMin]
print('Minimum scale')
print(self.scMin)
vis.add(self.robotSystemName, [so3.identity(), [10, 10, -10]])
vis.setAttribute(self.robotSystemName, "size", 12)
vis.edit(self.robotSystemName)
vis.add("WCS", [so3.identity(), [0, 0, 0]])
vis.setAttribute("WCS", "size", 32)
vis.edit("WCS")
self.collisionChecker = collide.WorldCollider(self.world)
if self.showVis:
## Display the world coordinate system
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
## On-screen text display
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
vis.addText("textbottom",
"WCS: X-axis Red, Y-axis Green, Z-axis Blue",
(20, -30))
print "Starting visualization window#..."
## Run the visualizer, which runs in a separate thread
vis.setWindowTitle("Visualization for kinematic simulation")
vis.show()
simTime = 60
startTime = time.time()
oldTime = startTime
self.setConfig(0, 0, 1, 1, 0)
q = self.robots[0].getConfig()
if self.showVis:
q2f = ['{0:.2f}'.format(elem) for elem in q]
strng = "Robot configuration: " + str(q2f)
vis.addText("textConfig", strng)
colFlag = self.checkCollision()
print(colFlag)
if self.showVis:
time.sleep(10)
