def getCollisionFreePath(self, start, goal, iterations):
""" Given a start and a goal configuration, returns a collision-free path between the two configurations"""
""" Currently takes forever to find a path... Needs more work"""
#MotionPlan.setOptions(type="rrt", perturbationRadius=2.0, bidirectional=True)
#MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.25)
MotionPlan.setOptions(type="sbl",
perturbationRadius=2.0,
connectionThreshold=0.5,
bidirectional=True)
#MotionPlan.setOptions(type="lazyrrg*")
#space = ObstacleCSpace(self.collider, self.robot)
#planner = MotionPlan(space)
#planner = robotplanning.planToConfig(self.world, self.robot, goal, type="prm", knn=10, connectionThreshold=0.1)
space = robotcspace.RobotCSpace(self.robot, WorldCollider(self.world))
jointLimits = self.robot.getJointLimits()
lower = jointLimits[0]
higher = jointLimits[1]
for i in range(12):
lower[i] = 0
higher[i] = 0
newLimits = (lower, higher)
space.bound = zip(*newLimits)
planner = cspace.MotionPlan(space)
planner.setEndpoints(start, goal)
planner.planMore(iterations)
V, E = planner.getRoadmap()
self.roadMap = (V, E)
return planner.getPath()
def __init__(self, world):
GLWidgetProgram.__init__(self, "Manual poser")
#start up the poser in the currently commanded configuration
q = motion.robot.getKlamptCommandedPosition()
world.robot(0).setConfig(q)
self.world = world
self.robot = world.robot(0)
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
self.roadMap = ([], [])
robot = world.robot(0)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
self.left_arm_link_names = left_arm_link_names
self.right_arm_link_names = right_arm_link_names
missing_left_arm_names = [
'left_upper_forearm_visual', 'left_upper_elbow_visual'
]
missing_right_arm_names = [
'right_upper_forearm_visual', 'right_upper_elbow_visual'
]
#self.left_arm_link_names+=missing_left_arm_names
#self.right_arm_link_names+=missing_right_arm_names
self.left_arm_link_names = [
'left_upper_forearm', 'left_lower_forearm', 'left_wrist',
'left_upper_forearm_visual', 'left_gripper:base'
]
self.right_arm_link_names = [
'right_upper_forearm', 'right_lower_forearm', 'right_wrist',
'right_upper_forearm_visual', 'right_gripper:base'
]
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
self.firstTimeHack = True
self.world.makeTerrain('terrain')
self.subscribe()
self.newPC = None
self.collider = WorldCollider(self.world)
def getClosestConfig(self, robot, target, iterations, c, numsteps):
cost = 9999
res = None
start = robot.getConfig()
#goal = ik.objective(robot.link("left_gripper:base"), local = [(0,0,0)], world = [target])
#goal = ik.objective(robot.link("left_wrist"), local = [(0,0,0)], world = [target])
s = IKSolver(robot)
objective = IKObjective()
objective.setFixedPoint(
robot.link("left_gripper:base").getIndex(), (0, 0, 0), target)
s.add(objective)
s.setActiveDofs([12, 13, 14, 15, 16, 18, 19])
print "Active DOFs:", s.getActiveDofs()
for i in range(iterations):
#if ik.solve(goal):
(ret, iters) = s.solve(100, 1e-4)
if ret:
end = robot.getConfig()
print "*****************************", end
qmin, qmax = robot.getJointLimits()
#print qmin
#print qmax
flag = False
for k in range(len(end)):
if end[k] < qmin[k] or end[k] > qmax[k]:
flag = True
break
if flag:
start = self.perturb(start, 0.1)
robot.setConfig(start)
continue
for j in xrange(numsteps + 1):
u = float(j) / numsteps
print "u is:", u
q = robot.interpolate(end, start, u)
#q = end
print "interpolated q is:", q
if not inCollision(WorldCollider(self.world), robot, q):
newcost = vectorops.distance(
q, end) + c * vectorops.distance(q, start)
if newcost < cost:
res = q
cost = newcost
break
start = self.perturb(start, 0.1)
robot.setConfig(start)
print "res is:", res
return res
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
if c == 'h':
print '[space]: send the current posed milestone'
print 'q: clean quit'
elif c == ' ':
q = self.robotPoser.get()
q0 = motion.robot.getKlamptCommandedPosition()
t1 = time.time()
#collisions = obstaclecollision(WorldCollider(self.world),self.world.robot(0),q0,q)
collides = bisection(WorldCollider(self.world),
self.world.robot(0), q0, q)
print "Obstacle collision detection done in time", +time.time() - t1
exit(0)
for i in range(self.world.robot(0).numLinks()):
self.world.robot(0).link(i).appearance().setColor(
0.5, 0.5, 0.5, 1)
#if not self.firstTimeHack and selfcollision(self.world.robot(0),q0,q):
if collides:
print "Invalid configuration, it self-collides"
elif not self.firstTimeHack and collisions != None:
#clear all links to gray
for pairs in collisions:
print "Link " + str(
pairs[0].getIndex()) + " collides with obstacle"
self.world.robot(0).link(
pairs[0].getIndex()).appearance().setColor(1, 0, 0, 1)
else:
self.firstTimeHack = False
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg))
robot.right_gripper.command(qrg, [1] * len(qrg),
[1] * len(qrg))
#robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
#robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
elif c == 'q':
motion.robot.shutdown()
exit(0)
else:
GLWidgetProgram.keyboardfunc(self, c, x, y)
self.refresh()
def getClosestConfig(self, robot, target, iterations, c, numsteps):
""" given a target object position, returns a configuration with end effector close to the target object but without colliding with it"""
cost = 9999
res = None
start = robot.getConfig()
s = IKSolver(robot)
objective = IKObjective()
objective.setFixedPoint(
robot.link("left_gripper:base").getIndex(), (0, 0, 0), target)
s.add(objective)
s.setActiveDofs([12, 13, 14, 15, 16, 18, 19])
for i in range(iterations):
(ret, iters) = s.solve(100, 1e-4)
if ret:
end = robot.getConfig()
qmin, qmax = robot.getJointLimits()
flag = False
for k in range(len(end)):
if end[k] < qmin[k] or end[k] > qmax[k]:
flag = True
break
if flag:
start = self.perturb(start, 0.1)
robot.setConfig(start)
continue
for j in xrange(numsteps + 1):
u = float(j) / numsteps
q = robot.interpolate(end, start, u)
if not inCollision(WorldCollider(self.world), robot, q):
newcost = vectorops.distance(
q, end) + c * vectorops.distance(q, start)
if newcost < cost:
res = q
cost = newcost
break
start = self.perturb(start, 0.1)
robot.setConfig(start)
return res
def getCollisionFreePath(self, start, goal, iterations):
#MotionPlan.setOptions(type="rrt", perturbationRadius=2.0, bidirectional=True)
#MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.25)
MotionPlan.setOptions(type="sbl",
perturbationRadius=2.0,
connectionThreshold=0.5,
bidirectional=True)
#MotionPlan.setOptions(type="lazyrrg*")
#space = ObstacleCSpace(self.collider, self.robot)
#planner = MotionPlan(space)
#planner = robotplanning.planToConfig(self.world, self.robot, goal, type="prm", knn=10, connectionThreshold=0.1)
print "milestone 1"
space = robotcspace.RobotCSpace(self.robot, WorldCollider(self.world))
jointLimits = self.robot.getJointLimits()
lower = jointLimits[0]
higher = jointLimits[1]
for i in range(12):
lower[i] = 0
higher[i] = 0
newLimits = (lower, higher)
space.bound = zip(*newLimits)
print "milestone 2"
planner = cspace.MotionPlan(space)
print "milestone 3"
planner.setEndpoints(start, goal)
print "before planning"
planner.planMore(iterations)
print "after planning"
V, E = planner.getRoadmap()
self.roadMap = (V, E)
print "No. of vertices:", len(V)
print "Vertices:", V
print "Edges:", E
return planner.getPath()
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
if c == 'h':
print '[space]: send the current posed milestone'
print 'q: clean quit'
elif c == ' ':
q = self.robotPoser.get()
#print "space q:", q
q0 = motion.robot.getKlamptCommandedPosition()
#print q0
#collider = WorldCollider(self.world)
collisions = obstaclecollision(WorldCollider(self.world),
self.world.robot(0), q0, q)
for i in range(self.world.robot(0).numLinks()):
self.world.robot(0).link(i).appearance().setColor(
0.5, 0.5, 0.5, 1)
if not self.firstTimeHack and selfcollision(
self.world.robot(0), q0, q):
print "Invalid configuration, it self-collides"
elif not self.firstTimeHack and collisions != None:
#clear all links to gray
for pairs in collisions:
print "Link " + str(
pairs[0].getIndex()) + " collides with obstacle"
self.world.robot(0).link(
pairs[0].getIndex()).appearance().setColor(1, 0, 0, 1)
else:
self.firstTimeHack = False
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
print
print
print "Moving", q0, "->", q
print
print
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
print "space prg:", qrg
robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg))
robot.right_gripper.command(qrg, [1] * len(qrg),
[1] * len(qrg))
#robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
#robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
elif c == 'q':
motion.robot.shutdown()
exit(0)
elif c == 'p':
"""given a target object position, automatically moves the end effector close to the target object"""
#print "Joint limits:", self.robot.getJointLimits()
start = self.world.robot(0).getConfig()
#print "end effector original position:", self.robot.link('left_gripper:base').getWorldPosition((0,0,0))
#print "start", start
"""target position currently fixed at (0.8,0.1,1) for testing"""
target = self.getClosestConfig(self.world.robot(0), (0.8, 0.1, 1),
100, 0.1, 100)
if target == None:
# print "Cannot solve IK"
return
self.robot.setConfig(target)
#print "ik result:", target
#print "end effector position:", self.robot.link('left_gripper:base').getWorldPosition((0,0,0))
path = self.getCollisionFreePath(start, target, 10)
#print "path:", path
for q in path:
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
if qlg:
robot.left_gripper.command(qlg, [1] * len(qlg),
[1] * len(qlg))
if qrg:
robot.right_gripper.command(qrg, [1] * len(qrg),
[1] * len(qrg))
print "getKlamptCommandedPosition:", robot.getKlamptCommandedPosition(
)
else:
GLWidgetProgram.keyboardfunc(self, c, x, y)
self.refresh()
for i in range(world.numTerrains()):
visualization.add("terrain" + str(i), world.terrain(i))
#if you want to just see the robot in a pop up window...
if DO_SIMPLIFY and DEBUG_SIMPLIFY:
visualization.dialog()
#Automatic construction of space
if not CLOSED_LOOP_TEST:
if not MANUAL_SPACE_CREATION:
space = robotplanning.makeSpace(world=world,
robot=robot,
edgeCheckResolution=1e-2,
movingSubset='all')
else:
#Manual construction of space
collider = WorldCollider(world)
space = robotcspace.RobotCSpace(robot, collider)
space.eps = 1e-2
space.setup()
else:
#TESTING: closed loop robot cspace
collider = WorldCollider(world)
obj = ik.objective(robot.link(robot.numLinks() - 1),
local=[0, 0, 0],
world=[0.5, 0, 0.5])
visualization.add("IK goal", obj)
visualization.dialog()
space = robotcspace.ClosedLoopRobotCSpace(robot, obj, collider)
space.eps = 1e-2
space.setup()
