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()