def add_multiple_lights(properties,object,dist,numLight,gravity=[0,0,-9.81],tgt=None,color=[1.,1.,1.], \ spotlight=False,radius=15.,falloff=20.,tightness=10., \ area=0.,sample=9,adaptive=True,jitter=True): """This is a convenient function that calls add_light multiple times""" #normalize gravity g = op.mul(gravity, -1 / op.norm(gravity)) #compute frame gabs = [abs(gi) for gi in g] id = gabs.index(min(gabs)) t0 = [1. if i == id else 0. for i in range(3)] t1 = op.cross(t0, g) t1 = op.mul(t1, 1 / op.norm(t1)) t0 = op.cross(t1, g) #find highest direction bb = compute_bb(object) ctr = op.mul(op.add(bb[0], bb[1]), 0.5) distg = sum([abs((bb[1][i] - bb[0][i]) / 2 * g[i]) for i in range(3)]) #add each light for i in range(numLight): angle = math.pi * 2 * i / numLight d0 = op.mul(g, distg) d1 = op.mul(t0, math.sin(angle) * dist) d2 = op.mul(t1, math.cos(angle) * dist) add_light(properties, op.add(d0, op.add(d1, d2)), ctr, color, spotlight, radius, falloff, tightness, area, sample, adaptive, jitter)
def getCommandVelocity(self, q, dq, dt): """Gets the command velocity from the current state of the robot. """ eP = self.getSensedError(q) #vcmd = hP*eP + hD*eV + hI*eI vP = gen_mul(self.hP, eP) vcmd = vP #D term vcur = self.getSensedVelocity(q, dq, dt) eD = None if vcur != None: eD = vectorops.sub(vcur, self.dxdes) vD = gen_mul(self.hD, eD) vcmd = vectorops.add(vcmd, vD) #I term if self.eI != None: vI = gen_mul(self.hI, self.eI) vcmd = vectorops.add(vcmd, vI) #print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI #do acceleration limiting if vcur != None: dvcmd = vectorops.div(vectorops.sub(vcmd, vcur), dt) dvnorm = vectorops.norm(dvcmd) if dvnorm > self.dvcmdmax: vcmd = vectorops.madd(vcur, dvcmd, self.dvcmdmax / dvnorm * dt) print self.name, "acceleration limited by factor", self.dvcmdmax / dvnorm * dt, "to", vcmd #do velocity limiting vnorm = vectorops.norm(vcmd) if vnorm > self.vcmdmax: vcmd = vectorops.mul(vcmd, self.vcmdmax / vnorm) print self.name, "velocity limited by factor", self.vcmdmax / vnorm, "to", vcmd return vcmd
def getCommandVelocity(self, q, dq, dt): """Gets the command velocity from the current state of the robot. """ eP = self.getSensedError(q) #vcmd = hP*eP + hD*eV + hI*eI vP = gen_mul(self.hP,eP) vcmd = vP #D term vcur = self.getSensedVelocity(q,dq,dt) eD = None if vcur != None: eD = vectorops.sub(vcur, self.dxdes) vD = gen_mul(self.hD,eD) vcmd = vectorops.add(vcmd, vD) #I term if self.eI != None: vI = gen_mul(self.hI,self.eI) vcmd = vectorops.add(vcmd, vI) #print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI #do acceleration limiting if vcur != None: dvcmd = vectorops.div(vectorops.sub(vcmd,vcur),dt) dvnorm = vectorops.norm(dvcmd) if dvnorm > self.dvcmdmax: vcmd = vectorops.madd(vcur,dvcmd,self.dvcmdmax/dvnorm*dt) print self.name,"acceleration limited by factor",self.dvcmdmax/dvnorm*dt,"to",vcmd #do velocity limiting vnorm = vectorops.norm(vcmd) if vnorm > self.vcmdmax: vcmd = vectorops.mul(vcmd,self.vcmdmax/vnorm) print self.name,"velocity limited by factor",self.vcmdmax/vnorm,"to",vcmd return vcmd
def get_left_dir(self, zeroZ=False): dir = op.cross([0, 0, 1], self.get_camera_dir()) if zeroZ: dir = (dir[0], dir[1], 0) if op.norm(dir) > 1e-6: dir = op.mul(dir, 1 / op.norm(dir)) return dir
def get_camera_dir(self, zeroZ=False): cam = self.view.camera dir = op.sub(cam.tgt, self.get_camera_pos()) if zeroZ: dir = (dir[0], dir[1], 0) if op.norm(dir) > 1e-6: dir = op.mul(dir, 1 / op.norm(dir)) dir[1] *= -1 return dir
def solve_2R_inverse_kinematics(x,y,L1=1,L2=1): """For a 2R arm centered at the origin, solves for the joint angles (q1,q2) that places the end effector at (x,y). The result is a list of up to 2 solutions, e.g. [(q1,q2),(q1',q2')]. """ D = vectorops.norm((x,y)) thetades = math.atan2(y,x) if D == 0: raise ValueError("(x,y) at origin, infinite # of solutions") c2 = (D**2-L1**2-L2**2)/(2.0*L1*L2) q2s = [] if c2 < -1: print("solve_2R_inverse_kinematics: (x,y) inside inner circle") return [] elif c2 > 1: print("solve_2R_inverse_kinematics: (x,y) out of reach") return [] else: if c2 == 1: q2s = [math.acos(c2)] else: q2s = [math.acos(c2),-math.acos(c2)] res = [] for q2 in q2s: thetaactual = math.atan2(math.sin(q2),L1+L2*math.cos(q2)) q1 = thetades - thetaactual res.append((q1,q2)) return res
def distMetric(self, n1, n2): dist = self.env.n * vectorops.norm( vectorops.sub([n1.x, n1.y, n1.z], [n2.x, n2.y, n2.z])) dist += self.env.sumDist * math.fabs(n1.sc - n2.sc) dist += 1 * self.env.sumDist * math.fabs( n1.sc + n2.sc) / 2 * math.fabs(n1.tht - n2.tht) return dist
def printStatus(self,q): """Prints a status printout summarizing all tasks' errors.""" priorities = set() names = dict() errors = dict() totalerrors = dict() for t in self.taskList: if t.weight==0: continue priorities.add(t.level) s = t.name if len(s) > 8: s = s[0:8] err = t.getSensedError(q) names.setdefault(t.level,[]).append(s) errors.setdefault(t.level,[]).append("%.3f"%(vectorops.norm(err)),) werrsq = vectorops.normSquared(vectorops.mul(err,t.weight)) totalerrors[t.level] = totalerrors.get(t.level,0.0) + werrsq cols = 5 colwidth = 10 for p in priorities: print "Priority",p,"weighted error^2",totalerrors[p] pnames = names[p] perrs = errors[p] start = 0 while start < len(pnames): last = min(start+cols,len(pnames)) print " Name: ", for i in range(start,last): print pnames[i],' '*(colwidth-len(pnames[i])), print print " Error: ", for i in range(start,last): print perrs[i],' '*(colwidth-len(perrs[i])), print start=last
def draw_hull(hull,scale=0.01): """Draws a ConvexHull with 3D texture coordinates""" if len(hull.simplices)==0: print "Hull with no simplices?" return centroid = sum([hull.points[v] for v in hull.vertices],[0.0]*3) / len(hull.vertices) * scale vmin = [min([hull.points[v][i] for v in hull.vertices])*scale for i in range(3)] vmax = [max([hull.points[v][i] for v in hull.vertices])*scale for i in range(3)] uvwscale = [1.0/(b-a) if b != a else 1.0 for (a,b) in zip(vmin,vmax)] for simplex in hull.simplices: ia,ib,ic = simplex a = vectorops.mul(hull.points[ia].tolist(),scale) b = vectorops.mul(hull.points[ib].tolist(),scale) c = vectorops.mul(hull.points[ic].tolist(),scale) n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a)) if vectorops.dot(n,vectorops.sub(centroid,a)) > 0: b,c = c,b n = vectorops.mul(n,-1) try: n = vectorops.mul(n,1.0/vectorops.norm(n)) glNormal3f(*n) except ZeroDivisionError: pass glBegin(GL_TRIANGLES) glTexCoord3f(uvwscale[0]*(a[0]-vmin[0]),uvwscale[1]*(a[1]-vmin[1]),uvwscale[2]*(a[2]-vmin[2])) glVertex3f(*a) glTexCoord3f(uvwscale[0]*(b[0]-vmin[0]),uvwscale[1]*(b[1]-vmin[1]),uvwscale[2]*(b[2]-vmin[2])) glVertex3f(*b) glTexCoord3f(uvwscale[0]*(c[0]-vmin[0]),uvwscale[1]*(c[1]-vmin[1]),uvwscale[2]*(c[2]-vmin[2])) glVertex3f(*c) glEnd()
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 score(self,robot): """Returns an error score that is equal to the optimum at a feasible solution. Evaluated at the robot's current configuration.""" for obj in self.objectives: obj.robot = robot solver = ik.solver(self.objectives) c = (0 if self.costFunction is None else self.costFunction(robot.getConfig())) return c+vectorops.norm(solver.getResidual())
def cal_reactive_vel(self, pos_hand, pos_target, vel_ratio): reactive_vel = [0.0, 0.0, 0.0] pos_diff = vectorops.sub(tuple(pos_target), tuple(pos_hand)) pos_diff_norm = vectorops.norm(pos_diff) if pos_diff_norm >= 0.02: vel = vectorops.mul(pos_diff, vel_ratio) reactive_vel = list(vel) return reactive_vel
def score(self, robot): """Returns an error score that is equal to the optimum at a feasible solution. Evaluated at the robot's current configuration.""" for obj in self.objectives: obj.robot = robot solver = ik.solver(self.objectives) c = (0 if self.costFunction is None else self.costFunction( robot.getConfig())) return c + vectorops.norm(solver.getResidual())
def interpolate_rotation(R1, R2, u): """Interpolate linearly between the two rotations R1 and R2. TODO: the current implementation doesn't work properly. Why? """ m1 = so3.moment(R1) m2 = so3.moment(R2) mu = interpolate_linear(m1, m2, u) angle = vectorops.norm(mu) axis = vectorops.div(mu, angle) return so3.rotation(axis, angle)
def findOptPrepareLoc(self, hit_xyz, prep_loc_dict): best_loc = None min_dist = float('inf') for each_key in prep_loc_dict.keys(): cur_prep = prep_loc_dict[each_key] cur_dist = vectorops.norm(vectorops.sub(hit_xyz, cur_prep)) if cur_dist < min_dist: best_loc = each_key min_dist = cur_dist return best_loc
def look_at(self, pos, tgt, scale=None): if self.lockX: tgt[0] = pos[0] if self.lockY: tgt[1] = pos[1] cam = self.view.camera if scale is not None: cam.dist = op.norm(op.sub(tgt, pos)) * scale cam.rot = self.get_camera_rot(op.sub(pos, tgt)) cam.tgt = tgt
def add_light(properties,pos,tgt=None,color=[1.,1.,1.], \ spotlight=False,radius=15.,falloff=20.,tightness=10., \ area=0.,sample=9,adaptive=True,jitter=True): if 'lights' not in properties: properties['lights'] = [] if isinstance(pos, list) and isinstance(pos[0], list): if isinstance(tgt, list) and isinstance(tgt[0], list): for p, t in zip(pos, tgt): add_light(properties,pos=p,tgt=t,color=color, \ spotlight=spotlight,radius=radius,falloff=falloff,tightness=tightness, \ area=area,sample=sample,adaptive=adaptive,jitter=jitter) else: for p in pos: add_light(properties,pos=p,tgt=tgt,color=color, \ spotlight=spotlight,radius=radius,falloff=falloff,tightness=tightness, \ area=area,sample=sample,adaptive=adaptive,jitter=jitter) else: light_params = [list(pos), 'color', list(color)] if spotlight: light_params += [ 'spotlight', 'radius', radius, 'falloff', falloff, 'tightness', tightness, 'point_at', tgt ] if area > 0.: d = op.sub(tgt, pos) d = op.mul(d, 1 / op.norm(d)) minId = None for i in range(3): if abs(d[i]) <= abs(d[(i + 1) % 3]) and abs(d[i]) <= abs( d[(i + 2) % 3]): minId = i t0 = [0, 0, 0] t0[minId] = 1. t1 = op.cross(d, t0) t1 = op.mul(t1, 1 / op.norm(t1)) t0 = op.cross(d, t1) light_params += [ 'area_light', list(op.mul(t0, area)), list(op.mul(t1, area)), sample, sample, 'adaptive', 1, 'jitter' ] properties['lights'].append(vp.LightSource(*light_params))
def updateVis(self): """This gets called every control loop. TODO: You may consider visually debugging some of your code here, along with initVis(). For example, to draw a ghost robot at a given configuration q, you can call: kviz.add_ghost() (in initVis) kviz.set_ghost(q) (in updateVis) The current code draws gravity-inflenced arcs leading from all the object position / velocity estimates from your state estimator. Event C folks should set gravity=0 in the following code. """ #print "Start of a new time increment" if self.objectEstimates: for o in self.objectEstimates.objects: x = [o.x[0], o.x[1], o.x[2]] v = [o.x[3], o.x[4], o.x[5]] # draw the actual spheres kviz.update_sphere("object_est" + str(o.name), o.x[0], o.x[1], o.x[2], 0.03) kviz.set_color("object_est" + str(o.name), o.name[0], o.name[1], o.name[2]) # if the ball is off the table, don't evaluate if ((x[2] < (-.5))): #print o.name, " is off the table" pass # if ball is in the corner, # if ball is moving backwards, # if ball's velocity is below threshold elif (x[0] >= 3 or o.x[3] > 0 or vectorops.norm(v) < 0.5): #print o.name, " is not valid" pass else: #draw a point #draw an arc trace = [] #print o.name, " x_position", o.x[0], " ", o.x[1], " ", o.x[2] gravity = 9.8 for i in range(40): # set time interval t = 0.05 # append to trace trace.append(x) # predict the next location [x, v] = self.predict_next_location(x, v, t, gravity) kviz.update_polyline("object_trace" + str(o.name), trace) kviz.set_color("object_trace" + str(o.name), o.name[0], o.name[1], o.name[2])
def myPlayerLogic(self, dt, sensorReadings, objectStateEstimate, robotController): """ TODO: fill this out to updates the robot's low level controller in response to a new time step. This is allowed to set any attributes of MyController that you wish, such as self.state. Arguments: - dt: the simulation time elapsed since the last call - sensorReadings: the sensor readings given on the current time step. this will be a dictionary mapping sensor names to sensor data. The name "blobdetector" indicates a sensor reading coming from the blob detector. The name "omniscient" indicates a sensor reading coming from the omniscient object sensor. You will not need to use raw sensor data directly, if you have a working state estimator. - objectStateEstimate: a MultiObjectStateEstimate class (see stateestimation.py) produced by the state estimator. - robotController: a SimRobotController instance giving access to the robot's low-level controller. You can call any of the methods. At the end of this call, you can either compute some PID command via robotController.setPIDCommand(), or compute a trajectory to execute via robotController.set/addMilestone(). (if you are into masochism you can use robotController.setTorque()) """ robot = robotController.model() # Solver possible_catch = [] # if no result return to the center prepare location num_obs = len(objectStateEstimate.objects) if num_obs == 0: self.qdes = self.ini_config else: for o in objectStateEstimate.objects: if (o.meanVelocity()[0] < -1 and abs(o.meanPosition()[1]) < WALL_Y and o.meanPosition()[0] > WALL_XS[1] and o.meanPosition()[0] < 2) or vectorops.norm( vectorops.sub(o.meanVelocity(), [0] * 3)) == 0: CATCH_EST.addRecord(o) catch_array = CATCH_EST.getRecord(o.name) if catch_array != []: last_record = catch_array[-1] for idx in range(last_record.shape[0]): possible_catch.append(last_record[idx, :].tolist()) opt_result = self.calOptCatch(robot, possible_catch) self.qdes = opt_result[0] robotController.setMilestone(self.qdes) return
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 advance(self, q, dq, dt): """ Updates internal state: accumulates iterm and updates x_last """ if self.weight > 0: eP = self.getSensedError(q) # update iterm if self.eI == None: self.eI = vectorops.mul(eP, dt) else: self.eI = vectorops.madd(self.eI, eP, dt) einorm = vectorops.norm(self.eI) if einorm > self.eImax: self.eI = vectorops.mul(self.eI, self.eImax / einorm) #update qLast self.qLast = q return
def advance(self, q, dq, dt): """ Updates internal state: accumulates iterm and updates x_last """ if self.weight > 0: eP = self.getSensedError(q) # update iterm if self.eI == None: self.eI = vectorops.mul(eP,dt) else: self.eI = vectorops.madd(self.eI, eP, dt) einorm = vectorops.norm(self.eI) if einorm > self.eImax: self.eI = vectorops.mul(self.eI,self.eImax/einorm) #update qLast self.qLast = q return
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 create_env_light_for_bb(bb, distRatio=1.5, zdim=2, nLight=3): xdim = (zdim + 1) % 3 ydim = (zdim + 2) % 3 ctr = [0., 0., 0.] ctr[xdim] = (bb[0][xdim] + bb[1][xdim]) / 2 ctr[ydim] = (bb[0][ydim] + bb[1][ydim]) / 2 ctr[zdim] = (bb[1][zdim] - bb[0][zdim]) * distRatio + bb[1][zdim] x = (bb[1][xdim] - bb[0][xdim]) / 2 y = (bb[1][ydim] - bb[0][ydim]) / 2 dist = op.norm((x, y)) * (1. + distRatio) pos = [] for d in range(nLight): ang = math.pi * 2 * d / nLight ctrd = [c for c in ctr] ctrd[xdim] += dist * math.cos(ang) ctrd[ydim] += dist * math.sin(ang) pos.append(ctrd) tgt = [c for c in ctr] tgt[zdim] = bb[0][zdim] return pos, tgt
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 localPlanner(self, n1, n2, sleepFlag=False): dist = self.env.n * vectorops.norm( vectorops.sub([n1.x, n1.y, n1.z], [n2.x, n2.y, n2.z])) vel = 0.04 tm = dist / vel for i in range(int(tm)): delta = i / float(tm) x = n1.x + (n2.x - n1.x) * delta y = n1.y + (n2.y - n1.y) * delta z = n1.z + (n2.z - n1.z) * delta sc = n1.sc + (n2.sc - n1.sc) * delta tht = n1.tht + (n2.tht - n1.tht) * delta vis.lock() self.env.setConfig(x, y, z, sc, tht) vis.unlock() colFlag = False colFlag = self.env.checkCollision() if (colFlag): return False if sleepFlag: time.sleep(vel / 2) self.env.setConfig(n2.x, n2.y, n2.z, n2.sc, n2.tht) return True
def myPlayerLogic(self, dt, sensorReadings, objectStateEstimate, robotController): """ TODO: fill this out to updates the robot's low level controller in response to a new time step. This is allowed to set any attributes of MyController that you wish, such as self.state. Arguments: - dt: the simulation time elapsed since the last call - sensorReadings: the sensor readings given on the current time step. this will be a dictionary mapping sensor names to sensor data. The name "blobdetector" indicates a sensor reading coming from the blob detector. The name "omniscient" indicates a sensor reading coming from the omniscient object sensor. You will not need to use raw sensor data directly, if you have a working state estimator. - objectStateEstimate: a MultiObjectStateEstimate class (see stateestimation.py) produced by the state estimator. - robotController: a SimRobotController instance giving access to the robot's low-level controller. You can call any of the methods. At the end of this call, you can either compute some PID command via robotController.setPIDCommand(), or compute a trajectory to execute via robotController.set/addMilestone(). (if you are into masochism you can use robotController.setTorque()) """ #these are pulled out here for your convenience qcmd = robotController.getCommandedConfig() vcmd = robotController.getCommandedVelocity() qsns = robotController.getSensedConfig() vsns = robotController.getSensedVelocity() #setting a PID command can be accomplished with the following #robotController.setPIDCommand(self.qdes,[0.0]*7) #queuing up linear interpolation can be accomplished with the following #dt = 0.5 #how much time it takes for the robot to reach the target #robotController.appendLinear(self.qdes,dt) #queuing up a fast-as possible interpolation can be accomplished with the following #robotController.addMilestone(self.qdes) # get the RobotModel object from the robotController robot = robotController.model() solved_location = 0 # Check if a ball is within the space "reachable" by our robot. for o in self.objectEstimates.objects: x = [o.x[0], o.x[1], o.x[2]] v = [o.x[3], o.x[4], o.x[5]] #print "Object Position: ", x #print "Object Velocity: ", v # if the ball is off the table, don't evaluate if ((x[2] < (-.5))): #print o.name, " is off the table" pass # if ball is in the corner, # if ball is moving backwards, # if ball's velocity is below threshold elif (x[0] >= 3 or o.x[3] > 0 or vectorops.norm(v) < 0.5): #print o.name, " is not valid" pass else: #print o.name, " is moving" # define velocity for these calculations gravity = 9.8 for i in range(20): # set time interval t = 0.05 # check if the trajectory falls within robot's arm [x, v] = self.predict_next_location(x, v, t, gravity) inBounds = self.withinReach(x, -2.5, -1.7, -1.2, 1.2, 0.09, 1.2) if (inBounds == 1): #print o.name, " is projected to score. Block this location at:" #print x[0], x[1], x[2] (q, solved) = self.ik_solver(robot, x, qsns) self.state = "blocking" solved_location = 1 if (solved): #print "setting milestone for blocking robot" q[robot.numLinks() - 1] = 0 self.lastx = x robotController.setMilestone(q) break # predict the next location # end loop # state machine to determine which state the robot should enter # requires 4 misses to enter blocking mode if (solved_location == 0): if (self.state == "blocking"): self.state = "blocking1" elif (self.state == "blocking1"): self.state = "blocking2" elif (self.state == "blocking2"): self.state = "blocking3" else: # if waiting or blocking 3 self.state = 'waiting' # if the robot doesn't have any obstacles to block, go back to initial position if self.state == 'waiting': #print qsns #print "waiting" if (self.lastx[1] <= -0.29): if (self.lastx[1] < 0.3): xdes = [-1.5, -0.3, 0.5] else: xdes = [-1.5, -0.3, 0.2] elif (self.lastx[1] >= 0.29): if (self.lastx[1] < 0.3): xdes = [-1.5, 0.3, 0.5] else: xdes = [-1.5, 0.3, 0.2] #if(qsns[1] > 0.4 or qsns[1] < -0.4): # xdes = [-1.2, 0, 0.5] else: xdes = [-1.2, 0, 0.9] self.lastx = xdes (q, solved) = self.ik_solver(robot, xdes, qsns) q[robot.numLinks() - 1] = 0 if (solved): #print "setting milestone for waiting robot" robotController.setMilestone(q) elif ( self.state == "reset" ): # if you're blocking, the controller was already programmed so just wait it out pass else: #print "blocking" pass return
def make_object_pile(world,container,objects,container_wall_thickness=0.01,randomize_orientation=True, visualize=False,verbose=0): """For a given container and a list of objects in the world, drops the objects inside the container and simulates until stable. Args: world (WorldModel): the world containing the objects and obstacles container: the container RigidObject / Terrain in world into which objects should be spawned. Assumed axis-aligned. objects (list of RigidObject): a list of RigidObjects in the world, at arbitrary locations. They are placed in order. container_wall_thickness (float, optional): a margin subtracted from the container's outer dimensions into which the objects are spawned. randomize_orientation (bool or str, optional): if True, the orientation of the objects are completely randomized. If 'z', only the z orientation is randomized. If False or None, the orientation is unchanged visualize (bool, optional): if True, pops up a visualization window to show the progress of the pile verbose (int, optional): if > 0, prints progress of the pile. Side effect: the positions of objects in world are modified Returns: (tuple): (world,sim), containing - world (WorldModel): the original world - sim (Simulator): the Simulator instance at the state used to obtain the stable placement of the objects. Note: Since world is modified in-place, if you wish to make multiple worlds with piles of the same objects, you should use world.copy() to store the configuration of the objects. You may also wish to randomize the object ordering using random.shuffle(objects) between instances. """ container_outer_bb = _get_bound(container) container_inner_bb = (vectorops.add(container_outer_bb[0],[container_wall_thickness]*3),vectorops.sub(container_outer_bb[1],[container_wall_thickness]*3)) spawn_area = (container_inner_bb[0][:],container_inner_bb[1][:]) collision_margin = 0.0025 if visualize: from klampt import vis from klampt.model import config import time oldwindow = vis.getWindow() newwindow = vis.createWindow("make_object_pile dynamic visualization") vis.setWindow(newwindow) vis.show() visworld = world.copy() vis.add("world",visworld) sim = Simulator(world) sim.setSetting("maxContacts","20") sim.setSetting("adaptiveTimeStepping","0") Tfar = (so3.identity(),[0,0,-100000]) for object in objects: R,t = object.getTransform() object.setTransform(R,Tfar[1]) sim.body(object).setTransform(*Tfar) sim.body(object).enable(False) if verbose: print("Spawn area",spawn_area) if visualize: vis.lock() config.setConfig(visworld,config.getConfig(world)) vis.unlock() for index in range(len(objects)): #always spawn above the current height of the pile if index > 0: objects_bound = _get_bound(objects[:index]) if verbose: print("Existing objects bound:",objects_bound) zshift = max(0.0,objects_bound[1][2] - spawn_area[0][2]) spawn_area[0][2] += zshift spawn_area[1][2] += zshift object = objects[index] obb = _get_bound(object) zmin = obb[0][2] R0,t0 = object.getTransform() feasible = False for sample in range(1000): R,t = R0[:],t0[:] if randomize_orientation == True: R = so3.sample() t[2] = spawn_area[1][2] - zmin + t0[2] + collision_margin object.setTransform(R,t) xy_randomize(object,spawn_area[0],spawn_area[1]) if verbose: print("Sampled position of",object.getName(),object.getTransform()[1]) if not randomize_orientation: _,t = object.getTransform() object.setTransform(R,t) #object spawned, now settle sobject = sim.body(object) sobject.enable(True) sobject.setTransform(*object.getTransform()) res = sim.checkObjectOverlap() if len(res[0]) == 0: feasible = True #get it low as possible without overlapping R,t = object.getTransform() for lower in range(100): sobject.setTransform(R,vectorops.add(t,[0,0,-(lower+1)*0.01])) res = sim.checkObjectOverlap() if len(res[0]) != 0: if verbose: print("Terminated lowering at",lower,"cm lower") sobject.setTransform(R,vectorops.add(t,[0,0,-lower*0.01])) res = sim.checkObjectOverlap() break sim.updateWorld() break if not feasible: if verbose: print("Failed to place object",object.getName()) return None if visualize: vis.lock() config.setConfig(visworld,config.getConfig(world)) vis.unlock() time.sleep(0.1) if verbose: print("Beginning to simulate") #start letting everything fall for firstfall in range(10): sim.simulate(0.01) if visualize: vis.lock() config.setConfig(visworld,config.getConfig(world)) vis.unlock() time.sleep(0.01) maxT = 5.0 dt = 0.01 t = 0.0 wdamping = -0.01 vdamping = -0.1 while t < maxT: settled = True for object in objects: sobject = sim.body(object) w,v = sobject.getVelocity() sobject.applyWrench(vectorops.mul(v,vdamping),vectorops.mul(w,wdamping)) if vectorops.norm(w) + vectorops.norm(v) > 1e-4: #settled settled=False break if settled: break if visualize: t0 = time.time() sim.simulate(dt) if visualize: vis.lock() config.setConfig(visworld,config.getConfig(world)) vis.unlock() time.sleep(max(0.0,dt-(time.time()-t0))) t += dt if visualize: vis.show(False) vis.setWindow(oldwindow) return (world,sim)
def isArrive(cur_q, target_q, error_limit): return vectorops.norm(vectorops.sub(cur_q, target_q)) < error_limit
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 linearKernel(p1,p2,param): r = vo.norm(vo.sub(p1,p2)) if r <= param: return (param-r)/param else: return 0
def solve(self,robot=None,params=IKSolverParams()): """Globally solves the given problem. Returns the solution configuration or None if failed.""" #set this to False if you want to run the local optimizer for each #random restart. postOptimize = True t0 = time.time() if len(self.objectives) == 0: if self.costFunction is not None or self.feasibilityTest is not None: raise NotImplementedError("Raw optimization without IK goals not done yet") return None if robot is None: if not hasattr(self.objectives[0],'robot'): print "The objectives passed to IKSolver should come from ik.objective() or have their 'robot' member manually set" robot = self.objectives[0].robot else: for obj in self.objectives: obj.robot = robot solver = ik.solver(self.objectives) if self.activeDofs is not None: solver.setActiveDofs(self.activeDofs) ikActiveDofs = self.activeDofs if self.jointLimits is not None: solver.setJointLimits(*self.jointLimits) qmin,qmax = solver.getJointLimits() if self.activeDofs is None: #need to distinguish between dofs that affect feasibility vs ikActiveDofs = solver.getActiveDofs() if self.costFunction is not None or self.feasibilityTest is not None: activeDofs = [i for i in range(len(qmin)) if qmin[i] != qmax[i]] nonIKDofs = [i for i in activeDofs if i not in ikActiveDofs] ikToActive = [activeDofs.index(i) for i in ikActiveDofs] else: activeDofs = ikActiveDofs nonIKDofs = [] ikToActive = range(len(activeDofs)) else: activeDofs = ikActiveDofs nonIKDofs = [] ikToActive = range(len(ikActiveDofs)) #sample random start point if params.startRandom: solver.sampleInitial() if len(nonIKDofs)>0: q = robot.getConfig() for i in nonIKDofs: q[i] = random.uniform(qmin[i],qmax[i]) robot.setConfig(q) if params.localMethod is not None or params.globalMethod is not None: #set up optProblem, an instance of optimize.Problem optProblem = optimize.Problem() Jactive = [[0.0]*len(activeDofs)]*len(solver.getResidual()) def ikConstraint(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v robot.setConfig(q) return solver.getResidual() if NEW_KLAMPT: def ikConstraintJac(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v robot.setConfig(q) return solver.getJacobian() else: #old version of Klamp't didn't compute the jacobian w.r.t. the active DOFS def ikConstraintJac(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v robot.setConfig(q) Jikdofs = solver.getJacobian() for i in ikActiveDofs: for j in xrange(len(Jactive)): Jactive[j][ikToActive[i]] = Jikdofs[j][i] return Jactive def costFunc(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v return self.costFunction(q) def feasFunc(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v return self.feasibilityTest(q) optProblem.addEquality(ikConstraint,ikConstraintJac) if len(qmax) > 0: optProblem.setBounds([qmin[d] for d in activeDofs],[qmax[d] for d in activeDofs]) if self.costFunction is None: optProblem.setObjective(lambda x:0) else: optProblem.setObjective(costFunc) if self.feasibilityTest is not None: optProblem.setFeasibilityTest(feasFunc) #optProblem is now ready to use if self.softObjectives: softOptProblem = optimize.Problem() def costFunc(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v robot.setConfig(q) return vectorops.normSquared(solver.getResidual())*0.5 def costFuncGrad(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v robot.setConfig(q) return solver.getResidual() if len(qmax) > 0: softOptProblem.setBounds([qmin[d] for d in activeDofs],[qmax[d] for d in activeDofs]) if self.feasibilityTest is not None: softOptProblem.setFeasibilityTest(feasFunc) softOptProblem.setObjective(costFunc,costFuncGrad) #softOptProblem is now ready to use if params.globalMethod is not None: q = robot.getConfig() x0 = [q[d] for d in activeDofs] if self.softObjectives: #globally optimize the soft objective function. If 0 objective value is obtained, use equality constrained #optProblem. If 0 objective value is not obtained, constrain the residual norm-squared to be that value (succ,res) = global_solve(softOptProblem,params,x0) if not succ: print "Global soft optimize returned failure" return None for d,v in zip(activeDofs,res): q[d] = v if self.costFunction is None: #no cost function, just return print "Global optimize succeeded! Cost",self.costFunction(q) return q x0 = res #now modify the constraint of optProblem robot.setConfig(q) residual = solver.getResidual() if max(abs(v) for v in residual) < params.tol: #the constraint is satisfied, now just optimize cost pass else: #the constraint is not satisfied, now use the residual as the constraint threshold = 0.5*vectorops.normSquared(residual) def inequality(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v robot.setConfig(q) return [vectorops.normSquared(solver.getResidual())*0.5 - threshold] def inequalityGrad(x): return [costFuncGrad(x)] optProblem.equalities = [] optProblem.equalityGrads = [] optProblem.addInequality(inequality,inequalityGrad) #do global optimization of the cost function and return (succ,res) = global_solve(optProblem,params,x0) if not succ: print "Global optimize returned failure" return None for d,v in zip(activeDofs,res): q[d] = v #check feasibility if desired if self.feasibilityTest is not None and not self.feasibilityTest(q): print "Result from global optimize isn't feasible" return None if not softObjectives: if max(abs(v) for v in solver.getResidual()) > params.tol: print "Result from global optimize doesn't satisfy tolerance. Residual",vectorops.norm(solver.getResidual()) return None #passed print "Global optimize succeeded! Cost",self.costFunction(q) return q #DONT DO THIS... much faster to do newton solves first, then local optimize. if not postOptimize and params.localMethod is not None: if self.softObjectives: raise RuntimeError("Direct local optimization of soft objectives is not done yet") #random restart + local optimize optSolver = optimize.LocalOptimizer(method=params.localMethod) q = robot.getConfig() x0 = [q[d] for d in activeDofs] optSolver.setSeed(x0) best = None bestQuality = float('inf') for restart in xrange(params.numRestarts): if time.time() - t0 > params.timeout: return best res = optSolver.solve(optProblem,params.numIters,params.tol) if res[0]: q = robot.getConfig() for d,v in zip(activeDofs,res[1]): q[d] = v #check feasibility if desired if self.feasibilityTest is not None and not self.feasibilityTest(q): continue if self.costFunction is None: #feasible return q else: #optimize quality = self.costFunction(q) if quality < bestQuality: best = q bestQuality = quality #random restart solver.sampleInitial() q = robot.getConfig() if len(nonIKDofs)>0: for i in nonIKDofs: q[i] = random.uniform(qmin[i],qmax[i]) x0 = [q[d] for d in activeDofs] optSolver.setSeed(x0) else: #random-restart newton-raphson solver.setMaxIters(params.numIters) solver.setTolerance(params.tol) best = None bestQuality = float('inf') if self.softObjectives: #quality is a tuple bestQuality = bestQuality,bestQuality quality = None for restart in xrange(params.numRestarts): if time.time() - t0 > params.timeout: return best t0 = time.time() res = solver.solve() if res or self.softObjectives: q = robot.getConfig() #check feasibility if desired t0 = time.time() if self.feasibilityTest is not None and not self.feasibilityTest(q): if len(nonIKDofs) > 0: u = float(restart+0.5)/params.numRestarts q = robot.getConfig() #perturbation sampling for i in nonIKDofs: delta = u*(qmax[i]-qmin[i])*0.5 q[i] = random.uniform(max(q[i]-delta,qmin[i]),min(q[i]+delta,qmax[i])) robot.setConfig(q) if not self.feasibilityTest(q): solver.sampleInitial() continue else: solver.sampleInitial() continue if self.softObjectives: residual = solver.getResidual() ikerr = max(abs(v) for v in residual) if ikerr < params.tol: ikerr = 0 else: #minimize squared error ikerr = vectorops.normSquared(residual) if self.costFunction is None: cost = 0 if ikerr == 0: #feasible and no cost return q else: cost = self.costFunction(q) quality = ikerr,cost else: if self.costFunction is None: #feasible return q else: #optimize quality = self.costFunction(q) if quality < bestQuality: best = q bestQuality = quality #sample a new ik seed solver.sampleInitial() #post-optimize using local optimizer if postOptimize and best is not None and params.localMethod is not None: if self.softObjectives: robot.setConfig(best) residual = solver.getResidual() if max(abs(v) for v in residual) > params.tol: #the constraint is not satisfied, now use the residual as the constraint threshold = 0.5*vectorops.normSquared(residual) def inequality(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v robot.setConfig(q) return [vectorops.normSquared(solver.getResidual())*0.5 - threshold] def inequalityGrad(x): return [costFuncGrad(x)] optProblem.equalities = [] optProblem.equalityGrads = [] optProblem.addInequality(inequality,inequalityGrad) optSolver = optimize.LocalOptimizer(method=params.localMethod) x0 = [best[d] for d in activeDofs] optSolver.setSeed(x0) res = optSolver.solve(optProblem,params.numIters,params.tol) if res[0]: q = robot.getConfig() for d,v in zip(activeDofs,res[1]): q[d] = v #check feasibility if desired if self.feasibilityTest is not None and not self.feasibilityTest(q): pass elif self.costFunction is None: #feasible best = q else: #optimize quality = self.costFunction(q) if quality < bestQuality: #print "Optimization improvement",bestQuality,"->",quality best = q bestQuality = quality elif quality > bestQuality + 1e-2: print "Got worse solution by local optimizing?",bestQuality,"->",quality return best
def setGravity(self, g): if len(g) == 3 and vectorops.norm(g) < 10.0: self._gravity = g
def make_grasp_map(world, id): """Estimates a grasp quality and regression map for a sensor image. Input: world (WorldModel): contains a robot and a sensor Output: a 4-channel numpy array of size (w,h,3) with w x h the sensor dimensions and all values in the range [0,1]. The first channel encodes the quality. The second encodes the grasp opening amount. The third and fourth encode the orientation of the grasp relative to the camera frame, as a heading and elevation. Make sure to note how you've transformed these quantities to [0,1]! These will be needed for decoding. """ robot = world.robot(0) sensor = robot.sensor(0) sensor_xform = sensing.get_sensor_xform(sensor, robot) w = int(sensor.getSetting("xres")) h = int(sensor.getSetting("yres")) #You'll be filling this image out result = np.zeros((h, w, 4)) result[:, :, 3] = 0.5 #this shows how to go from a point in space to a pixel # point = (0,0,0) # projected = sensing.camera_project(sensor,robot,point) # if projected is None: # pass # else: # x,y,z = projected # if x < 0 or x > w or y < 0 or y > h: # pass # else: # result[int(y),int(x),0]=1 # result[50,10,0]=1 #load the gripper info and grasp database source_gripper = robotiq_85 global gripper_robot, gripper_world, grasp_db if gripper_robot is None: gripper_world = WorldModel() if not gripper_world.readFile(source_gripper.klampt_model): raise ValueError("Unable to read gripper file " + source_gripper.klampt_model) gripper_robot = gripper_world.robot(0) gripper_geom_open = source_gripper.get_geometry(gripper_robot, source_gripper.open_config) if grasp_db is None: db = grasp_database.GraspDatabase(source_gripper) if not db.load("../data/grasps/robotiq_85_more_sampled_grasp_db.json"): raise RuntimeError("Can't load grasp database?") grasp_db = db for i in range(world.numRigidObjects()): obj = world.rigidObject(i) ycb_obj = obj.getName() if ycb_obj not in grasp_db.object_to_grasps: print("Can't find object", ycb_obj, "in database") print("Database only contains objects", grasp_db.objects) raise ValueError() grasps = grasp_db.object_to_grasps[ycb_obj] gripper_tip = vectorops.madd(source_gripper.center, source_gripper.primary_axis, source_gripper.finger_length) num_grasps_valid = 0 for gindex, g in enumerate(grasps): #TODO: problem 1B: do collision testing of gripper_geom found_approach = False if (id, ycb_obj, gindex) in grasps_feasible: found_approach = grasps_feasible[(id, ycb_obj, gindex)] else: #this is the Geometry3D corresponding to the robot at the opening configuration gripper_geom = source_gripper.get_geometry( gripper_robot, g.finger_config) local_pt, world_pt = g.ik_constraint.getPosition() local_axis, world_axis = g.ik_constraint.getRotationAxis() #TODO: put your code here R = so3.vector_rotation(source_gripper.primary_axis, world_axis) t = vectorops.sub(world_pt, source_gripper.center) gripper_geom_open.setCurrentTransform(R, t) non_collision = True for i in range(world.numRigidObjects()): if i == gripper_robot.getID(): continue if gripper_geom_open.collides( world.rigidObject(i).geometry()): non_collision = False break found_approach = True if non_collision else False if found_approach: gripper_geom.setCurrentTransform(R, t) for i in range(world.numRigidObjects()): if i == gripper_robot.getID() or i == obj.getID(): continue if gripper_geom.collides( world.rigidObject(i).geometry()): found_approach = False break grasps_feasible[(id, ycb_obj, gindex)] = found_approach if not found_approach: continue num_grasps_valid += 1 Tfixed = g.ik_constraint.closestMatch(*se3.identity()) Tworld = se3.mul(obj.getTransform(), Tfixed) gworld = se3.apply(Tworld, gripper_tip) projected = sensing.camera_project(sensor, robot, gworld) if projected is not None: x, y, z = projected x = int(x) y = int(y) if x < 0 or x >= w or y < 0 or y >= h: continue gripper_opening_axis = so3.apply(Tworld[0], source_gripper.secondary_axis) gripper_opening_axis_cam = so3.apply(so3.inv(sensor_xform[0]), gripper_opening_axis) if gripper_opening_axis_cam[1] < 0: gripper_opening_axis_cam = vectorops.mul( gripper_opening_axis_cam, -1) gripper_axis_heading = math.atan2(gripper_opening_axis_cam[1], gripper_opening_axis_cam[0]) xy = vectorops.norm(gripper_opening_axis_cam[:2]) if xy < 1e-7: gripper_axis_elevation = math.pi * 0.5 else: gripper_axis_elevation = math.atan( gripper_opening_axis_cam[2] / xy) score0 = math.exp(-g.score) window = 10 std = 5 for u in range(-window, window + 1): if y + u < 0 or y + u >= h: continue for v in range(-window, window + 1): if x + v < 0 or x + v >= w: continue score = score0 * math.exp(-(u**2 + v**2) / (2 * std**2)) if score > result[y + u, x + v, 0]: result[y + u, x + v, :] = [ score, source_gripper.config_to_opening( g.finger_config), gripper_axis_heading / math.pi, gripper_axis_elevation / math.pi + 0.5 ] print("{}/{} grasps valid for object {}".format( num_grasps_valid, len(grasps), ycb_obj)) return result
def invKernel(p1,p2,param): r = vo.norm(vo.sub(p1,p2)) return param/(param+r)