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 random_rotation(): """Returns a uniformly distributed rotation matrix.""" q = [random.gauss(0,1),random.gauss(0,1),random.gauss(0,1),random.gauss(0,1)] q = vectorops.unit(q) theta = math.acos(q[3])*2.0 if abs(theta) < 1e-8: m = [0,0,0] else: m = vectorops.mul(vectorops.unit(q[0:3]),theta) return so3.from_moment(m)
def so3_grid(N): """Returns a nx.Graph describing a grid on SO3 with resolution N. The resulting graph has ~4N^3 nodes The node attribute 'params' gives the so3 element. """ assert N >= 1 G = nx.Graph() R0 = so3.from_quaternion(vectorops.unit((1, 1, 1, 1))) namemap = dict() for ax in range(4): for i in xrange(N + 1): u = 2 * float(i) / N - 1.0 u = math.tan(u * math.pi * 0.25) for j in xrange(N + 1): v = 2 * float(j) / N - 1.0 v = math.tan(v * math.pi * 0.25) for k in xrange(N + 1): w = 2 * float(k) / N - 1.0 w = math.tan(w * math.pi * 0.25) idx = [i, j, k] idx = idx[:ax] + [N] + idx[ax:] idx = tuple(idx) if idx in namemap: continue else: namemap[idx] = idx flipidx = tuple([N - x for x in idx]) namemap[flipidx] = idx q = [u, v, w] q = q[:ax] + [1.0] + q[ax:] #print idx,"->",q q = vectorops.unit(q) R = so3.mul(so3.inv(R0), so3.from_quaternion(q)) G.add_node(idx, params=R) for ax in range(4): for i in xrange(N + 1): for j in xrange(N + 1): for k in xrange(N + 1): baseidx = [i, j, k] idx = baseidx[:ax] + [N] + baseidx[ax:] idx = tuple(idx) for n in range(3): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: continue nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) G.add_edge(namemap[idx], namemap[nidx]) return G
def df_dz(self, x, y, z): n = len(y) m = len(z) // n result = np.zeros((6, len(z))) T = self.env_geom.getTransform() CoM = se3.apply(T, self.env_obj.getMass().getCom()) for i in range(n): point = y[i][1] Normal_normal = normal(self.env_geom, point) n1 = so3.canonical(Normal_normal)[3:6] n2 = so3.canonical(Normal_normal)[6:9] Normal_friction = [] for j in range(6): n_tmp = (math.cos((math.pi / 3) * j) * np.array(n1) + math.sin( (math.pi / 3) * j) * np.array(n2)).tolist() Normal_friction.append(vectorops.unit(n_tmp)) Cross_Normal = list( vectorops.cross(vectorops.sub(point, CoM), Normal_normal)) Cross_Normal_friction = [] for j in range(6): cross_Normal_v = list( vectorops.cross(vectorops.sub(point, CoM), Normal_friction[j])) Cross_Normal_friction.append(cross_Normal_v) result[0:3, i * m:(i + 1) * m - 1] = np.asarray(Normal_friction).T result[0:3, (i + 1) * m - 1:(i + 1) * m] = np.asarray(Normal_normal).reshape((3, 1)) result[3:6, i * m:(i + 1) * m - 1] = np.asarray(Cross_Normal_friction).T result[3:6, (i + 1) * m - 1:(i + 1) * m] = np.asarray(Cross_Normal).reshape((3, 1)) return result
def segment_triangle_intersection(seg, tri, umin=0, umax=1): """Given a 3D segment (p,q) and a triangle (a,b,c), returns the point of intersection if the region of the segment in interval [umin,umax] intersects the triangle. Otherwise returns None""" p, q = seg a, b, c = tri d = vectorops.sub(b, a) e = vectorops.sub(c, a) n = vectorops.cross(d, e) if vectorops.norm(n) < 1e-7: #degenerate return None n = vectorops.unit(n) ofs = vectorops.dot(n, a) #Solve for n^T(p + u(q-p)) = ofs denom = vectorops.dot(n, vectorops.sub(q, p)) numer = ofs - vectorops.dot(n, p) if abs(denom) < 1e-7: #near parallel return None u = numer / denom if u < umin or u > umax: return None #now check whether point of intersection lies in triangle x = vectorops.madd(p, vectorops.sub(q, p), u) xloc = vectorops.sub(x, a) #solve for coordinates [r,s,t] such that xloc = r*n + s*d + t*e try: rst = np.dot(np.linalg.inv(np.array([n, d, e]).T), xloc) except np.linalg.linalg.LinAlgError: return None r, s, t = rst assert abs(r) < 1e-4 if s >= 0 and t >= 0 and s + t <= 1: return x return None
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 rand_rotation(): q = [ random.gauss(0, 1), random.gauss(0, 1), random.gauss(0, 1), random.gauss(0, 1) ] q = vectorops.unit(q) return so3.from_quaternion(q)
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 df_dx(self, q, index_pt): link_idx, pt_obj = index_pt dist, point, _ = self.robot.geometry[link_idx].distance(pt_obj) if dist > 1e-3: worlddir = vectorops.unit(vectorops.sub(point, pt_obj)) #worlddir2 = self.robot.geometry[link_idx].normal(point) #worlddir3 = vectorops.mul(normal(self.obj,pt_obj),-1.0) #print("Comparing normals: {} vs {} vs {}".format('%.2f %.2f %.2f'%tuple(worlddir),'%.2f %.2f %.2f'%tuple(worlddir2),'%.2f %.2f %.2f'%tuple(worlddir3))) else: worlddir = self.robot.geometry[link_idx].normal(point) #worlddir = vectorops.mul(normal(self.obj,pt_obj),-1.0) Jp = self.robot.robot.link(link_idx).getPositionJacobian( self.robot.robot.link(link_idx).getLocalPosition(point[0:3])) return np.dot(np.array(Jp).T, worlddir) * self.scale
def force(q, target, obstacles): """Returns the potential field force for a robot at configuration q, trying to reach the given target, with the specified obstacles. Input: - q: a 2D point giving the robot's configuration - robotRadius: the radius of the robot - target: a 2D point giving the robot's target - obstacles: a list of Circle's giving the obstacles """ #basic target-tracking potential field implemented here # calculate the attractive force due to the goal f = vectorops.mul(vectorops.sub(target, q), attractiveConstant) for obstacle in obstacles: dist = obstacle.distance(q) # only add the repulsive force if the obstacle is "within" range if dist > repulsiveDistance: continue magnitude = (1.0 / dist - 1 / repulsiveDistance) # alter the magnitude to have a repulsiveConstant and to # divide by square of repulsiveDistance # Dividing by the square strengthens the repulsive force # The repuslvieConstant scales the strength of the repulsive force linearly magnitude = repulsiveConstant * magnitude / (dist**2) # set the direction of repulsive force away from the obstacle direction = vectorops.unit(vectorops.sub(q, obstacle.center)) f = vectorops.madd(f, direction, magnitude) #limit the norm of f if vectorops.norm(f) > 1: f = vectorops.unit(f) return f
def value(self, x, y, z): result = np.zeros(6) if len(y) == 0: result[0:3] += self.mass * self.gravity_coefficient * np.asarray( self.gravity_direction) return result n = len(y) m = len(z) // n assert m == 7 T = self.env_geom.getTransform() CoM = se3.apply(T, self.env_obj.getMass().getCom()) for i in range(n): point = y[i][1] f_normal = z[m * (i + 1) - 1] f_friction = z[m * i:m * (i + 1) - 1] Normal_normal = normal(self.env_geom, point) n1 = so3.canonical(Normal_normal)[3:6] n2 = so3.canonical(Normal_normal)[6:9] Normal_friction = [] for j in range(6): n_tmp = (math.cos((math.pi / 3) * j) * np.array(n1) + math.sin( (math.pi / 3) * j) * np.array(n2)).tolist() Normal_friction.append(vectorops.unit(n_tmp)) Cross_Normal = list( vectorops.cross(vectorops.sub(point, CoM), Normal_normal)) Cross_Normal_friction = [] for j in range(6): cross_Normal_v = list( vectorops.cross(vectorops.sub(point, CoM), Normal_friction[j])) Cross_Normal_friction.append(cross_Normal_v) result[0:3] += np.asarray(Normal_normal) * f_normal + np.asarray( f_friction) @ np.asarray(Normal_friction) result[3:6] += np.asarray(Cross_Normal) * f_normal + np.asarray( f_friction) @ np.asarray(Cross_Normal_friction) result[0:3] += self.mass * self.gravity_coefficient * np.asarray( self.gravity_direction) return result
def force(q, target, obstacles): """Returns the potential field force for a robot at configuration q, trying to reach the given target, with the specified obstacles. Input: - q: a 2D point giving the robot's configuration - robotRadius: the radius of the robot - target: a 2D point giving the robot's target - obstacles: a list of Circle's giving the obstacles """ #basic target-tracking potential field implemented here #TODO: implement your own potential field att_force = vectorops.mul(vectorops.sub(target, q), attractiveConstant) total_rep_force = [0, 0] for each_ob in obstacles: cur_rep_force = repForce(q, each_ob, repulsiveDistance) total_rep_force = vectorops.add(total_rep_force, cur_rep_force) total_force = vectorops.add(att_force, total_rep_force) #limit the norm of f if vectorops.norm(total_force) > 1: total_force = vectorops.unit(vectorops.add(total_force, (1e-10, 1e-10))) return total_force
def repForce(point_loc, in_obstacle, max_detect_dist): ''' return the replusive force according to the distance between the point and the obstacle if it is too far away just return 0: point_loc: (x,y) in_obstacle: circle object ''' obs_center = in_obstacle.getCenter() dist = in_obstacle.distance(point_loc) # if dist <= 0: # return(0,0) # #assert("Check current point location, overlaps the obstacle") if dist > max_detect_dist: return (0, 0) else: # direction to the obstacle center dir_center = vectorops.sub(point_loc, obs_center) # normalize vector dir_center = vectorops.unit(dir_center) # to avoid local min add a small tangential vector # dir_around = (dir_center[1],dir_center[0]) return vectorops.mul(dir_center, 1 / abs(dist))
def sphere_grid(N): """Returns a nx.Graph describing a grid on S2 with resolution N. The resulting graph has ~6N^2 nodes The node attribute 'params' gives the point. """ assert N >= 1 G = nx.Graph() namemap = dict() for ax in range(3): for i in xrange(N + 1): u = 2 * float(i) / N - 1.0 u = math.tan(u * math.pi * 0.25) for j in xrange(N + 1): v = 2 * float(j) / N - 1.0 v = math.tan(v * math.pi * 0.25) idx = [i, j] idx = idx[:ax] + [N] + idx[ax:] idx = tuple(idx) if idx in namemap: continue else: namemap[idx] = idx x = [u, v] x = x[:ax] + [1.0] + x[ax:] x = vectorops.unit(x) G.add_node(idx, params=x) idx = [i, j] idx = idx[:ax] + [0] + idx[ax:] idx = tuple(idx) if idx in namemap: continue else: namemap[idx] = idx x = [u, v] x = x[:ax] + [-1.0] + x[ax:] x = vectorops.unit(x) G.add_node(idx, params=x) for ax in range(3): for i in xrange(N + 1): for j in xrange(N + 1): baseidx = [i, j] idx = baseidx[:ax] + [N] + baseidx[ax:] idx = tuple(idx) for n in range(2): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: continue nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) G.add_edge(namemap[idx], namemap[nidx]) idx = baseidx[:ax] + [0] + baseidx[ax:] idx = tuple(idx) for n in range(2): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: continue nidx = nidx[:ax] + [0] + nidx[ax:] nidx = tuple(nidx) G.add_edge(namemap[idx], namemap[nidx]) return G
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 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
def run_poking_line_probe(config,tableHeight,probeLength,forceLimit,dt,moveStep,shortServoTime,longServoTime, IKErrorTolerence,maxDev,EEZLimit,probe_transform,point_probe_to_local,world,res, robot,link,CONTROLLER,collider,intermediateConfig): """ this is the main function of poking object. - line probe """ # reconstruct probepcd.txt if input('[*]Reconstruct probe pcd?') == 1: theta_list_num = input('---need theta list number: ') reconstruct_pcd(config.exp_path+'exp_'+str(config.exp_number)+'/probePcd.txt', config.exp_path+'exp_'+str(config.exp_number)+'/probePcd_theta.txt', theta_list_num) print('---New probe list done') # Read In the pcd points, normals, theta_list, theta, pti = load_pcd(config.exp_path+'exp_'+str(config.exp_number)+'/probePcd_theta.txt', pcdtype='xyzrgbntheta') # control interface if CONTROLLER == 'physical': robotControlApi = UR5WithGripperController(host=config.robot_host,gripper=False) robotControlApi.start() time.sleep(2) print '---------------------robot started -----------------------------' constantVServo(robotControlApi,4,intermediateConfig,dt)#controller format # set in simul model robot.setConfig(controller_2_klampt(robot,intermediateConfig)) print '---------------------at home configuration -----------------------------' if CONTROLLER == 'debugging': differences=[] print('[*]Debug: Poking process start') i = 0 # use this to catch points pti_ = pti[i] while(i < len(points)): robotCurrentConfig=intermediateConfig goalPosition=deepcopy(points[i]) approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0)) #get unit vector in the direction '- normals' _pti = pti_ if pti[i] == _pti: print('point %d, pos: %s, normals: %s, theta: %s, -> %f'%(i,points[i],normals[i],theta_list[i],theta[i])) ## perform IK local_NY_UnitV = vectorops.unit(back_2_line(approachVector,theta_list[i])) # the probe's line direction pt1=goalPosition pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) # use 1m in normals direction. pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,use_collision_detect=False,use_ik_detect=True) differences.append(difference) print('difference: %f'%difference) ### now start colecting data.. travel = 0.0 stepVector = vectorops.mul(approachVector,moveStep) while travel<0.0001: robotCurrentConfig=klampt_2_controller(robot.getConfig()) pt1=vectorops.add(pt1,stepVector) pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,use_collision_detect=False) travel = travel + moveStep ### move the probe away, note: a bit different to physical mode robotCurrentConfig=klampt_2_controller(robot.getConfig()) pt1=vectorops.add(points[i],vectorops.mul(approachVector,-0.05)) ## move the probe 5 cm from the object surface pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,use_collision_detect=False) ### move back to intermediate config robot.setConfig(controller_2_klampt(robot,intermediateConfig)) i = i + 1 # important else: pti_ = pti[i] print('[*]Debug: Poking process done, with max difference:%f'%max(differences)) vis.show() while vis.shown(): time.sleep(1.0) elif CONTROLLER == 'physical': exe_number = input('There are %d poking point, go?'%len(points)) start_i = 0 #72,93,94,97,99,100,101,108,116,125,128,147,148,150,151,152,189,194~197,207~210 !40,37,67,68 -> 111 112 113 120 121 122 201 206 end_i = 1 #len(points) i = start_i # use this to catch points, set manully! # TODO: pti_ = pti[i] probe_list = random.sample(range(282),282) #18,15 finish_list= range(16)+range(17,21)+range(25,44)+range(45,51)+range(52,57)+[58,59,60,63,67,68,71,73,75,76,78]+range(81,96)\ +[97,99,101,103,104,108,110,112,114,115,117,118,120,124,125,128,129,130]+range(132,138)+[141,142,144,147,149,150,151]+range(153,156)\ +[159,160,161,167,168,170,172,175,176,177,178]+range(180,186)\ +[189,192,195,196,199,200,201,203]+range(204,210)+range(211,217)+[219]+range(221,229)+range(230,236)+range(237,241)\ +range(244,250)+[251]+range(254,261)+[262]+range(264,276)+range(277,282) probe_list = [x for x in probe_list if x not in finish_list] +[227] probe_list = [95] for i in probe_list: print('point %d, pos: %s, normals: %s, theta: %s, -> %f'%(i,points[i],normals[i],theta_list[i],theta[i])) robotCurrentConfig=robotControlApi.getConfig() robot.setConfig(controller_2_klampt(robot,robotCurrentConfig)) # calculate start position goalPosition=deepcopy(points[i]) approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0)) # init record file forceData=open(config.exp_path+'exp_'+str(config.exp_number)+'/line/force_'+str(i)+'.txt','w') torqueData=open(config.exp_path+'exp_'+str(config.exp_number)+'/line/torque_'+str(i)+'.txt','w') travel = -0.025 ## perform IK local_NY_UnitV = vectorops.unit(back_2_line(approachVector,theta_list[i])) # the probe's line direction #### Make sure no contact, backup 0.01m pt1=vectorops.add(goalPosition,vectorops.mul(approachVector,travel)) pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) # use 1m in normals direction. pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,shortServoTime,dt) #TODO: time.sleep(0.2) # Zero the sensor before straight line push, Note that the force is recorded in the global frame.. counter = 0.0 totalF = [0,0,0] totalTorque = [0,0,0] startTime=time.time() while (time.time()-startTime) < 1: # use 1s to cal the Force totalF = vectorops.add(totalF,robotControlApi.getWrench()[0:3]) totalTorque = vectorops.add(totalTorque,robotControlApi.getWrench()[3:6]) counter = counter + 1.0 time.sleep(dt) forceBias = vectorops.mul(totalF,1.0/float(counter)) # when probe no touch the obj, F_avr = sum(F)/n torqueBias = vectorops.mul(totalTorque,1.0/float(counter)) ### now start collecting data.. wrench = robotControlApi.getWrench() Force = vectorops.sub(wrench[0:3],forceBias) Torque = vectorops.sub(wrench[3:6],torqueBias) Force_normal = math.fabs(vectorops.dot(Force,approachVector)) #|F||n|cos(theta) = F dot n, set it >= 0 local_Z_UnitV = vectorops.cross(normals[i],local_NY_UnitV) Torque_normal = vectorops.dot(Torque,local_Z_UnitV) #TODO: forceHistory = [Force] force_normalHistory = [Force_normal] torqueHistory = [Torque] torque_normalHistory = [Torque_normal] displacementHistory = [travel] stepVector = vectorops.mul(approachVector,moveStep) while Force_normal < forceLimit: robotCurrentConfig=robotControlApi.getConfig() robot.setConfig(controller_2_klampt(robot,robotCurrentConfig)) pt1=vectorops.add(pt1,stepVector) pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime,dt,use_const=False) time.sleep(dt) Force = vectorops.sub(robotControlApi.getWrench()[0:3],forceBias) Torque = vectorops.sub(robotControlApi.getWrench()[3:6],torqueBias) Force_normal = math.fabs(vectorops.dot(Force,approachVector)) local_Z_UnitV = vectorops.cross(normals[i],local_NY_UnitV) Torque_normal = vectorops.dot(Torque,local_Z_UnitV) travel = travel + moveStep forceHistory.append([Force[0],Force[1],Force[2]]) force_normalHistory.append(Force_normal) torqueHistory.append([Torque[0],Torque[1],Torque[2]]) torque_normalHistory.append(Torque_normal) displacementHistory.append(travel) #record all the data in 2 files, one N*2 containts all the force data collected at various locations, another #file specifies the number of datapoints at each detected point for (f,fn,d) in zip(forceHistory,force_normalHistory,displacementHistory): forceData.write(str(f[0])+' '+str(f[1])+' '+str(f[2])+' '+str(fn)+' '+str(d)+'\n') for (t,tn,d) in zip(torqueHistory,torque_normalHistory,displacementHistory): torqueData.write(str(t[0])+' '+str(t[1])+' '+str(t[2])+' '+str(tn)+' '+str(d)+'\n') ### move the probe away, sometimes z up 5cm is better than normal direction up 5cm... pt1=vectorops.add(pt1,[0,0,0.05]) ## move the probe 10 cm up-z-axis, find another point pt2=vectorops.add(pt2,[0,0,0.05]) pt3=vectorops.add(pt3,[0,0,0.05]) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,shortServoTime-1,dt) constantVServo(robotControlApi,longServoTime,intermediateConfig,dt)#TODO: # close record file for point i forceData.close() torqueData.close() print'----------------------- pt '+str(i)+' completed -------------------------------' ''' while(i < end_i): robotCurrentConfig=robotControlApi.getConfig() robot.setConfig(controller_2_klampt(robot,robotCurrentConfig)) # calculate start position goalPosition=deepcopy(points[i]) approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0)) # init record file forceData=open(config.exp_path+'exp_'+str(config.exp_number)+'/line/force_'+str(i)+'.txt','w') torqueData=open(config.exp_path+'exp_'+str(config.exp_number)+'/line/torque_'+str(i)+'.txt','w') _pti = pti_ if pti[i] == _pti: print('point %d, pos: %s, normals: %s, theta: %s, -> %f'%(i,points[i],normals[i],theta_list[i],theta[i])) travel = -0.015 ## perform IK local_NY_UnitV = vectorops.unit(back_2_line(approachVector,theta_list[i])) # the probe's line direction #### Make sure no contact, backup 0.01m pt1=vectorops.add(goalPosition,vectorops.mul(approachVector,travel)) pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) # use 1m in normals direction. pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime-1,dt) #TODO: time.sleep(0.2) # Zero the sensor before straight line push, Note that the force is recorded in the global frame.. counter = 0.0 totalF = [0,0,0] totalTorque = [0,0,0] startTime=time.time() while (time.time()-startTime) < 1: # use 1s to cal the Force totalF = vectorops.add(totalF,robotControlApi.getWrench()[0:3]) totalTorque = vectorops.add(totalTorque,robotControlApi.getWrench()[3:6]) counter = counter + 1.0 time.sleep(dt) forceBias = vectorops.mul(totalF,1.0/float(counter)) # when probe no touch the obj, F_avr = sum(F)/n torqueBias = vectorops.mul(totalTorque,1.0/float(counter)) ### now start collecting data.. wrench = robotControlApi.getWrench() Force = vectorops.sub(wrench[0:3],forceBias) Torque = vectorops.sub(wrench[3:6],torqueBias) Force_normal = math.fabs(vectorops.dot(Force,approachVector)) #|F||n|cos(theta) = F dot n, set it >= 0 local_Z_UnitV = vectorops.cross(normals[i],local_NY_UnitV) Torque_normal = vectorops.dot(Torque,local_Z_UnitV) #TODO: forceHistory = [Force] force_normalHistory = [Force_normal] torqueHistory = [Torque] torque_normalHistory = [Torque_normal] displacementHistory = [travel] stepVector = vectorops.mul(approachVector,moveStep) while Force_normal < forceLimit: robotCurrentConfig=robotControlApi.getConfig() robot.setConfig(controller_2_klampt(robot,robotCurrentConfig)) pt1=vectorops.add(pt1,stepVector) pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime,dt,use_const=False) time.sleep(dt) Force = vectorops.sub(robotControlApi.getWrench()[0:3],forceBias) Torque = vectorops.sub(robotControlApi.getWrench()[3:6],torqueBias) Force_normal = math.fabs(vectorops.dot(Force,approachVector)) local_Z_UnitV = vectorops.cross(normals[i],local_NY_UnitV) Torque_normal = vectorops.dot(Torque,local_Z_UnitV) travel = travel + moveStep forceHistory.append([Force[0],Force[1],Force[2]]) force_normalHistory.append(Force_normal) torqueHistory.append([Torque[0],Torque[1],Torque[2]]) torque_normalHistory.append(Torque_normal) displacementHistory.append(travel) #record all the data in 2 files, one N*2 containts all the force data collected at various locations, another #file specifies the number of datapoints at each detected point for (f,fn,d) in zip(forceHistory,force_normalHistory,displacementHistory): forceData.write(str(f[0])+' '+str(f[1])+' '+str(f[2])+' '+str(fn)+' '+str(d)+'\n') for (t,tn,d) in zip(torqueHistory,torque_normalHistory,displacementHistory): torqueData.write(str(t[0])+' '+str(t[1])+' '+str(t[2])+' '+str(tn)+' '+str(d)+'\n') ### move the probe away, sometimes z up 5cm is better than normal direction up 5cm... pt1=vectorops.add(pt1,[0,0,0.05]) ## move the probe 10 cm up-z-axis, find another point pt2=vectorops.add(pt2,[0,0,0.05]) pt3=vectorops.add(pt3,[0,0,0.05]) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,shortServoTime-0.5,dt) #constantVServo(robotControlApi,longServoTime,intermediateConfig,dt)#TODO: i = i + 1 # close record file for point i forceData.close() torqueData.close() print'----------------------- pt '+str(i)+' completed -------------------------------' else: # up 10cm is faster but not good. # since the points are close, no need to go back home constantVServo(robotControlApi,longServoTime,intermediateConfig,dt) pti_ = pti[i] ''' #### move back to intermediate config constantVServo(robotControlApi,shortServoTime,intermediateConfig,dt) # finish all points robotControlApi.stop()
def cal_curvature(pcdSegmented): NofPts = len(pcdSegmented.points) NofNN = 30 searchR = 0.01 #the points are 2mm apart... pcdTree = KDTreeFlann(pcdSegmented) curvatures = [] print 'calculating curvatures......' for i in range(len(pcdSegmented.points)): #for i in [0]: [k, idx, _] = pcdTree.search_knn_vector_3d(pcdSegmented.points[i], NofNN + 1) #[k,idx,_] = pcdTree.search_radius_vector_3d(pcdSegmented.points[i],searchR) #k is the total Number, while idx is the list of indices #for debugging #for ii in idx: # pcdSegmente.colors[ii] = [0,1,0] #draw_geometries([pcdSegmented]) #if k < 3.5: # [k,idx,_] = pcdTree.search_knn_vector_3d(pcdSegmented.points[i],NofNN+1) nearbyPts = np.asarray(pcdSegmented.points)[idx[1:], :] nearbyPtsNormal = np.asarray(pcdSegmented.normals)[idx[1:], :] normalCurvatures = [] ## Define the local coordinates N = vectorops.unit(pcdSegmented.normals[i]) # also the "z" Y = vectorops.cross(N, [1, 0, 0]) # pq = vectorops.(q,p) X = vectorops.cross(Y, N) MMatrix = [] for j in range(k - 1): #estimate the point normal, in Local corrdinate qGlobal = vectorops.sub(nearbyPts[j], pcdSegmented.points[i]) qLocal = [ vectorops.dot(qGlobal, X), vectorops.dot(qGlobal, Y), vectorops.dot(qGlobal, N) ] MGlobal = nearbyPtsNormal[j] MLocal = [ vectorops.dot(MGlobal, X), vectorops.dot(MGlobal, Y), vectorops.dot(MGlobal, N) ] [x_i, y_i, z_i] = qLocal [n_xi, n_yi, n_zi] = MLocal #print x_i,n_xi,y_i,n_yi n_xy = (x_i * n_xi + y_i * n_yi) / math.sqrt(x_i * x_i + y_i * y_i) #print n_xy k_i_n = -(n_xy) / (math.sqrt(n_xy * n_xy + n_zi * n_zi) + math.sqrt(x_i * x_i + y_i * y_i)) normalCurvatures.append(k_i_n) ## calculate the M matrix pQ = [qLocal[0], qLocal[1], 0] angle = np.arccos( np.dot(X, pQ) / (np.linalg.norm(X) * np.linalg.norm(pQ))) row = [ math.cos(angle) * math.cos(angle), 2 * math.cos(angle) * math.sin(angle), math.sin(angle) * math.sin(angle) ] MMatrix.append(row) #perform least squres fitting tmp = np.dot(np.transpose(MMatrix), MMatrix) tmp2 = np.dot(np.linalg.inv(tmp), np.transpose(MMatrix)) x = tmp2.dot(normalCurvatures) eigenValues, v = np.linalg.eig([[x[0], x[1]], [x[1], x[2]]]) curvatures.append(vectorops.norm_L1(eigenValues)) #print vectorops.norm_L1(eigenValues) print 'progress', float(i) / float(NofPts) print "max and min of curvatures: ", np.max(curvatures), np.min(curvatures) return curvatures
def run_poking_point_probe(config,tableHeight,probeLength,forceLimit,dt,moveStep,shortServoTime,longServoTime, IKErrorTolerence,maxDev,EEZLimit,probe_transform,point_probe_to_local,world,res, robot,link,CONTROLLER,collider,intermediateConfig): """ this is the main function of poking object. - point probe """ # Read In the pcd points, normals = load_pcd(config.exp_path+'exp_'+str(config.exp_number)+'/probePcd.txt') # control interface if CONTROLLER == 'physical': robotControlApi = UR5WithGripperController(host=config.robot_host,gripper=False) robotControlApi.start() time.sleep(2) print '---------------------robot started -----------------------------' constantVServo(robotControlApi,4,intermediateConfig,dt)#controller format # in simulation ,set robot.setConfig(controller_2_klampt(robot,intermediateConfig)) print '---------------------at home configuration -----------------------------' if CONTROLLER == 'debugging': differences=[] print('[*]Debug: Poking process start!') for i in range(len(points)): print('point %d, pos: %s, normals: %s'%(i,points[i],normals[i])) goalPosition=deepcopy(points[i]) approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0)) #get unit vector in the direction '- normals' ## perform IK local_NY_UnitV=vectorops.unit(vectorops.cross([0,1,0],approachVector)) pt1=goalPosition pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) # use 1m in normals direction. pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],maxDev, IKErrorTolerence,EEZLimit,collider,use_collision_detect=True) differences.append(difference) print('difference: %f'%difference) ### now start colecting data.. travel = 0.0 stepVector = vectorops.mul(approachVector,moveStep) while travel<0.0001: #just try 0.1mm? pt1=vectorops.add(pt1,stepVector) pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],maxDev, IKErrorTolerence,EEZLimit,collider,use_const=False) travel = travel + moveStep ### move the probe away, note: a bit different to physical mode pt1=vectorops.add(points[i],vectorops.mul(approachVector,-0.05)) ## move the probe 5 cm from the object surface pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],maxDev, IKErrorTolerence,EEZLimit,collider) ### move back to intermediate config robot.setConfig(controller_2_klampt(robot,intermediateConfig)) print('[*]Debug: Poking process done, with max difference:%f'%max(differences)) vis.show() while vis.shown(): time.sleep(1.0) elif CONTROLLER == 'physical': input('There are %d poking point, go?'%len(points)) point_list = range(112,116) # !delete 85, 90 #point_list = random.sample(range(97),97) #point_list = [40,37,67,68] for i in point_list: print('point %d, pos: %s, normals: %s'%(i,points[i],normals[i])) travel = -0.01 #init record file forceData=open(config.exp_path+'exp_'+str(config.exp_number)+'/point/force_'+str(i)+'.txt','w') robotCurrentConfig=robotControlApi.getConfig() robot.setConfig(controller_2_klampt(robot,robotCurrentConfig)) #calculate start position goalPosition=deepcopy(points[i]) approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0)) #### Make sure no contact, backup 0.01m local_NY_UnitV=vectorops.unit(vectorops.cross([0,1,0],approachVector)) pt1=vectorops.add(goalPosition,vectorops.mul(approachVector,travel)) pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime-1,dt) #TODO: time.sleep(0.2) # Zero the sensor before straight line push, Note that the force is recorded in the global frame.. counter = 0.0 totalF = [0,0,0] startTime=time.time() while (time.time()-startTime) < 1: # use 1s to cal the Force totalF = vectorops.add(totalF,robotControlApi.getWrench()[0:3]) counter = counter + 1.0 time.sleep(dt) forceBias = vectorops.mul(totalF,1.0/float(counter)) # when probe no touch the obj, F_avr = sum(F)/n ### now start collecting data.. wrench = robotControlApi.getWrench() Force = vectorops.sub(wrench[0:3],forceBias) Force_normal = math.fabs(vectorops.dot(Force,approachVector)) #|F||n|cos(theta) = F dot n, set it >= 0 forceHistory = [Force] force_normalHistory = [Force_normal] displacementHistory = [travel] stepVector = vectorops.mul(approachVector,moveStep) while Force_normal < forceLimit: robotCurrentConfig=robotControlApi.getConfig() robot.setConfig(controller_2_klampt(robot,robotCurrentConfig)) pt1=vectorops.add(pt1,stepVector) pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime,dt,use_const=False) time.sleep(dt) Force = vectorops.sub(robotControlApi.getWrench()[0:3],forceBias) Force_normal = math.fabs(vectorops.dot(Force,approachVector)) travel = travel + moveStep forceHistory.append([Force[0],Force[1],Force[2]]) force_normalHistory.append(Force_normal) displacementHistory.append(travel) #record all the data in 2 files, one N*2 containts all the force data collected at various locations, another #file specifies the number of datapoints at each detected point for (f,fn,d) in zip(forceHistory,force_normalHistory,displacementHistory): forceData.write(str(f[0])+' '+str(f[1])+' '+str(f[2])+' '+str(fn)+' '+str(d)+'\n') forceData.close() ### move the probe away robotCurrentConfig=robotControlApi.getConfig() robot.setConfig(controller_2_klampt(robot,robotCurrentConfig)) pt1=vectorops.add(points[i],vectorops.mul(approachVector,-0.10)) ## move the probe 8 cm from the object surface pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,shortServoTime,dt) #constantVServo(robotControlApi,longServoTime,intermediateConfig,dt) print'----------------------- pt '+str(i)+' completed -------------------------------' #### move back to intermediate config constantVServo(robotControlApi,shortServoTime,intermediateConfig,dt) robotControlApi.stop()
def run_poking_ellipse_probe(config,tableHeight,probeLength,forceLimit,dt,moveStep,shortServoTime,longServoTime, IKErrorTolerence,maxDev,EEZLimit,probe_transform,point_probe_to_local,world,res, robot,link,CONTROLLER,collider,intermediateConfig): """ this is the main function of poking object. - point probe """ ########################## Read In the pcd ###################################### points, normals = load_pcd(config.exp_path+'exp_'+str(config.exp_number)+'/probePcd.txt') # control interface if CONTROLLER == 'physical': robotControlApi = UR5WithGripperController(host=config.robot_host,gripper=False) robotControlApi.start() time.sleep(2) print '---------------------robot started -----------------------------' ## Record some home configuration intermediateConfig = config.intermediateConfig intermediateConfig = intermediateConfig if CONTROLLER == "physical": constantVServo(robotControlApi,4,intermediateConfig,dt)#controller format robot.setConfig(controller_2_klampt(robot,intermediateConfig)) print '---------------------at home configuration -----------------------------' if CONTROLLER == 'debugging': differences=[] print('[*]Debug: Poking process start!') for i in range(len(points)): print('point %d, pos: %s, normals: %s'%(i,points[i],normals[i])) #robotCurrentConfig=intermediateConfig # TODO: compare to the intermediateConfig, I comment it goalPosition=deepcopy(points[i]) approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0)) #get unit vector in the direction '- normals' ## perform IK local_NY_UnitV=vectorops.unit(vectorops.cross([0,1,0],approachVector)) pt1=goalPosition pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) # use 1m in normals direction. pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],maxDev, IKErrorTolerence,EEZLimit,collider,use_collision_detect=True,use_ik_detect=True) differences.append(difference) print('difference: %f'%difference) ### now start colecting data.. travel = 0.0 stepVector = vectorops.mul(approachVector,moveStep) while travel<0.0001: #just try 0.1mm? pt1=vectorops.add(pt1,stepVector) pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],maxDev, IKErrorTolerence,EEZLimit,collider,use_const=False) travel = travel + moveStep ### move the probe away, note: a bit different to physical mode pt1=vectorops.add(points[i],vectorops.mul(approachVector,-0.05)) ## move the probe 5 cm from the object surface pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],maxDev, IKErrorTolerence,EEZLimit,collider) ### move back to intermediate config robot.setConfig(controller_2_klampt(robot,intermediateConfig)) print('[*]Debug: Poking process done, with max difference:%f'%max(differences)) vis.show() while vis.shown(): time.sleep(1.0) elif CONTROLLER == 'physical': ######################################## Ready to Take Measurements ################################################ input('[!]Warning: There are %d poking point, Robot act!:'%len(points)) point_list = range(65,120) #64 #point_list = random.sample(range(0,94),94) #finish_list = [0,1,4,9,10,11,12,13,14,15,17,18,19,20,25,26,28,30,33,34,35,37,42,43,44,46,47,50,51,53,54,57,58,59,64,69,72,73,74,75,76,77,78,79,81,83,86,95] #point_list = [x for x in point_list if x not in finish_list] point_list = [64] for i in point_list: print('point %d, pos: %s, normals: %s'%(i,points[i],normals[i])) #init record file forceData=open(config.exp_path+'exp_'+str(config.exp_number)+'/ellipse/force_'+str(i)+'.txt','w') torqueData=open(config.exp_path+'exp_'+str(config.exp_number)+'/ellipse/torque_'+str(i)+'.txt','w') #init the backforward distance travel = -0.018 robotCurrentConfig=robotControlApi.getConfig() robot.setConfig(controller_2_klampt(robot,robotCurrentConfig)) #calculate start position goalPosition=deepcopy(points[i]) approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0)) #### Make sure no contact, backup 0.01m local_NY_UnitV=vectorops.unit(vectorops.cross([0,1,0],approachVector)) pt1=vectorops.add(goalPosition,vectorops.mul(approachVector,travel)) pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime-1,dt, use_ik_detect=False,use_collision_detect=False) time.sleep(0.2) ## Zero the sensor before straight line push # # Note that the force is recorded in the global frame.. # And the global frame has x and y axis flipped w.r.t the URDF.... counter = 0.0 totalF = [0,0,0] totalTorque = [0,0,0] startTime=time.time() while (time.time()-startTime) < 1: # use 1s to cal the Force totalF = vectorops.add(totalF,robotControlApi.getWrench()[0:3]) totalTorque = vectorops.add(totalTorque,robotControlApi.getWrench()[3:6]) counter = counter + 1.0 time.sleep(dt) forceBias = vectorops.mul(totalF,1.0/float(counter)) # when probe no touch the obj, F_avr = sum(F)/n torqueBias = vectorops.mul(totalTorque,1.0/float(counter)) ### now start collecting data.. # Force direction x, y inverse, refer to correct force.py wrench = robotControlApi.getWrench() Force = fix_direction(vectorops.sub(wrench[0:3],forceBias)) Force_normal = math.fabs(vectorops.dot(Force,approachVector)) #|F||n|cos(theta) = F dot n, set it >= 0 Torque = vectorops.sub(wrench[3:6],torqueBias) Torque = fix_direction(Torque) forceHistory = [Force] torqueHistory = [Torque] force_normalHistory = [Force_normal] displacementHistory = [travel] stepVector = vectorops.mul(approachVector,moveStep) while Force_normal < forceLimit: robotCurrentConfig=robotControlApi.getConfig() robot.setConfig(controller_2_klampt(robot,robotCurrentConfig)) pt1=vectorops.add(pt1,stepVector) pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime,dt, use_const=False,use_ik_detect=False) time.sleep(dt) Force = fix_direction(vectorops.sub(robotControlApi.getWrench()[0:3],forceBias)) Force_normal = math.fabs(vectorops.dot(Force,approachVector)) Torque = vectorops.sub(robotControlApi.getWrench()[3:6],torqueBias) travel = travel + moveStep forceHistory.append([Force[0],Force[1],Force[2]]) torqueHistory.append([Torque[0],Torque[1],Torque[2]]) force_normalHistory.append(Force_normal) displacementHistory.append(travel) #record all the data in 2 files, one N*2 containts all the force data collected at various locations, another #file specifies the number of datapoints at each detected point for (f,fn,d) in zip(forceHistory,force_normalHistory,displacementHistory): forceData.write(str(f[0])+' '+str(f[1])+' '+str(f[2])+' '+str(fn)+' '+str(d)+'\n') for (t,d) in zip(torqueHistory,displacementHistory): torqueData.write(str(t[0])+' '+str(t[1])+' '+str(t[2])+' '+str(d)+'\n') forceData.close() torqueData.close() ### move the probe away robotCurrentConfig=robotControlApi.getConfig() robot.setConfig(controller_2_klampt(robot,robotCurrentConfig)) pt1=vectorops.add(points[i],vectorops.mul(approachVector,-0.10)) ## move the probe 5 cm from the object surface pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) pt3=vectorops.add(pt1,local_NY_UnitV) [robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3], maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,shortServoTime,dt, use_ik_detect=False) #constantVServo(robotControlApi,longServoTime,intermediateConfig,dt) print'----------------------- pt '+str(i)+' completed -------------------------------' #### move back to intermediate config constantVServo(robotControlApi,shortServoTime,intermediateConfig,dt) robotControlApi.stop()
def load_data(point_path, force_path, probe_type='point', datatype='1'): points=[] colors=[] normals=[] curvatures=[] theta = [] dataFile=open(point_path,'r') for line in dataFile: line=line.rstrip() l=[num for num in line.split(' ')] l2=[float(num) for num in l] points.append(l2[0:3]) colors.append(l2[3:6]) normals.append(l2[6:9]) curvatures.append(l2[9]) #normalize tmp_theta = vectorops.unit(l2[10:13]) theta.append(tmp_theta) dataFile.close() # normalize, note colors and normals is 0~1 points = np.array(points) colors = np.array(colors) normals = np.array(normals) curvatures = np.array(curvatures) max_range = max([ (np.max(points[:,0])-np.min(points[:,0])) , (np.max(points[:,1])-np.min(points[:,1])) , (np.max(points[:,2])-np.min(points[:,2])) ]) for i in range(3): points[:,i] = (points[:,i]-np.min(points[:,i]))/max_range num_point = len(points) print('[*]load %d points, and normalized'%num_point) ''' X = np.array([[]]) Y = np.array([[]]) insert_i = 0 ''' X=[] Y=[] for i in range(num_point): force_path = './'+probe_type+'/force_'+str(i)+'.txt' force=[] force_normal=[] displacement=[] dataFile=open(force_path,'r') for line in dataFile: line=line.rstrip() l=[num for num in line.split(' ')] l2=[float(num) for num in l] force.append(l2[0:3]) force_normal.append(l2[3]) displacement.append(l2[4]) dataFile.close() # clean #TODO: # final if probe_type == 'point': num_dis = len(displacement) #print('---load %d displacement'%num_dis) displacement = np.resize(np.array(displacement),(num_dis,1)) X_i = np.hstack((np.tile(points[i],(num_dis,1)), np.tile(normals[i],(num_dis,1)), np.tile(theta[i],(num_dis,1)) displacement)) Y_i = np.array(force_normal,ndmin=2).T ''' if insert_i == 0: X=X_i Y=Y_i else: X = np.vstack((X,X_i)) Y = np.vstack((Y,Y_i)) insert_i = insert_i + 1 ''' X.append(X_i) Y.append(Y_i) return X,Y
def predict_line(lineStarts,lineEnds,lineNormals,lineTorqueAxes,pcd,param,discretization,num_iter,queryDList,vis,world): vision_task_1 = False#True means we are doing the collision detection. o3d = False #create a pcd in open3D equilibriumPcd = open3d.geometry.PointCloud() xyz = [] rgb = [] normals = [] for ele in pcd: xyz.append(ele[0:3]) rgb.append(ele[3:6]) normals.append(ele[6:9]) equilibriumPcd.points = open3d.utility.Vector3dVector(np.asarray(xyz,dtype=np.float32)) equilibriumPcd.colors = open3d.utility.Vector3dVector(np.asarray(rgb,dtype=np.float32)) equilibriumPcd.normals = open3d.utility.Vector3dVector(np.asarray(normals,dtype=np.float32)) # equilibriumPcd,ind=statistical_outlier_removal(equilibriumPcd,nb_neighbors=20,std_ratio=0.75) if o3d: vis3 = open3d.visualization.Visualizer() vis3.create_window() open3dPcd = open3d.geometry.PointCloud() vis3.add_geometry(open3dPcd) probe = world.rigidObject(0) klampt_pcd_object = world.rigidObject(1) #this is a rigid object klampt_pcd = klampt_pcd_object.geometry().getPointCloud() #this is now a PointCloud() N_pts = klampt_pcd.numPoints() dt = 0.1 for pointNumber in num_iter: ####Preprocess the pcd #### lineStart0 = lineStarts[pointNumber] lineEnd0 =lineEnds[pointNumber] lineTorqueAxis = lineTorqueAxes[pointNumber] N = 1 + round(vo.norm(vo.sub(lineStart0,lineEnd0))/discretization) s = np.linspace(0,1,N) lineNormal = lineNormals[pointNumber] localXinW = vo.sub(lineEnd0,lineStart0)/np.linalg.norm(vo.sub(lineEnd0,lineStart0)) localYinW = (lineTorqueAxis)/np.linalg.norm(lineTorqueAxis) lineStartinLocal = [0,0] lineEndinLocal = [np.dot(vo.sub(lineEnd0,lineStart0),localXinW), np.dot(vo.sub(lineEnd0,lineStart0),localYinW)] #################first project the pcd to the plane of the line############## #The start point is the origin of the plane... projectedPcd = [] #Each point is R^10 projectedPcdLocal = [] #localXY coordinate #the projected Pcd is in local frame.... for i in range(len(pcd)): p = pcd[i][0:3] projectedPt = vo.sub(p,vo.mul(lineNormal,vo.dot(vo.sub(p,lineStart0),lineNormal))) ##world origin projectedPt2D = vo.sub(projectedPt,lineStart0)#world origin projectedPt2DinLocal = [vo.dot(projectedPt2D,localXinW),vo.dot(projectedPt2D,localYinW)] #%make sure point is in the "box" defined by the line if ((projectedPt2DinLocal[0]<0.051) and (projectedPt2DinLocal[0] > -0.001) and (projectedPt2DinLocal[1]<0.001) and (projectedPt2DinLocal[1]>-0.001)): projectedPcdLocal.append(projectedPt2DinLocal) projectedPcd.append(pcd[i]) #Create a KDTree for searching projectedPcdTree = spatial.KDTree(projectedPcdLocal) ###############Find the corresponding point on the surface to the line############ surfacePtsAll = []# %These are the surface pts that will be displaced. #%part of the probe not in contact with the object... #average 3 neighbors NofN = 3 #rigidPointsFinal = [] ##we have a potential bug here for not using rigidPointsFinal.... for i in range(int(N)): tmp =vo.mul(vo.sub(lineEnd0,lineStart0),s[i])#%the point on the line, projected linePt = [vo.dot(tmp,localXinW),vo.dot(tmp,localYinW)] d,Idx = projectedPcdTree.query(linePt[0:2],k=NofN) #We might end up having duplicated pts... #We should make sure that the discretization is not too fine.. #or should average a few neighbors if d[0] < 0.002: surfacePt = [0]*10 for j in range(NofN): surfacePt = vo.add(surfacePt,projectedPcd[Idx[j]][0:10]) surfacePt = vo.div(surfacePt,NofN) surfacePtsAll.append(surfacePt) #position in the global frame.. #rigidPointsFinal.append(linePts) N = len(surfacePtsAll) ##### Preprocesss done if not o3d: vis.show() time.sleep(15.0) ############# Go through a list of displacements totalFinNList = [] #for queryD = -0.003:0.001:0.014 for queryD in queryDList: lineStart = vo.sub(lineStart0,vo.mul(lineNormal,queryD)) lineEnd = vo.sub(lineEnd0,vo.mul(lineNormal,queryD)) lineCenter = vo.div(vo.add(lineStart,lineEnd),2) torqueCenter = vo.add(lineCenter,vo.mul(lineNormal,0.09)) if vision_task_1: vis.lock() print(queryD) x_axis = vo.mul(vo.unit(vo.sub(lineEnd,lineStart)),-1.0) y_axis = [lineTorqueAxis[0],lineTorqueAxis[1],lineTorqueAxis[2]] z_axis = vo.cross(x_axis,y_axis) z_axis = [z_axis[0],z_axis[1],z_axis[2]] R = x_axis+y_axis+z_axis t = vo.add(lineCenter,vo.mul(lineNormal,0.002)) print(R,t) probe.setTransform(R,t) vis.unlock() time.sleep(dt) else: if not o3d: vis.lock() print(queryD) x_axis = vo.mul(vo.unit(vo.sub(lineEnd,lineStart)),-1.0) y_axis = [lineTorqueAxis[0],lineTorqueAxis[1],lineTorqueAxis[2]] z_axis = vo.cross(x_axis,y_axis) z_axis = [z_axis[0],z_axis[1],z_axis[2]] R = x_axis+y_axis+z_axis t = vo.add(lineCenter,vo.mul(lineNormal,0.003)) probe.setTransform(R,t) ####calculate the nominal displacements surfacePts = [] #These are the surface pts that will be displaced... rigidPtsInContact = [] nominalD = [] #Column Vector.. for i in range(int(N)): ##we have a potential bug here for keep interating trhough N, since about, if d[0] < 0.002, the points is not added... #weird no bug occured... linePt = vo.add(vo.mul(vo.sub(lineEnd,lineStart),s[i]),lineStart) surfacePt = surfacePtsAll[i][0:3] normal = surfacePtsAll[i][6:9] nominalDisp = -vo.dot(vo.sub(linePt,surfacePt),normal) if nominalDisp > 0: surfacePts.append(surfacePtsAll[i][0:10])#position in the global frame.. rigidPtsInContact.append(linePt) nominalD.append(nominalDisp) originalNominalD = deepcopy(nominalD) NofSurfacePts = len(surfacePts) if NofSurfacePts > 0: negativeDisp = True while negativeDisp: NofSurfacePts = len(surfacePts) K = np.zeros((NofSurfacePts,NofSurfacePts)) for i in range(NofSurfacePts): for j in range(NofSurfacePts): K[i][j] = invKernel(surfacePts[i][0:3],surfacePts[j][0:3],param) #print K #startTime = time.time() actualD = np.dot(np.linalg.inv(K),nominalD) #print('Time spent inverting matrix',time.time()- startTime) #print nominalD,actualD negativeIndex = actualD < 0 if np.sum(negativeIndex) > 0: actualD = actualD.tolist() surfacePts = [surfacePts[i] for i in range(len(surfacePts)) if actualD[i]>=0] nominalD = [nominalD[i] for i in range(len(nominalD)) if actualD[i]>=0] rigidPtsInContact = [rigidPtsInContact[i] for i in range(len(rigidPtsInContact)) if actualD[i]>=0] else: negativeDisp = False #hsv's hue: 0.25 = 0.0002 displacement 1 == 10 displacement #generate the entire displacement field #surfacePts are the equlibrium pts that provide entireDisplacedPcd = [] colorThreshold = 0.0005 colorUpperBound = 0.012 forceColorUpperBound = 0.004 colorRange = colorUpperBound - colorThreshold greyColor = 0.38 xyz = [] rgb = [] xyzForce = [] rgbForce = [] maxDisp = 0 for pt,ptNormal in zip(equilibriumPcd.points,equilibriumPcd.normals): shapeVector = [] for pt2 in surfacePts: shapeVector.append(invKernel(pt[0:3],pt2[0:3],param)) nominalDisplacement = vo.dot(shapeVector,actualD) if nominalDisplacement > maxDisp: maxDisp = nominalDisplacement color = colorsys.hsv_to_rgb(nominalDisplacement/colorRange*0.75+0.25,1,0.75) color = [color[0],color[1],color[2]] displacedDeformablePoint = vo.sub(pt[0:3],vo.mul(ptNormal,nominalDisplacement)) xyz.append(displacedDeformablePoint) rgb.append(color) counter = 0 for (p,c) in zip(xyz,equilibriumPcd.colors):#rgb): klampt_pcd.setProperty(counter,'r',c[0]) klampt_pcd.setProperty(counter,'g',c[1]) klampt_pcd.setProperty(counter,'b',c[2]) klampt_pcd.setPoint(counter,p) counter = counter + 1 klampt_pcd_object.geometry().setPointCloud(klampt_pcd) if o3d: open3dPcd.points = open3d.utility.Vector3dVector(np.asarray(xyz,dtype=np.float32)) open3dPcd.colors = open3d.utility.Vector3dVector(np.asarray(rgb,dtype=np.float32)) open3dPcd.estimate_normals(search_param = open3d.geometry.KDTreeSearchParamHybrid(radius = 0.1, max_nn = 30)) ###open3dPcd,ind=statistical_outlier_removal(open3dPcd,nb_neighbors=10,std_ratio=2) #open3d.visualization.draw_geometries([open3dPcd])#_box_bottom]) vis3.add_geometry(open3dPcd) vis3.poll_events() vis3.update_renderer() if not o3d: vis.unlock() time.sleep(dt) else: pass while vis.shown(): time.sleep(0.1) return
def so3_staggered_grid(N): """Returns a nx.Graph describing a staggered grid on SO3 with resolution N. The resulting graph has ~8N^3 nodes The node attribute 'params' gives the so3 element. """ import itertools assert N >= 1 G = nx.Graph() R0 = so3.from_quaternion(vectorops.unit((1, 1, 1, 1))) #R0 = so3.identity() namemap = dict() for ax in range(4): for i in xrange(N + 1): u = 2 * float(i) / N - 1.0 u = math.tan(u * math.pi * 0.25) u2 = 2 * float(i + 0.5) / N - 1.0 u2 = math.tan(u2 * math.pi * 0.25) for j in xrange(N + 1): v = 2 * float(j) / N - 1.0 v = math.tan(v * math.pi * 0.25) v2 = 2 * float(j + 0.5) / N - 1.0 v2 = math.tan(v2 * math.pi * 0.25) for k in xrange(N + 1): w = 2 * float(k) / N - 1.0 w = math.tan(w * math.pi * 0.25) w2 = 2 * float(k + 0.5) / N - 1.0 w2 = math.tan(w2 * math.pi * 0.25) idx = [i, j, k] idx = idx[:ax] + [N] + idx[ax:] idx = tuple(idx) if idx in namemap: pass else: namemap[idx] = idx flipidx = tuple([N - x for x in idx]) namemap[flipidx] = idx q = [u, v, w] q = q[:ax] + [1.0] + q[ax:] q = vectorops.unit(q) R = so3.mul(so3.inv(R0), so3.from_quaternion(q)) G.add_node(idx, params=R) if i < N and j < N and k < N: #add staggered point sidx = [i + 0.5, j + 0.5, k + 0.5] sidx = sidx[:ax] + [N] + sidx[ax:] sidx = tuple(sidx) sfidx = tuple([N - x for x in sidx]) namemap[sidx] = sidx namemap[sfidx] = sidx q = [u2, v2, w2] q = q[:ax] + [1.0] + q[ax:] q = vectorops.unit(q) R = so3.mul(so3.inv(R0), so3.from_quaternion(q)) G.add_node(sidx, params=R) for ax in range(4): for i in xrange(N + 1): for j in xrange(N + 1): for k in xrange(N + 1): baseidx = [i, j, k] idx = baseidx[:ax] + [N] + baseidx[ax:] idx = tuple(idx) for n in range(3): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: continue nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) G.add_edge(namemap[idx], namemap[nidx]) #edges between staggered point and grid points baseidx = [i + 0.5, j + 0.5, k + 0.5] if any(v > N for v in baseidx): continue idx = baseidx[:ax] + [N] + baseidx[ax:] idx = tuple(idx) for ofs in itertools.product(*[(-0.5, 0.5)] * 3): nidx = vectorops.add(baseidx, ofs) nidx = [int(x) for x in nidx] nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) #print "Stagger-grid edge",idx,nidx G.add_edge(namemap[idx], namemap[nidx]) #edges between staggered points -- if it goes beyond face, #go to the adjacent face for n in range(3): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: #swap face nidx[n] = N nidx = nidx[:ax] + [N - 0.5] + nidx[ax:] nidx = tuple(nidx) #if not G.has_edge(namemap[idx],namemap[nidx]): # print "Stagger-stagger edge",idx,nidx,"swap face" else: nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) #if not G.has_edge(namemap[idx],namemap[nidx]): # print "Stagger-stagger edge",idx,nidx G.add_edge(namemap[idx], namemap[nidx]) return G
def loop_through_sensors( world=world, sensor=sensor, world_camera_viewpoints=world_camera_viewpoints, index=index, counters=counters): viewno = counters[0] variation = counters[1] total_count = counters[2] print("Generating data for sensor view", viewno, "variation", variation) sensor_xform = world_camera_viewpoints[viewno] #TODO: problem 1.B: perturb camera and change colors # Generate random axis, random angle, rotate about angle for orientation perturbation # Generate random axis, random dist, translate distance over axis for position perturbation rand_axis = np.random.rand(3) rand_axis = vectorops.unit(rand_axis) multiplier = np.random.choice([True, False], 1) rand_angle = (np.random.rand(1)[0] * 10 + 10) * (-1 if multiplier else 1) # print(rand_angle) R = so3.from_axis_angle((rand_axis, np.radians(rand_angle))) rand_axis = vectorops.unit(np.random.random(3)) rand_dist = np.random.random(1)[0] * .10 + .10 # print(rand_dist) t = vectorops.mul(rand_axis, rand_dist) sensor_xform = se3.mul((R, t), sensor_xform) sensing.set_sensor_xform(sensor, sensor_xform) rgb_filename = "image_dataset/color_%04d_var%04d.png" % ( total_count, variation) depth_filename = "image_dataset/depth_%04d_var%04d.png" % ( total_count, variation) grasp_filename = "image_dataset/grasp_%04d_var%04d.png" % ( total_count, variation) #Generate sensor images sensor.kinematicReset() sensor.kinematicSimulate(world, 0.01) print("Done with kinematic simulate") rgb, depth = sensing.camera_to_images(sensor) print("Saving to", rgb_filename, depth_filename) Image.fromarray(rgb).save(rgb_filename) depth_scale = 8000 depth_quantized = (depth * depth_scale).astype(np.uint32) Image.fromarray(depth_quantized).save(depth_filename) with open("image_dataset/metadata.csv", 'a') as f: line = "{},{},{},{},{},{},{}\n".format( index, viewno, loader.write(sensor_xform, 'RigidTransform'), os.path.basename(rgb_filename), os.path.basename(depth_filename), os.path.basename(grasp_filename), variation) f.write(line) #calculate grasp map and save it grasp_map = make_grasp_map(world, total_count) grasp_map_quantized = (grasp_map * 255).astype(np.uint8) channels = ['score', 'opening', 'axis_heading', 'axis_elevation'] for i, c in enumerate(channels): base, ext = os.path.splitext(grasp_filename) fn = base + '_' + c + ext Image.fromarray(grasp_map_quantized[:, :, i]).save(fn) #loop through variations and views counters[1] += 1 if counters[1] >= max_variations: counters[1] = 0 counters[0] += 1 if counters[0] >= len(world_camera_viewpoints): vis.show(False) counters[2] += 1
def sphere_staggered_grid(N): """Returns a nx.Graph describing a staggered grid on S2 with resolution N. The resulting graph has ~12N^2 nodes The node attribute 'params' gives the so3 element. """ import itertools assert N >= 1 G = nx.Graph() namemap = dict() for ax in range(3): for i in xrange(N + 1): u = 2 * float(i) / N - 1.0 u = math.tan(u * math.pi * 0.25) u2 = 2 * float(i + 0.5) / N - 1.0 u2 = math.tan(u2 * math.pi * 0.25) for j in xrange(N + 1): v = 2 * float(j) / N - 1.0 v = math.tan(v * math.pi * 0.25) v2 = 2 * float(j + 0.5) / N - 1.0 v2 = math.tan(v2 * math.pi * 0.25) idx = [i, j] idx = idx[:ax] + [N] + idx[ax:] idx = tuple(idx) if idx in namemap: pass else: namemap[idx] = idx x = [u, v] x = x[:ax] + [1.0] + x[ax:] x = vectorops.unit(x) G.add_node(idx, params=x) if i < N and j < N: #add staggered point sidx = [i + 0.5, j + 0.5] sidx = sidx[:ax] + [N] + sidx[ax:] sidx = tuple(sidx) namemap[sidx] = sidx x = [u2, v2] x = x[:ax] + [1.0] + x[ax:] x = vectorops.unit(x) G.add_node(sidx, params=x) idx = [i, j] idx = idx[:ax] + [0] + idx[ax:] idx = tuple(idx) if idx in namemap: pass else: namemap[idx] = idx x = [u, v] x = x[:ax] + [-1.0] + x[ax:] x = vectorops.unit(x) G.add_node(idx, params=x) if i < N and j < N: #add staggered point sidx = [i + 0.5, j + 0.5] sidx = sidx[:ax] + [0] + sidx[ax:] sidx = tuple(sidx) namemap[sidx] = sidx x = [u2, v2] x = x[:ax] + [-1.0] + x[ax:] x = vectorops.unit(x) G.add_node(sidx, params=x) for ax in range(3): for i in xrange(N + 1): for j in xrange(N + 1): baseidx = [i, j] idx = baseidx[:ax] + [N] + baseidx[ax:] idx = tuple(idx) for n in range(2): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: continue nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) G.add_edge(namemap[idx], namemap[nidx]) idx = baseidx[:ax] + [0] + baseidx[ax:] idx = tuple(idx) for n in range(2): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: continue nidx = nidx[:ax] + [0] + nidx[ax:] nidx = tuple(nidx) G.add_edge(namemap[idx], namemap[nidx]) #edges between staggered point and grid points baseidx = [i + 0.5, j + 0.5] if baseidx[0] > N or baseidx[1] > N: continue idx = baseidx[:ax] + [N] + baseidx[ax:] idx = tuple(idx) for ofs in itertools.product(*[(-0.5, 0.5)] * 2): nidx = vectorops.add(baseidx, ofs) nidx = [int(x) for x in nidx] nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) #print "Stagger-grid edge",idx,nidx G.add_edge(namemap[idx], namemap[nidx]) idx = baseidx[:ax] + [0] + baseidx[ax:] idx = tuple(idx) for ofs in itertools.product(*[(-0.5, 0.5)] * 2): nidx = vectorops.add(baseidx, ofs) nidx = [int(x) for x in nidx] nidx = nidx[:ax] + [0] + nidx[ax:] nidx = tuple(nidx) #print "Stagger-grid edge",idx,nidx G.add_edge(namemap[idx], namemap[nidx]) return G