def workspace_distance(a, b): """Returns a distance in R^n or SE(3) between points a and b. If len(a)<=3, this will do cartesian distance. Otherwise, len(a)==6 is required and this will do SE(3) distance (with a max orientation distance of 1).""" if len(a) <= 3: return vectorops.distance(a, b) assert len(a) == 6, "Can only interpolate in R2, R3, or SE(3)" dt = vectorops.distance(a[:3], b[:3]) dR = so3.distance(so3.from_moment(a[3:]), so3.from_moment(b[3:])) return dt + dR / math.pi
def item_in_hand(self, current_pos): min_dist = 10 for i in self.waiting_list: p = self.sim.world.rigidObject(i).getTransform()[1] if vectorops.distance(current_pos[1], p) < min_dist: min_dist = vectorops.distance(current_pos[1], p) if min_dist < 0.4: return True else: return False
def lab2a(robot_model, q, ee_link, ee_local_position, target): """In: - robot_model: a RobotModel instance. - q: a configuration of the robot (an n-element list of floats where n=robot_model.numLinks()). - ee_link: the index of the end effector - ee_local_position: the position of the end effector (ex,ey,ez), expressed in the frame of link ee_link. - target: a 3d target position (tx,ty,tz) Out: - The distance between the end effector's *world* position and target You will need to study the documentation for the RobotModel and RobotModelLink classes. Don't forget that the robot_model's current configuration and the layout of its frames are considered temporary variables. Before lab2a is called, you have no control over the model's current configuration, and anything after this will disregard where the robot is placed. In other words, q is the authoritative representation of the robot's configuration, and robot_model is a "scratch pad". """ #TODO: put your code here robot_model.setConfig(q) end_link = robot_model.link(ee_link) ee_world_position = end_link.getWorldPosition(ee_local_position) return vectorops.distance(ee_world_position, target)
def callback(robot_controller=robot_controller,drawing_controller=drawing_controller,trace=trace, storage=[sim_world,planning_world,sim,controller_vis]): start_clock = time.time() dt = 1.0/robot_controller.controlRate() #advance the controller robot_controller.startStep() drawing_controller.advance(dt) robot_controller.endStep() #update the visualization sim_robot.setConfig(robot_controller.configToKlampt(robot_controller.sensedPosition())) Tee=sim_robot.link(ee_link).getTransform() wp = se3.apply(Tee,ee_local_pos) if len(trace.milestones) == 0 or vectorops.distance(wp,trace.milestones[-1]) > 0.01: trace.milestones.append(wp) trace.times.append(0) if len(trace.milestones)==2: vis.add("trace",trace,color=(0,1,1,1),pointSize=0) if len(trace.milestones) > 200: trace.milestones = trace.milestones[-100:] trace.times = trace.times[-100:] if len(trace.milestones)>2: if _backend=='IPython': vis.remove("trace") vis.add("trace",trace,color=(0,1,1,1),pointSize=0) else: vis.dirty("trace") controller_vis.update() cur_clock = time.time() duration = cur_clock - start_clock time.sleep(max(0,dt-duration))
def segment_point_distance(a, b, x): """Returns (distance, parameter) pair between segment (a,b) and point x""" d = vectorops.sub(b, a) u = vectorops.dot(vectorops.sub(x, a), d) / vectorops.dot(d, d) u = min(1.0, max(u, 0.0)) d = vectorops.distance(x, vectorops.interpolate(a, b, u)) return (d, u)
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 sphere_grid_test(N=5, staggered=True): from klampt import vis from klampt.model import trajectory if staggered: G = sphere_staggered_grid(N) else: G = sphere_grid(N) for n in G.nodes(): x = G.node[n]['params'] vis.add(str(n), x) vis.hideLabel(str(n)) #draw edges? minDist = float('inf') maxDist = 0.0 for i, j in G.edges(): xi = G.node[i]['params'] xj = G.node[j]['params'] vis.add(str(i) + '-' + str(j), trajectory.Trajectory([0, 1], [xi, xj])) vis.hideLabel(str(i) + '-' + str(j)) dist = vectorops.distance(xi, xj) if dist > maxDist: maxDist = dist print "Max dispersion at", i, j, ":", dist if dist < minDist: minDist = dist print "Number of points:", G.number_of_nodes() print "Min/max dispersion:", minDist, maxDist vis.run()
def grasp_from_contacts(contact1,contact2): """Helper: if you have two contacts, this returns an AntipodalGrasp""" d = vectorops.unit(vectorops.sub(contact2.x,contact1.x)) grasp = AntipodalGrasp(vectorops.interpolate(contact1.x,contact2.x,0.5),d) grasp.finger_width = vectorops.distance(contact1.x,contact2.x) grasp.contact1 = contact1 grasp.contact2 = contact2 return grasp
def lab2b(l1, l2, l3, point): """ Compute all IK solutions of a 3R manipulator. The joint axes in the reference configuration are ZYY, with each link's origin displaced by a given amount on the X axis. In: - L1, L2, L3: the link lengths - point: the target position (x,y,z) Out: - A pair (n,solutionList) where n is the number of solutions (either 0, 1, 2, 4, or float('inf')) and solutionList is a list of all solutions. In the n=0 case, it should be an empty list [], and in the n=inf case it should give one example solution. Each solution is a 3-tuple giving the joint angles, in radians. """ a1_1 = math.atan2(point[1], point[0]) a1_2 = a1_1 + math.pi a1 = [a1_1, a1_2] solution_num = 0 solution_list = [] # starting from a1_1 for idx in range(len(a1)): each_a1 = a1[idx] j1 = (l1 * math.cos(each_a1), l1 * math.sin(each_a1), 0) dist = vectorops.distance(point, j1) # from j1 to target if dist > (l2 + l3): pass # no solution elif dist < abs(l2 - l3): pass # no solution elif dist == abs(l2 - l3) and l2 != l3: solution_num += 1 solution_list.append((each_a1, math.pi, 0)) elif dist == (l2 + l3): solution_num += 1 solution_list.append((each_a1, math.pi, 0)) elif dist == 0: # point at the joint 2 return (float('inf'), [(each_a1, math.pi, 0)]) else: cos_angle = (dist**2.0 - l2**2.0 - l3**2.0) / (-2.0 * l2 * l3) a3_1 = (math.pi - math.acos(cos_angle)) a3_2 = (-a3_1) a3 = [a3_1, a3_2] for each_a3 in a3: h = l3 * math.sin(each_a3) ratio = h / dist delta_a = math.asin(ratio) total_a = math.asin(point[2] / dist) if idx == 0: a2 = -(delta_a + total_a) if idx == 1: a2 = math.pi - delta_a + total_a solution_num += 1 solution_list.append((each_a1, a2, each_a3)) return (solution_num, solution_list)
def svg_path_to_trajectory(p): """Produces either a Trajectory or HermiteTrajectory according to an SVG Path. The path must be continuous. """ from svgpathtools import Path, Line, QuadraticBezier, CubicBezier, Arc if not p.iscontinuous(): raise ValueError("Can't do discontinuous paths") pwl = not any(isinstance(seg, (CubicBezier, QuadraticBezier)) for seg in p) if pwl: milestones = [] for seg in p: a, b = seg.start, seg.end if not milestones: milestones.append([a.real, a.imag]) milestones.append([b.real, b.imag]) return Trajectory(list(range(len(milestones))), milestones) times = [] milestones = [] for i, seg in enumerate(p): if isinstance(seg, CubicBezier): a, c1, c2, b = seg.start, seg.control1, seg.control2, seg.end vstart = (c1.real - a.real) * 3, (c1.imag - a.imag) * 3 vend = (b.real - c2.real) * 3, (b.imag - c2.imag) * 3 if not milestones: milestones.append([a.real, a.imag, vstart[0], vstart[1]]) times.append(i) elif vectorops.distance(milestones[-1][2:], vstart) > 1e-4: milestones.append([a.real, a.imag, 0, 0]) times.append(i) milestones.append([a.real, a.imag, vstart[0], vstart[1]]) times.append(i) milestones.append([b.real, b.imag, vend[0], vend[1]]) times.append(i + 1) elif isinstance(seg, Line): a, b = seg.start, seg.end if not milestones: milestones.append([a.real, a.imag, 0, 0]) times.append(i) elif vectorops.norm(milestones[-1][2:]) > 1e-4: milestones.append([a.real, a.imag, 0, 0]) times.append(i) milestones.append([b.real, b.imag, 0, 0]) times.append(i + 1) else: raise NotImplementedError( "Can't handle pieces of type {} yet".format( seg.__class.__name__)) return HermiteTrajectory(times, milestones)
def maximum_violation_oracle( grasping_problem, x, y, z, sample_surrounding_size=EXTRA_SAMPLE_NEIGHBORHOOD_SIZE, sample_number=EXTRA_SAMPLE_COUNT, smallest_distance=MINIMUM_INDEX_DISTANCE): IndexSet = [] dim_robo = len(x) # Select new index point for i in range(dim_robo): if grasping_problem.colliding_links is not None and i not in grasping_problem.colliding_links: continue if not grasping_problem.complementarity.robot.geometry[i]: continue # Find the closest point between the ith link and the object dist, p_robo, p_obj = grasping_problem.complementarity.robot.geometry[ i].distance(grasping_problem.complementarity.obj) # Only add the index points which are not too close to the existing index points IndexSet.append([i, p_obj]) # Sample some points around the closest point if sample_number > 0: pc_sub = find_k_nearest(p_obj, grasping_problem.xyz_eq.point_cloud_tree, grasping_problem.xyz_eq.point_cloud_array, sample_surrounding_size) sampled = [] successed_sample = [] while len(successed_sample) < sample_number and len( sampled) < sample_surrounding_size: idx = np.random.randint(sample_surrounding_size) if idx not in sampled: point = pc_sub[idx] if all( vectorops.distance(idx[1], point) > smallest_distance for idx in y + IndexSet): IndexSet.append([i, point]) successed_sample.append(idx) sampled.append(idx) return IndexSet
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 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 makeHapticCartesianPoseMsg(self): deviceState = self.haptic_client.deviceState if len(deviceState)==0: print "No device state" return None transforms = [self.serviceThread.eeGetter_left.get(),self.serviceThread.eeGetter_right.get()] update = False commanding_arm=[0,0] for i,dstate in enumerate(deviceState): if dstate.buttonDown[0]: commanding_arm[i]=1 if self.startTransforms[i] == None: self.startTransforms[i] = transforms[i] #RESET THE HAPTIC FORCE FEEDBACK CENTER #self.serviceThread.hapticupdater.activateSpring(kP=hapticArmSpringStrength,kD=0,center=dstate.position,device=i) #UPDATE THE HAPTIC FORCE FEEDBACK CENTER FROM ROBOT EE POSITION #need to invert world coordinates to widget coordinates to haptic device coordinates #widget and world translations are the same dx = vectorops.sub(transforms[i][1],self.startTransforms[i][1]); dxwidget = vectorops.div(dx,hapticArmTranslationScaling) R,device_center = widgetToDeviceTransform(so3.identity(),vectorops.add(dxwidget,dstate.widgetInitialTransform[0][1])) #print "Device position error",vectorops.sub(dstate.position,device_center) if vectorops.distance(dstate.position,device_center) > 0.03: c = vectorops.madd(device_center,vectorops.unit(vectorops.sub(dstate.position,device_center)),0.025) self.serviceThread.hapticupdater.activateSpring(kP=hapticArmSpringStrength,kD=0,center=c,device=i) else: #deactivate self.serviceThread.hapticupdater.activateSpring(kP=0,kD=0,device=i) if dstate.newupdate: update = True else: if self.startTransforms[i] != None: self.serviceThread.hapticupdater.deactivateHapticFeedback(device=i) self.startTransforms[i] = None dstate.newupdate = False if update: msg = {} msg['type'] = 'CartesianPose' T1 = self.getDesiredCartesianPose('left',0) R1,t1=T1 T2 = self.getDesiredCartesianPose('right',1) R2,t2=T2 transforms = [(R1,t1),(R2,t2)] if commanding_arm[0] and commanding_arm[1]: msg['limb'] = 'both' msg['position'] = t1+t2 msg['rotation'] = R1+R2 msg['maxJointDeviation']=0.5 msg['safe'] = int(CollisionDetectionEnabled) self.CartesianPoseControl = True return msg elif commanding_arm[0] ==0: msg['limb'] = 'right' msg['position'] = t2 msg['rotation'] = R2 msg['maxJointDeviation']=0.5 msg['safe'] = int(CollisionDetectionEnabled) self.CartesianPoseControl = True return msg else: msg['limb'] = 'left' msg['position'] = t1 msg['rotation'] = R1 msg['maxJointDeviation']=0.5 msg['safe'] = int(CollisionDetectionEnabled) self.CartesianPoseControl = True return msg else: return None
vis.setColor('B',1,1,0,0.5) else: vis.setColor('B',0,1,0,0.5) elif mode == 'distance': settings = DistanceQuerySettings() try: res = a.distance_ext(b,settings) except Exception as e: print("Exception encountered:",e) continue if res < 0: vis.setColor('B',1,1,0,0.5) else: vis.setColor('B',0,1,0,0.5) if res.hasClosestPoints: if abs(abs(res.d) - vectorops.distance(res.cp1,res.cp2)) > 1e-7: print("ERROR IN DISTANCE?",res.d,vectorops.distance(res.cp1,res.cp2)) vis.lock() vis.add("cpa",[v for v in res.cp1]) vis.add("cpb",[v for v in res.cp2]) vis.setColor("cpa",0,0,1) vis.setColor("cpb",0,0,1) vis.add("cpline",Trajectory([0,1],[[v for v in res.cp1],[v for v in res.cp2]])) vis.setColor("cpline",1,0.5,0) drawExtra.add("cpa") drawExtra.add("cpb") drawExtra.add("cpline") if res.hasGradients: l = 0.1 vis.add("cpagrad",vectorops.madd(res.cp1,res.grad1,l)) vis.add("cpbgrad",vectorops.madd(res.cp2,res.grad2,l))
def find_target(self): self.current_target=self.waiting_list[0] best_p=self.sim.world.rigidObject(self.current_target).getTransform()[1] self.current_target_pos=best_p for i in self.waiting_list: p=self.sim.world.rigidObject(i).getTransform()[1] # print i,p[2],vectorops.distance([0,0],[p[0],p[1]]) if i not in self.failedpickup: if p[2]>best_p[2]+0.05: self.current_target=i self.current_target_pos=p # print 'higher is easier!' best_p=p elif p[2]>best_p[2]-0.04 and vectorops.distance([0,0],[p[0],p[1]])<vectorops.distance([0,0],[best_p[0],best_p[1]]): self.current_target=i self.current_target_pos=p best_p=p elif self.current_target in self.failedpickup: self.current_target=i self.current_target_pos=p best_p=p d_y=-0.02 face_down=face_down_forward self.tooclose = False; if best_p[1]>0.15: d_y=-0.02 face_down=face_down_backward self.tooclose = True; print 'too close to the wall!!' elif best_p[1]<-0.15: d_y=0.02 print best_p self.tooclose = True; print 'too close to the wall!!' #here is hardcoding the best relative position for grasp the ball target=(face_down,vectorops.add(best_p,[0,d_y,0.14])) if self.tooclose: if best_p[0]<0: self.boxside=boxleft aready_pos=start_pos_left else: self.boxside=boxright aready_pos=start_pos_right aready_pos[1][0]=best_p[0] balllocation = best_p handballdiff = vectorops.sub(aready_pos[1],best_p) axis = vectorops.unit(vectorops.cross(handballdiff,aready_pos[1])) axis = (1,0,0) if best_p[1]>0: axis=(-1,0,0) angleforaxis = -1*math.acos(vectorops.dot(aready_pos[1],handballdiff)/vectorops.norm(aready_pos[1])/vectorops.norm(handballdiff)) angleforaxis = so2.normalize(angleforaxis) #if angleforaxis>math.pi: # angleforaxis=angleforaxis-2*math.pi adjR = so3.rotation(axis, angleforaxis) print balllocation print vectorops.norm(aready_pos[1]),vectorops.norm(handballdiff),angleforaxis target=(adjR,vectorops.add(best_p,vectorops.div(vectorops.unit(handballdiff),5))) # print self.current_target # print 'find target at:',self.current_target_pos return target
def contains(self, point): return (vectorops.distance(point, self.center) <= self.radius)
def go_to(self, controller, current_pos, goal_pos, speed=move_speed): t = vectorops.distance(current_pos[1], goal_pos[1]) / speed send_moving_base_xform_linear(controller, goal_pos[0], goal_pos[1], t)
def at_destination(self, current_pos, goal_pos): if vectorops.distance(current_pos[1], goal_pos[1]) < 0.05: return True else: return False
def target_is_not_moving(self): if vectorops.distance(self.current_target_pos,self.sim.world.rigidObject(self.current_target).getTransform()[1])<0.03: return True else: return False
def ball_in_hand(self,current_pos): if vectorops.distance(current_pos[1],self.sim.world.rigidObject(self.current_target).getTransform()[1])<0.3: return True else: return False
def local_score_oracle(grasping_problem, x, y, z, score_surrounding_size=LOCAL_NEIGHBORHOOD_SIZE, sample_surrounding_size=EXTRA_SAMPLE_NEIGHBORHOOD_SIZE, sample_number=EXTRA_SAMPLE_COUNT, smallest_distance=MINIMUM_INDEX_DISTANCE): t0 = time.time() IndexSet = [] dim_robo = len(x) T = grasping_problem.xyz_eq.env_geom.getTransform() CoM = se3.apply(T, grasping_problem.xyz_eq.env_obj.getMass().getCom()) balance_residual = grasping_problem.xyz_eq.value(x, y, z) # Select new index point for i in range(dim_robo): # Add the point with the highest score as index point if grasping_problem.colliding_links is not None and i not in grasping_problem.colliding_links: continue if not grasping_problem.complementarity.robot.geometry[i]: continue # Find the closest point between the ith link and the object dist, p_robo, p_obj = grasping_problem.complementarity.robot.geometry[ i].distance(grasping_problem.complementarity.obj) # Only add the index points which are not too close to the existing index points if all( vectorops.distance(idx[1], p_obj) > smallest_distance for idx in y): IndexSet.append([i, p_obj]) # Sample some points around the closest point if sample_number > 0: pc_sub = find_k_nearest(p_obj, grasping_problem.xyz_eq.point_cloud_tree, grasping_problem.xyz_eq.point_cloud_array, sample_surrounding_size) sampled = [] successed_sample = [] while len(successed_sample) < sample_number and len( sampled) < sample_surrounding_size: idx = np.random.randint(sample_surrounding_size) if idx not in sampled: point = pc_sub[idx] if all( vectorops.distance(idx[1], point) > smallest_distance for idx in y + IndexSet): IndexSet.append([i, point]) successed_sample.append(idx) sampled.append(idx) # Calculate score for points in the surrounding of the closest point score = [] pc_sub_score = find_k_nearest( p_obj, grasping_problem.xyz_eq.point_cloud_tree, grasping_problem.xyz_eq.point_cloud_array, score_surrounding_size) for point in pc_sub_score: score.append( score_function(grasping_problem, point, i, CoM, balance_residual, x, y, z)) # Find the point has the highest score index = heapq.nlargest(1, list(range(len(score))), score.__getitem__) highest_score_point = pc_sub_score[index[0]] if all( vectorops.distance(idx[1], highest_score_point) > smallest_distance for idx in y + IndexSet): IndexSet.append([i, highest_score_point]) # Sample some points around the highest score point # pc_sub = find_k_nearest(highest_score_point,grasping_problem.xyz_eq.point_cloud_tree,grasping_problem.xyz_eq.point_cloud_array,sample_surrounding_size) # sampled = [] # successed_sample = [] # while len(successed_sample) < sample_number and len(sampled) < sample_surrounding_size: # idx_ = np.random.randint(sample_surrounding_size) # if idx_ not in sampled: # point = pc_sub[idx_] # dist, p_robo, p_obj = grasping_problem.complementarity.robot.geometry[i].distance(point) # if all (vectorops.distance(idx[1],p_obj) > smallest_distance for idx in y + IndexSet): # IndexSet.append([i,p_obj]) # successed_sample.append(idx) # sampled.append(idx_) t1 = time.time() print("Oracle computation time %.3f" % (t1 - t0)) return IndexSet
else: vis.setColor('B', 0, 1, 0, 0.5) elif mode == 'distance': settings = DistanceQuerySettings() try: res = a.distance_ext(b, settings) except Exception as e: print("Exception encountered:", e) continue tq1 = time.time() if res.d < 0: vis.setColor('B', 1, 1, 0, 0.5) else: vis.setColor('B', 0, 1, 0, 0.5) if res.hasClosestPoints: if abs(abs(res.d) - vectorops.distance(res.cp1, res.cp2)) > 1e-7: print("ERROR IN DISTANCE?", res.d, vectorops.distance(res.cp1, res.cp2)) vis.lock() vis.add("cpa", [v for v in res.cp1]) vis.add("cpb", [v for v in res.cp2]) vis.setColor("cpa", 0, 0, 1) vis.setColor("cpb", 0, 0, 1) vis.add( "cpline", Trajectory([0, 1], [[v for v in res.cp1], [v for v in res.cp2]])) vis.setColor("cpline", 1, 0.5, 0) drawExtra.add("cpa") drawExtra.add("cpb") drawExtra.add("cpline")
def monotonicImprovementCost(a, b): if vectorops.distance(a, goal) < vectorops.distance(b, goal): return float('inf') else: return space.cspace.distance(a, b)
def distance(self, y1, y2): if y1[0] != y2[0]: return np.inf return vectorops.distance(y1[1], y2[1])
def euclideanMetric(a,b): return vectorops.distance(a,b)
def distance(self, point): return (vectorops.distance(point, self.center) - self.radius)
def distance(self, a, b): # need to set the angle weight return vectorops.distance(a[:2], b[:2]) + abs(so2.diff(a[2], b[2])) * 0.1