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
