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 relevance(problem): feasible = problem.concept() d = vectorops.distance(problem.start, problem.goal) h = 0.2 nsteps = int(math.ceil(d / h)) env_relevance = np.zeros((W, H)) #try including some adjacent features #footprint = [(0,0),(-1,0),(1,0),(0,-1),(0,1)] footprint = [] for i in range(nsteps + 1): if nsteps == 0: x = problem.start else: x = vectorops.interpolate(problem.start, problem.goal, float(i) / nsteps) x = (int(x[0]), int(x[1])) if feasible or problem.env[x]: env_relevance[x] = 1 for (dx, dy) in footprint: if 0 <= x[0] + dx < W and 0 <= x[1] + dy < H: env_relevance[x[0] + dx, x[1] + dy] = 1 if hierarchical: features = [1] * 2 + [1] * 2 h = build_hierarchy(env_relevance) for m in h: features += [int(v) for v in m.flatten().tolist()] return features return [1] * 2 + [1] * 2 + [ int(v) for v in env_relevance.flatten().tolist() ]
def distBetwLinks(self, index1, index2): ''' returns the distance between 2 links in centimeters ''' robot = self.world.robot(0) link1 = robot.link(index1).getWorldPosition((0, 0, 0)) link2 = robot.link(index2).getWorldPosition((0, 0, 0)) return vectorops.distance(link1, link2) * 100
def bisection(collider, robot, qstart, qgoal): distance = vectorops.distance(qstart, qgoal) epsilon = math.radians(2) numsteps = int(math.ceil(distance / epsilon)) return bisectionhelper(collider, robot, qstart, qgoal, 0, numsteps, numsteps)
def sendPath(path,maxSmoothIters =0, INCREMENTAL=False, limb = None, readConstants=False, internalSpeed=SPEED): # interpolate path linearly between two endpoints if path == None: print "sending empty path" return False for smoothIter in range(maxSmoothIters): # path = path smoothePath = [0]*(len(path)*2-1) for i in range(len(path)-1): smoothePath[i*2] = path[i] smoothePath[i*2 +1] = vectorops.div(vectorops.add(path[i],path[i+1]), 2) smoothePath[-1] = path[-1] path = smoothePath while i <endIndex-1: # print i, endIndex q = path[i] qNext = path[i+1] dt = vectorops.distance(q,qNext) # smooth trajectory by interpolating between two consecutive configurations # if the distance between the two is big # Note: might want to make dt bigger for arm movements (only 7 configurations vs 52) if dt>0.1: qInterp = vectorops.div(vectorops.add(q, qNext), 2) path.insert(i+1, qInterp) endIndex +=1 continue else: i += 1 waitForMove() if counter%internalSpeed == 0 or INCREMENTAL: if limb == 'left': #CONTROLLER.appendMilestoneLeft(q) pass elif limb == 'right': #CONTROLLER.appendMilestoneRight(q) pass else: #print 'milestone #', i, q #CONTROLLER.appendMilestone(q) pass counter +=1 if limb == 'left': #CONTROLLER.appendMilestoneLeft(path[-1]) pass elif limb == 'right': #CONTROLLER.appendMilestoneRight(path[-1]) pass else: pass
def incremental(self, x, u): e = self.space.interpolator(x, u) tmax = u[0] t = 0 c = 0 while t < tmax: t = min(tmax, t + self.timestep) xnext = e.eval(t / tmax) c += vectorops.distance(x, xnext) x = xnext return c
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 selfcollision(robot, qstart, qgoal): """returns whether a self collision is predicted along the route from qstart to qgoal""" distance = vectorops.distance(qstart, qgoal) epsilon = math.radians(2) numsteps = int(math.ceil(distance / epsilon)) for i in xrange(numsteps + 1): u = float(i) / numsteps q = robot.interpolate(qstart, qgoal, u) robot.setConfig(q) if robot.selfCollides(): return True return False
def obstaclecollision(collider, robot, qstart, qgoal): """returns whether a obstacle collision is predicted along the route from qstart to qgoal""" #return None distance = vectorops.distance(qstart, qgoal) epsilon = math.radians(2) numsteps = int(math.ceil(distance / epsilon)) print "numTerrains:", collider.world.numTerrains() for i in xrange(numsteps + 1): u = float(i) / numsteps q = robot.interpolate(qstart, qgoal, u) robot.setConfig(q) collisions = collider.robotTerrainCollisions(robot.index) if any(collisions): return collisions return None
def relevance(problem): feasible = problem.concept() d = vectorops.distance(problem.start, problem.goal) h = 0.2 nsteps = int(math.ceil(d / h)) env_relevance = np.zeros(workspace_size) for i in range(nsteps + 1): if nsteps == 0: x = problem.start else: x = vectorops.interpolate(problem.start, problem.goal, float(i) / nsteps) for (a, b, c) in sphere_collision_tests(x, robot_radius, problem.env): if feasible or problem.env[(a, b, c)]: env_relevance[(a, b, c)] = 1 return [1] * 3 + [1] * 3 + env_relevance.flatten().tolist()
def concept(problem): # compute y d = vectorops.distance(problem.start, problem.goal) h = 0.2 nsteps = int(math.ceil(d / h)) collisions = 0 #hacky "rasterization" by marching down segment for i in range(nsteps + 1): if nsteps == 0: x = problem.start else: x = vectorops.interpolate(problem.start, problem.goal, i / nsteps) if sphere_collision(x, robot_radius, problem.env): # colliding return 0 #collision free return 1
def apply_tendon_forces(self,link1,link2,rest_length): tendon_c2 = 30000.0 tendon_c1 = 100000.0 b1 = self.sim.body(self.model.robot.link(link1)) b2 = self.sim.body(self.model.robot.link(link2)) p1w = se3.apply(b1.getTransform(),self.tendon1_local) p2w = se3.apply(b2.getTransform(),self.tendon2_local) d = vectorops.distance(p1w,p2w) if d > rest_length: #apply tendon force direction = vectorops.unit(vectorops.sub(p2w,p1w)) f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length) b1.applyForceAtPoint(vectorops.mul(direction,f),p1w) b2.applyForceAtPoint(vectorops.mul(direction,-f),p2w) else: pass
def concept(problem): d = vectorops.distance(problem.start, problem.goal) h = 0.2 nsteps = int(math.ceil(d / h)) collisions = 0 #hacky "rasterization" by marching down segment for i in range(nsteps + 1): if nsteps == 0: x = problem.start else: x = vectorops.interpolate(problem.start, problem.goal, float(i) / nsteps) if problem.env[int(x[0]), int(x[1])]: collisions += 1 if collisions >= label_obstacle_threshold: return 0 #collision free return 1
def apply_tendon_forces(self, i, link1, link2, rest_length): tendon_c2 = 30000.0 tendon_c1 = 10000.0 b0 = self.sim.body( self.model.robot.link(self.model.proximal_links[0] - 3)) b1 = self.sim.body(self.model.robot.link(link1)) b2 = self.sim.body(self.model.robot.link(link2)) p0w = se3.apply(b1.getTransform(), self.tendon0_local) p1w = se3.apply(b1.getTransform(), self.tendon1_local) p2w = se3.apply(b2.getTransform(), self.tendon2_local) d = vectorops.distance(p1w, p2w) if d > rest_length: #apply tendon force direction = vectorops.unit(vectorops.sub(p2w, p1w)) f = tendon_c2 * (d - rest_length)**2 + tendon_c1 * (d - rest_length) #print d,rest_length #print "Force magnitude",self.model.robot.link(link1).getName(),":",f f = min(f, 100) #tendon routing force straight = vectorops.unit(vectorops.sub(p2w, p0w)) pulley_direction = vectorops.unit(vectorops.sub(p1w, p0w)) pulley_axis = so3.apply(b1.getTransform()[0], (0, 1, 0)) tangential_axis = vectorops.cross(pulley_axis, pulley_direction) cosangle = vectorops.dot(straight, tangential_axis) #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle base_direction = so3.apply(b0.getTransform()[0], [0, 0, -1]) b0.applyForceAtLocalPoint( vectorops.mul(base_direction, -f), vectorops.madd(p0w, base_direction, 0.04)) b1.applyForceAtLocalPoint( vectorops.mul(tangential_axis, cosangle * f), self.tendon1_local) b1.applyForceAtLocalPoint( vectorops.mul(tangential_axis, -cosangle * f), self.tendon0_local) b2.applyForceAtLocalPoint(vectorops.mul(direction, -f), self.tendon2_local) self.forces[i][1] = vectorops.mul(tangential_axis, cosangle * f) self.forces[i][2] = vectorops.mul(direction, f) else: self.forces[i] = [None, None, None] return
def apply_tendon_forces(self,link1,link2,rest_length): tendon_c2 = 30000.0 tendon_c1 = 100000.0 b1 = self.sim.getBody(self.model.robot.getLink(link1)) b2 = self.sim.getBody(self.model.robot.getLink(link2)) p1w = se3.apply(b1.getTransform(),self.tendon1_local) p2w = se3.apply(b2.getTransform(),self.tendon2_local) d = vectorops.distance(p1w,p2w) if d > rest_length: #apply tendon force direction = vectorops.unit(vectorops.sub(p2w,p1w)) f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length) #print "d",d,"rest length",rest_length,"force",f b1.applyForceAtPoint(vectorops.mul(direction,f),p1w) b2.applyForceAtPoint(vectorops.mul(direction,-f),p2w) else: #print "d",d,"rest length",rest_length pass
def dubinsCarTest(): cspace = Geometric2DCSpace() vspace = BoxConfigurationSpace([-1, -1], [1, 1]) start = [0.5, 0.3, 0] goal = [0.5, 0.7, 0] controlSpace = DubinsCarSpace(MultiConfigurationSpace(cspace, SO2Space())) controlSpace.setDistanceBounds(-0.25, 0.25) controlSpace.setTurnRateBounds(-3.14, 3.14) numControlsPerSample = 1 if numControlsPerSample > 1: controlSpace = RepeatedControlSpace(controlSpace, numControlsPerSample) objective = DubinsCarDistanceObjectiveFunction(numControlsPerSample) goalRadius = 0.1 return PlanningProblem( controlSpace, start, goal, objective=objective, visualizer=DubinsVisualizer(cspace), goalRadius=goalRadius, costLowerBound=lambda x, y: vectorops.distance(x[:2], y[:2]))
def generate_trajectory(qi, qf): i = 0 endIndex = 2 path = [0.0, 0.0] path[0] = qi path[1] = qf print qi, qf while i < endIndex-1: # print i, endIndex q = path[i] qNext = path[i+1] dt = vectorops.distance(q,qNext) # smooth trajectory by interpolating between two consecutive configurations # if the distance between the two is big if dt>0.1: qInterp = vectorops.div(vectorops.add(q, qNext), 2) path.insert(i+1, qInterp) endIndex +=1 continue else: i += 1 print len(path) return path
def distance(self,a,b): return vectorops.distance(a,b)
def distance(self, a, b): #TODO: return a distance metric between a and b that correctly #takes rotation into account. Current implementation assumes #a cartesian space return vectorops.distance(a, b)
def contains(self, point): return (vectorops.distance(point, self.center) <= self.radius)
def euclideanMetric(a, b): return vectorops.distance(a, b)
def length(self): return vectorops.distance(self.a, self.b)
def distance(self,a,b): #TODO: return a distance metric between a and b that correctly #takes rotation into account. Current implementation assumes #a cartesian space return vectorops.distance(a,b)
def contains(self,point): return (vectorops.distance(point,self.center) <= self.radius)
def euclideanMetric(a,b): return vectorops.distance(a,b)
def length(self): return vectorops.distance(self.a,self.b)
def sendPath(path,maxSmoothIters =0, INCREMENTAL=False, limb = None, readConstants=False, internalSpeed=SPEED): # interpolate path linearly between two endpoints if path == None: print "sending empty path" return False # n = self.robot.numLinks() # for i in range(len(path)): # # if we specify a limb, and the path only has seven numbers (DOF for each arm, we shouldn't append 0's) # if readConstants and limb is not None: # # if we're reading a path from the milestones # pass # else: # #print path # if len(path[i])<n: # path[i] += [0.0]*(n-len(path[i])) # #pass for smoothIter in range(maxSmoothIters): # path = path smoothePath = [0]*(len(path)*2-1) for i in range(len(path)-1): smoothePath[i*2] = path[i] smoothePath[i*2 +1] = vectorops.div(vectorops.add(path[i],path[i+1]), 2) smoothePath[-1] = path[-1] path = smoothePath i = 0 endIndex = len(path) # if endIndex==1: # q=path[0] # path[0]=self.robot.getConfig() # path.append(q) counter = 0 #path.append(path[-1]) #endIndex = len(path) #print 'myPath = ', path speed = 1 while i+1 <endIndex: # print i, endIndex q = path[i] qNext = path[i+1] dt = vectorops.distance(q,qNext) dt = dt / speed i += 1 waitForMove() # if INCREMENTAL: # time.sleep(.1) if limb == 'left': CONTROLLER.appendMilestoneLeft(q,dt) #pass elif limb == 'right': CONTROLLER.appendMilestoneRight(q,dt) #pass else: #print 'milestone #', i, q #CONTROLLER.appendMilestone(q) CONTROLLER.appendLinear(q,dt) #pass """