def calculate_workspace_free(robot,obstacles,end_effector,point_local): """Calculate the reachable workspace of the end effector point whose coordinates are `point_local` on link `end_effector`. Ensure that the robot is collision free with itself and with environment obstacles """ global lower_corner,upper_corner resolution = (20,20,20) cellsize = vectorops.div(vectorops.sub(upper_corner,lower_corner),resolution) invcellsize = vectorops.div(resolution,vectorops.sub(upper_corner,lower_corner)) reachable = np.zeros(resolution) #TODO: your code here feasible = collision_free(robot,obstacles) if feasible: wp = end_effector.getWorldPosition(point_local) index = [int(math.floor(v)) for v in vectorops.mul(vectorops.sub(wp,lower_corner),invcellsize)] if all(i>=0 and i<r for (i,r) in zip(index,resolution)): reachable[tuple(index)] = 1.0 # print(index) num_samples = 100000 rand_positions = np.random.rand(num_samples, 3) rand_positions[:,0] = rand_positions[:,0] * vectorops.sub(upper_corner, lower_corner)[0] + lower_corner[0] rand_positions[:,1] = rand_positions[:,1] * vectorops.sub(upper_corner, lower_corner)[1] + lower_corner[1] rand_positions[:,2] = rand_positions[:,2] * upper_corner[2] for i in range(num_samples): index = [int(math.floor(v)) for v in vectorops.mul(vectorops.sub(rand_positions[i],lower_corner),invcellsize)] for obstacle in obstacles: if obstacle.geometry().distance_point(rand_positions[i]).d <= 0: reachable[tuple(index)] = 0.0 else: reachable[tuple(index)] = 1.0 return reachable
def calculate_workspace_axis(robot, obstacles, end_effector, point_local, axis_local, axis_world): global lower_corner, upper_corner resolution = (15, 15, 15) cellsize = vectorops.div(vectorops.sub(upper_corner, lower_corner), resolution) invcellsize = vectorops.div(resolution, vectorops.sub(upper_corner, lower_corner)) reachable = np.zeros(resolution) #TODO: your code here return reachable
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 output_and_advance(self, **inputs): try: q = inputs['q'] dq = inputs['dq'] u = vectorops.div(vectorops.sub(inputs['qcmd'], q), inputs['dt']) except KeyError: print "Warning, cannot debug motion model, dq or dqcmd not in input" return None if self.dqpredlast != None: if self.activeDofs != None: dq = dq[:] for i in [ i for i in range(len(q)) if i not in self.activeDofs ]: dq[i] = self.dqpredlast[i] #compare motion model to dq print "Motion model error:", np.linalg.norm(self.dqpredlast - np.array(dq)) (v, i) = max( zip( np.abs(self.dqpredlast - np.array(dq)).tolist(), range(len(dq)))) print " Max error:", v, "at", i, if self.robot != None: print self.robot.link(i).getName() else: print print " Command:", self.ulast[i], "Predicted:", self.dqpredlast[ i], "Actual:", dq[i] print " pred:", self.Alast[i, i], "*u +", self.blast[i]
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 output_and_advance(self,**inputs): try: dt = inputs["dt"] q = inputs["q"] except KeyError: raise ValueError("Input needs to have configuration 'q' and timestep 'dt'") if len(q)==0: return None if self.qlast==None: dq = [0]*len(q) else: if self.robot==None: dq = vectorops.div(self.robot.sub(q,self.qlast),dt) else: assert(len(self.qlast)==len(q)) dq = vectorops.div(self.robot.interpolate_deriv(self.qlast,q),dt) self.qlast = q return {'dq':dq}
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 advance(self, **inputs): try: dt = inputs["dt"] q = inputs[self.name] except KeyError: raise ValueError( "Input needs to have value '%s' and timestep 'dt'" % (self.name, )) if len(q) == 0: return None if self.qlast == None: dq = [0] * len(q) else: if self.robot == None: dq = vectorops.div(self.robot.sub(q, self.qlast), dt) else: assert (len(self.qlast) == len(q)) dq = vectorops.div(self.robot.interpolate_deriv(self.qlast, q), dt) self.qlast = q return {'d' + self.name: dq}
def calculate_workspace_axis(robot,obstacles,end_effector,point_local,axis_local,axis_world): global lower_corner,upper_corner resolution = (15,15,15) cellsize = vectorops.div(vectorops.sub(upper_corner,lower_corner),resolution) invcellsize = vectorops.div(resolution,vectorops.sub(upper_corner,lower_corner)) reachable = np.zeros(resolution) #TODO: your code here for i in range(resolution[0]): for j in range(resolution[1]): for k in range(resolution[2]): orig_config = robot.getConfig() point = vectorops.add(vectorops.mul([i,j,k], cellsize), lower_corner) world_constraint = ik.fixed_rotation_objective(end_effector, world_axis=axis_world) local_constraint = ik.fixed_rotation_objective(end_effector, local_axis=axis_local) point_goal = ik.objective(end_effector, local=point_local, world=point) if ik.solve_global([point_goal, local_constraint, world_constraint], iters=10): reachable[i,j,k] = 1.0 robot.setConfig(orig_config) return reachable
def drawDisconnections(orientation=None): bmin, bmax = self.resolution.domain active = [ i for i, (a, b) in enumerate(zip(bmin, bmax)[:3]) if b != a ] if self.useboundary == None: self.useboundary = (len(active) == 2) verts, faces = self.resolution.computeDiscontinuities( self.useboundary, orientation) if len(active) == 3: glEnable(GL_LIGHTING) glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) glDisable(GL_CULL_FACE) #if self.walk_workspace_path != None: # gldraw.setcolor(1,0,1,0.25) #else: # gldraw.setcolor(1,0,1,0.5) for tri in faces: tverts = [verts[v] for v in tri] centroid = vectorops.div(vectorops.add(*tverts), len(tri)) params = [ (x - a) / (b - a) for (x, a, b) in zip(centroid, self.resolution.domain[0], self.resolution.domain[1]) ] if self.walk_workspace_path != None: gldraw.setcolor(params[0], params[1], params[2], 0.25) else: gldraw.setcolor(params[0], params[1], params[2], 1.0) gldraw.triangle(*tverts) glEnable(GL_CULL_FACE) else: glDisable(GL_LIGHTING) glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) glColor4f(1, 0, 1, 0.5) glLineWidth(4.0) glBegin(GL_LINES) for s in faces: spts = verts[s[0]], verts[s[1]] glVertex3f(spts[0][0], 0, spts[0][1]) glVertex3f(spts[1][0], 0, spts[1][1]) glEnd() glLineWidth(1.0)
def getSensedVelocity(self, q, dq, dt): """Gets task velocity from sensed configuration q. Default implementation uses finite differencing. Other implementations may use jacobian. """ #uncomment this to get a jacobian based technique #return np.dot(self.getJacobian(q),dq) if self.qLast == None: return None else: xlast = self.getSensedValue(self.qLast) xcur = self.getSensedValue(q) return vectorops.div(self.taskDifference(xcur, xlast), dt)
def getSensedVelocity(self, q, dq, dt): """Gets task velocity from sensed configuration q. Default implementation uses finite differencing. Other implementations may use jacobian. """ #uncomment this to get a jacobian based technique #return np.dot(self.getJacobian(q),dq) if self.qLast==None: return None else: xlast = self.getSensedValue(self.qLast) xcur = self.getSensedValue(q) return vectorops.div(self.taskDifference(xcur,xlast),dt)
def setDesiredVelocitiesFromDifference(self, qdes0, qdes1, dt, tasks=None): """Sets all the tasks' desired velocities from a given pair of configurations separated by dt (e.g., to follow a reference trajectory). If the 'tasks' variable is provided, it should be a list of tasks for which the desired values should be set. """ if tasks == None: tasks = self.taskList for t in tasks: xdes0 = t.getSensedValue(qdes0) xdes1 = t.getSensedValue(qdes1) dx = vectorops.div(t.taskDifference(xdes1, xdes0), dt) t.setDesiredVelocity(dx)
def calculate_workspace_free(robot, obstacles, end_effector, point_local): """Calculate the reachable workspace of the end effector point whose coordinates are `point_local` on link `end_effector`. Ensure that the robot is collision free with itself and with environment obstacles """ global lower_corner, upper_corner resolution = (20, 20, 20) cellsize = vectorops.div(vectorops.sub(upper_corner, lower_corner), resolution) invcellsize = vectorops.div(resolution, vectorops.sub(upper_corner, lower_corner)) reachable = np.zeros(resolution) #TODO: your code here feasible = collision_free(robot, obstacles) if feasible: wp = end_effector.getWorldPosition(point_local) index = [ int(math.floor(v)) for v in vectorops.mul( vectorops.sub(wp, lower_corner), invcellsize) ] if all(i >= 0 and i < r for (i, r) in zip(index, resolution)): reachable[tuple(index)] = 1.0 return reachable
def setDesiredVelocitiesFromDifference(self,qdes0,qdes1,dt,tasks=None): """Sets all the tasks' desired velocities from a given pair of configurations separated by dt (e.g., to follow a reference trajectory). If the 'tasks' variable is provided, it should be a list of tasks for which the desired values should be set. """ if tasks == None: tasks = self.taskList for t in tasks: xdes0 = t.getSensedValue(qdes0) xdes1 = t.getSensedValue(qdes1) dx = vectorops.div(t.taskDifference(xdes1,xdes0),dt) t.setDesiredVelocity(dx)
def advance(self,**inputs): try: qcmd = inputs['qcmd'] q = inputs['q'] dt = inputs['dt'] except KeyError: return if len(q)==0: return print len(qcmd),len(q) u = vectorops.sub(qcmd,q) u = vectorops.div(u,dt) print "u estimate:",u print "u estimate rarm:",u[26:33] try: q = inputs[self.xnames[0]] dq = inputs[self.xnames[1]] except KeyError as e: print "Warning, inputs do not contain x key",e return """ try: u = sum((inputs[uname] for uname in self.unames),[]) except KeyError as e: print "Warning, inputs do not contain u key",e #u = [0]*(len(x)/2) try: u = vectorops.sub(inputs['qcmd'],inputs['q']) u = vectorops.div(u,inputs['dt']) except KeyError as e: print "Warning, inputs do not contain qcmd key either",e u = [0]*(len(x)/2) """ #self.xlast = x #return #do system ID if self.qlast != None: self.estimator.add(self.qlast,self.dqlast,u,q,dq) pass #update last state self.qlast = q self.dqlast = dq return
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 signedDistance_gradient(self, point): d = vectorops.sub(point, self.center) return vectorops.div(d, vectorops.norm(d))
def display(self): if self.configs == None: self.robot.drawGL() else: for q in self.configs: self.robot.setConfig(q) self.robot.drawGL() if self.walk_workspace_path != None: if self.temp_config: self.robot.setConfig(self.temp_config) glEnable(GL_BLEND) #glDisable(GL_DEPTH_TEST) for i in xrange(self.robot.numLinks()): self.robot.link(i).appearance().setColor(1, 0, 0, 0.5) self.robot.drawGL() for i in xrange(self.robot.numLinks()): self.robot.link(i).appearance().setColor(0.5, 0.5, 0.5, 1) #glEnable(GL_DEPTH_TEST) glDisable(GL_BLEND) glColor3f(1, 1, 0) glLineWidth(5.0) glBegin(GL_LINE_STRIP) for w in self.walk_workspace_path.milestones: if len(w) == 2: glVertex2f(w[0], w[1]) else: glVertex3f(w[0], w[1], w[2]) glEnd() glLineWidth(1.0) assert len( self.resolution.ikTaskSpace) == 1, "Can only do one task for now" task = self.resolution.ikTaskSpace[0] #DEBUG if hasattr(self.resolution, 'delaunay'): delaunay = self.resolution.delaunay delaunay_pts = self.resolution.delaunay_pts delaunay_epts = self.resolution.delaunay_epts delaunay_others = self.resolution.delaunay_others ptset = set(tuple(v) for v in delaunay_pts) assert len(ptset) == len( delaunay_pts), "Duplicate points! %d unique / %d" % ( len(ptset), len(delaunay_pts)) glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) glColor3f(0, 0, 0) glLineWidth(1.0) import random for s in delaunay.simplices: """ glBegin(GL_LINE_LOOP) for v in s: glVertex3fv(delaunay_pts[v]) glEnd() """ centroid = vectorops.div( vectorops.add(*[delaunay_pts[v] for v in s]), len(s)) assert len(s) == 3 glBegin(GL_LINES) for i, v in enumerate(s): neighbors = [w for j, w in enumerate(s) if i < j] for w in neighbors: if (v in delaunay_epts and w in delaunay_others) or ( w in delaunay_epts and v in delaunay_others): glColor3f(1, 1, 1) else: glColor3f(0, 0, 0) glVertex3fv( vectorops.interpolate(delaunay_pts[v], centroid, 0.1)) glVertex3fv( vectorops.interpolate(delaunay_pts[w], centroid, 0.1)) glEnd() glColor3f(0, 1, 0) glPointSize(5.0) glBegin(GL_POINTS) for v in delaunay_epts: glVertex3fv(delaunay_pts[v]) glEnd() glColor3f(0, 0, 1) glPointSize(5.0) glBegin(GL_POINTS) for v in delaunay_others: glVertex3fv(delaunay_pts[v]) glEnd() glEnable(GL_DEPTH_TEST) #draw workspace graph if self.drawWorkspaceRoadmap: def drawWorkspaceRoadmap(orientation=None): G = self.resolution.Gw if orientation is not None: Rclosest, G = self.resolution.extractOrientedSubgraph( orientation) print G.number_of_nodes() if not self.resolution.hasResolution(): glDisable(GL_LIGHTING) glPointSize(5.0) glColor3f(1, 0, 0) glBegin(GL_POINTS) for n, d in G.nodes_iter(data=True): glVertex3fv(self.workspaceToPoint(d['params'])) glEnd() glColor3f(1, 0.5, 0) glBegin(GL_LINES) for (i, j) in G.edges_iter(): glVertex3fv(self.workspaceToPoint(G.node[i]['params'])) glVertex3fv(self.workspaceToPoint(G.node[j]['params'])) glEnd() else: #maxn = max(len(d['qlist']) for n,d in G.nodes_iter(data=True)) #maxe = max(len(d['qlist']) for i,j,d in G.edges_iter(data=True)) #maxn = max(maxn,1) #maxe = max(maxe,1) glDisable(GL_LIGHTING) glPointSize(5.0) glBegin(GL_POINTS) for n, d in G.nodes_iter(data=True): if not self.resolution.isResolvedNode(n): continue #u = float(len(d['qlist']))/float(maxn) #nsubset = sum(1 for iq in d['qlist'] if iq in self.rr.Qsubgraph) #if nsubset > 1: # glColor3f(1,0,1) #else: # glColor3f(u,nsubset*0.5,0) glColor3f(1, 0.5, 0) glVertex3fv(self.workspaceToPoint(d['params'])) glEnd() glBegin(GL_LINES) for (i, j, d) in G.edges_iter(data=True): if not self.resolution.isResolvedNode( i) or not self.resolution.isResolvedNode(j): continue #nsubset = sum(1 for (ia,ib) in d['qlist'] if (ia in self.Qsubgraph and ib in self.Qsubgraph)) #u = float(len(d['qlist']))/float(maxe) r, g, b = 1, 1, 0 if not self.resolution.isResolvedEdge(i, j): r, g, b = 1, 0, 1 else: qd = self.robot.distance(G.node[i]['config'], G.node[j]['config']) wd = self.resolution.workspaceDistance( G.node[i]['params'], G.node[j]['params']) u = 1.0 - math.exp(-max(0.0, 2.0 * qd / wd - 1.0)) if u > 0.8: r, g, b = 1, 1.0 - u * 5.0, 0.0 else: r, g, b = u / 0.8, 1, 0.0 #assert (nsubset >=1) == self.resolution.isResolvedEdge(i,j),"Mismatch between Qsubgraph and resolution?" glColor3f(r, g, b) glVertex3fv(self.workspaceToPoint(G.node[i]['params'])) glVertex3fv(self.workspaceToPoint(G.node[j]['params'])) glEnd() """ glEnable(GL_LIGHTING) q0 = self.robot.getConfig() for iw,d in self.rr.Gw.nodes_iter(data=True): qs = [iq for iq in d['qlist'] if iq in self.rr.Qsubgraph] if len(qs) > 1: for iq in qs: self.robot.setConfig(self.rr.Gq.node[iq]['config']) self.robot.drawGL() self.robot.setConfig(q0) glDisable(GL_LIGHTING) """ if task.numConstraints > 3: if not hasattr(self, 'roadmapDisplayLists_by_orientation'): self.roadmapDisplayLists_by_orientation = dict() self.lastROrientation = None self.lastRMbest = None orientation = self.xformWidget.get()[0] if orientation != self.lastROrientation: mbest = self.resolution.closestOrientation(orientation) self.lastRMbest = mbest else: mbest = self.lastRMbest if mbest not in self.roadmapDisplayLists_by_orientation: print "Drawing new display list for moment", mbest self.roadmapDisplayLists_by_orientation[ mbest] = CachedGLObject() self.roadmapDisplayLists_by_orientation[mbest].draw( drawWorkspaceRoadmap, args=(orientation, )) else: #there looks to be a bug in GL display lists for drawing lines... #self.roadmapDisplayList.draw(drawWorkspaceRoadmap) drawWorkspaceRoadmap() else: #render boundaries only def drawDisconnections(orientation=None): bmin, bmax = self.resolution.domain active = [ i for i, (a, b) in enumerate(zip(bmin, bmax)[:3]) if b != a ] if self.useboundary == None: self.useboundary = (len(active) == 2) verts, faces = self.resolution.computeDiscontinuities( self.useboundary, orientation) if len(active) == 3: glEnable(GL_LIGHTING) glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) glDisable(GL_CULL_FACE) #if self.walk_workspace_path != None: # gldraw.setcolor(1,0,1,0.25) #else: # gldraw.setcolor(1,0,1,0.5) for tri in faces: tverts = [verts[v] for v in tri] centroid = vectorops.div(vectorops.add(*tverts), len(tri)) params = [ (x - a) / (b - a) for (x, a, b) in zip(centroid, self.resolution.domain[0], self.resolution.domain[1]) ] if self.walk_workspace_path != None: gldraw.setcolor(params[0], params[1], params[2], 0.25) else: gldraw.setcolor(params[0], params[1], params[2], 1.0) gldraw.triangle(*tverts) glEnable(GL_CULL_FACE) else: glDisable(GL_LIGHTING) glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) glColor4f(1, 0, 1, 0.5) glLineWidth(4.0) glBegin(GL_LINES) for s in faces: spts = verts[s[0]], verts[s[1]] glVertex3f(spts[0][0], 0, spts[0][1]) glVertex3f(spts[1][0], 0, spts[1][1]) glEnd() glLineWidth(1.0) #draw stabbing lines """ glDisable(GL_LIGHTING) glColor3f(1,0,0) glBegin(GL_LINES) for (i,j,d) in self.resolution.Gw.edges_iter(data=True): if self.resolution.isResolvedEdge(i,j): continue if not self.resolution.isResolvedNode(i) or not self.resolution.isResolvedNode(j): continue glVertex3fv(self.workspaceToPoint(self.resolution.Gw.node[i]['params'])) glVertex3fv(self.workspaceToPoint(self.resolution.Gw.node[j]['params'])) glEnd() """ if task.numConstraints > 3: if not hasattr(self, 'disconnectionDisplayLists_by_orientation'): self.disconnectionDisplayLists_by_orientation = dict() self.lastOrientation = None self.lastMbest = None orientation = self.xformWidget.get()[0] if orientation != self.lastOrientation: mbest = self.resolution.closestOrientation(orientation) self.lastMbest = mbest else: mbest = self.lastMbest if mbest not in self.disconnectionDisplayLists_by_orientation: self.disconnectionDisplayLists_by_orientation[ mbest] = CachedGLObject() self.disconnectionDisplayLists_by_orientation[mbest].draw( drawDisconnections, args=(mbest, )) else: self.disconnectionDisplayList.draw(drawDisconnections) bmin, bmax = self.resolution.domain #draw workspace bound glDisable(GL_LIGHTING) glColor3f(1, 0.5, 0) gldraw.box(bmin[:3], bmax[:3], lighting=False, filled=False) GLWidgetPlugin.display(self)
def find_target(self): self.target_prep_pos = (so3.identity(), [0, 10, -1]) count = 0 # print self.waiting_list for idx in self.waiting_list: # print 'i',idx bb = self.sim.world.rigidObject(idx).geometry().getBB() #decide the hand facing and finger configuration z_limit = 0.65 ratio = 1.0 if bb[1][2] > z_limit: hand_facing = 'face_toward_shelf' count += 1 else: hand_facing = 'face_down' count += 1 if bb[1][2] < 0.54 and self.hand_facing != 'sweep': hand_facing = 'sweep' if hand_facing == 'face_toward_shelf': max_limit = forward_max min_limit = forward_min face = face_toward_shelf hand_mode = 'power' elif hand_facing == 'face_down': max_limit = face_down_max min_limit = face_down_min x_diff = bb[1][0] - bb[0][0] y_diff = bb[1][1] - bb[0][1] if y_diff > x_diff * ratio: hand_mode = 'power' face = face_down elif x_diff > y_diff * ratio: hand_mode = 'power' face = face_down_rot else: hand_mode = 'precision' face = face_down else: face = sweep_pos hand_mode = 'power' if hand_facing == 'face_down': target_prep_pos = (face, vectorops.add( vectorops.div( vectorops.add(bb[1], bb[0]), 2), [0, 0, 0.5])) top_bb = bb[1][2] target_pos = (face, vectorops.div(vectorops.add(bb[1], bb[0]), 2)) target_pos[1][2] = top_bb + 0.06 for i in range(3): target_prep_pos[1][i] = max( min(target_prep_pos[1][i], max_limit[i]), min_limit[i]) target_pos[1][i] = max(min(target_pos[1][i], max_limit[i]), min_limit[i]) elif hand_facing == 'face_toward_shelf': target_prep_pos = (face, vectorops.add( vectorops.div( vectorops.add(bb[1], bb[0]), 2), [0, 0, 0.02])) back_bb = bb[0][1] target_pos = (face, vectorops.div(vectorops.add(bb[1], bb[0]), 2)) target_prep_pos[1][1] = back_bb - 0.07 target_pos[1][1] = back_bb - 0.04 for i in range(3): target_prep_pos[1][i] = max( min(target_prep_pos[1][i], max_limit[i]), min_limit[i]) target_pos[1][i] = max(min(target_pos[1][i], max_limit[i]), min_limit[i]) else: target_prep_pos = (face, [(bb[1][0] + bb[0][0]) * 0.5, 0.55, 0.66]) y_len = bb[1][1] - bb[0][1] target_pos = (face, [(bb[1][0] + bb[0][0]) * 0.5, 0.92 - y_len, 0.66]) if count > 0 and hand_facing == 'sweep': continue if target_prep_pos[1][ 1] + self.num_pick[idx] * 0.01 < self.target_prep_pos[1][ 1] or self.hand_facing == 'sweep': self.hand_facing = hand_facing self.hand_mode = hand_mode self.target_prep_pos = target_prep_pos self.target_pos = target_pos self.target_id = idx self.num_pick[self.target_id] += 1
def drive(self, qcur, angVel, vel, dt): """Drives the robot by an incremental time step to reach the desired Cartesian (angular/linear) velocities of the links previously specified in start(). Args: qcur (list of floats): The robot's current configuration. angVel (3-vector or list of 3-vectors): the angular velocity of each driven link. Set angVel to None to turn off orientation control of every constraint. angVel[i] to None to turn off orientation control of constraint i. vel (3-vector or list of 3-vectors): the linear velocity of each driven link. Set vel to None to turn off position control of every constraint. Set vel[i] to None to turn off position control of constraint i. dt (float): the time step. Returns: tuple: A pair ``(progress,qnext)``. ``progress`` gives a value in the range [0,1] indicating indicating how far along the nominal drive amount the solver was able to achieve. If the result is < 0, this indicates that the solver failed to make further progress. ``qnext`` is the resulting configuration that should be sent to the robot's controller. For longer moves, you should pass qnext back to this function as qcur. """ if angVel is None: #turn off orientation control if vel is None: #nothing specified return (1.0, qcur) angVel = [None] * len(self.links) else: assert len(angVel) > 0 if not hasattr(angVel[0], '__iter__'): angVel = [angVel] if vel is None: #turn off position control vel = [None] * len(self.links) else: assert len(vel) > 0 if not hasattr(vel[0], '__iter__'): vel = [vel] if len(qcur) != self.robot.numLinks(): raise ValueError( "Invalid size of current configuration, %d != %d" % (len(qcur), self.robot.numLinks())) if len(angVel) != len(self.links): raise ValueError( "Invalid # of angular velocities specified, %d != %d" % (len(angVel), len(self.links))) if len(vel) != len(self.links): raise ValueError( "Invalid # of translational velocities specified, %d != %d" % (len(vel), len(self.links))) anyNonzero = False zeroVec = [0, 0, 0] for (w, v) in zip(angVel, vel): if w is not None and list(w) != zeroVec: anyNonzero = True break if v is not None and list(v) != zeroVec: anyNonzero = True break if not anyNonzero: return (1.0, qcur) qout = [v for v in qcur] #update drive transforms self.robot.setConfig(qcur) robotLinks = [self.robot.link(l) for l in self.links] #limit the joint movement by joint limits and velocity tempqmin, tempqmax = self.robot.getJointLimits() if self.qmin is not None: tempqmin = self.qmin if self.qmax is not None: tempqmax = self.qmax vmax = self.robot.getVelocityLimits() vmin = vectorops.mul(vmax, -1) if self.vmin is not None: vmin = self.vmin if self.vmax is not None: vmax = self.vmax for i in range(self.robot.numLinks()): tempqmin[i] = max(tempqmin[i], qcur[i] + dt * vmin[i]) tempqmax[i] = min(tempqmax[i], qcur[i] + dt * vmax[i]) #Setup the IK solver parameters self.solver.setJointLimits(tempqmin, tempqmax) tempGoals = [ik.IKObjective(g) for g in self.ikGoals] for i in range(len(self.links)): if math.isfinite(self.positionTolerance) and math.isfinite( self.rotationTolerance): tempGoals[i].rotationScale = self.positionTolerance / ( self.positionTolerance + self.rotationTolerance) tempGoals[i].positionScale = self.rotationTolerance / ( self.positionTolerance + self.rotationTolerance) elif not math.isfinite( self.positionTolerance) and not math.isfinite( self.rotationTolerance): pass else: tempGoals[i].rotationScale = min( self.positionTolerance, self.rotationTolerance) / self.rotationTolerance tempGoals[i].positionScale = min( self.positionTolerance, self.rotationTolerance) / self.positionTolerance tolerance = min( 1e-6, min(self.positionTolerance, self.rotationTolerance) / (math.sqrt(3.0) * len(self.links))) self.solver.setTolerance(tolerance) #want to solve max_t s.t. there exists q satisfying T_i(q) = TPath_i(t) #where TPath_i(t) = Advance(Tdrive_i,Screw_i*t) if self.driveSpeedAdjustment == 0: self.driveSpeedAdjustment = 0.1 anyFailures = False while self.driveSpeedAdjustment > 0: #advance the desired cartesian goals #set up IK parameters: active dofs, IKGoal amount = dt * self.driveSpeedAdjustment desiredTransforms = [[None, None] for i in range(len(self.links))] for i in range(len(self.links)): if vel[i] is not None: desiredTransforms[i][1] = vectorops.madd( self.driveTransforms[i][1], vel[i], amount) tempGoals[i].setFixedPosConstraint( self.endEffectorOffsets[i], desiredTransforms[i][1]) else: tempGoals[i].setFreePosition() if angVel[i] is not None: increment = so3.from_moment( vectorops.mul(angVel[i], amount)) desiredTransforms[i][0] = so3.mul( increment, self.driveTransforms[i][0]) tempGoals[i].setFixedRotConstraint(desiredTransforms[i][0]) else: tempGoals[i].setFreeRotConstraint() self.solver.set(i, tempGoals[i]) err0 = self.solver.getResidual() quality0 = vectorops.norm(err0) res = self.solver.solve() q = self.robot.getConfig() activeDofs = self.solver.getActiveDofs() for k in activeDofs: if q[k] < tempqmin[k] or q[k] > tempqmax[k]: #the IK solver normalizer doesn't care about absolute #values for joints that wrap around 2pi if tempqmin[k] <= q[k] + 2 * math.pi and q[ k] + 2 * math.pi <= tempqmax[k]: q[k] += 2 * math.pi elif tempqmin[k] <= q[k] - 2 * math.pi and q[ k] - 2 * math.pi <= tempqmax[k]: q[k] -= 2 * math.pi else: print( "CartesianDriveSolver: Warning, result from IK solve is out of bounds: index", k, ",", tempqmin[k], "<=", q[k], "<=", tempqmax[k]) q[k] = max(min(q[k], tempqmax[k]), tempqmin[k]) self.robot.setConfig(q) #now evaluate quality of the solve errAfter = self.solver.getResidual() qualityAfter = vectorops.norm(errAfter) if qualityAfter > quality0: #print("CartesianDriveSolver: Solve failed: original configuration was better:",quality0,"vs",qualityAfter) #print(" solver result was",res,"increment",amount) res = False elif qualityAfter < quality0 - 1e-8: res = True if res: #success! for k in activeDofs: qout[k] = q[k] assert tempqmin[k] <= q[k] and q[k] <= tempqmax[k] #now advance the driven transforms #figure out how much to drive along screw numerator = 0 # this will get sum of distance * screws denominator = 0 # this will get sum of |screw|^2 for all screws #result will be numerator / denominator achievedTransforms = [ (l.getTransform()[0], l.getWorldPosition(ofs)) for l, ofs in zip(robotLinks, self.endEffectorOffsets) ] #TODO: get transforms relative to baseLink for i in range(len(self.links)): if vel[i] is not None: trel = vectorops.sub(achievedTransforms[i][1], self.driveTransforms[i][1]) vellen = vectorops.norm(vel[i]) axis = vectorops.div(vel[i], max(vellen, 1e-8)) ut = vellen tdistance = vectorops.dot(trel, axis) tdistance = min(max(tdistance, 0.0), dt * vellen) numerator += ut * tdistance denominator += ut**2 if angVel[i] is not None: Rrel = so3.mul(achievedTransforms[i][0], so3.inv(self.driveTransforms[i][0])) vellen = vectorops.norm(angVel[i]) angVelRel = so3.apply( so3.inv(self.driveTransforms[i][0]), angVel[i]) rotaxis = vectorops.div(angVelRel, max(vellen, 1e-8)) Rdistance = axis_rotation_magnitude(Rrel, rotaxis) Rdistance = min(max(Rdistance, 0.0), dt * vellen) uR = vellen numerator += uR * Rdistance denominator += uR**2 distance = numerator / max(denominator, 1e-8) #computed error-minimizing distance along screw motion for i in range(len(self.links)): if vel[i] is not None: self.driveTransforms[i][1] = vectorops.madd( self.driveTransforms[i][1], vel[i], distance) else: self.driveTransforms[i][1] = achievedTransforms[i][1] if angVel[i] is not None: Rincrement = so3.from_moment( vectorops.mul(angVel[i], distance)) self.driveTransforms[i][0] = normalize_rotation( so3.mul(Rincrement, self.driveTransforms[i][0])) else: self.driveTransforms[i][0] = achievedTransforms[i][0] if math.ceil(distance / dt * 10) < math.floor( self.driveSpeedAdjustment * 10): self.driveSpeedAdjustment -= 0.1 self.driveSpeedAdjustment = max(self.driveSpeedAdjustment, 0.0) elif not anyFailures: #increase drive velocity if self.driveSpeedAdjustment < 1.0: self.driveSpeedAdjustment += 0.1 self.driveSpeedAdjustment = min( 1.0, self.driveSpeedAdjustment) return (distance / dt, qout) else: #IK failed: try again with a slower speed adjustment anyFailures = True self.driveSpeedAdjustment -= 0.1 if self.driveSpeedAdjustment <= 1e-8: self.driveSpeedAdjustment = 0.0 break #no progress return (-1, qcur)
def transform_average(Ts): t = vectorops.div(vectorops.add(*[T[1] for T in Ts]), len(Ts)) qs = [so3.quaternion(T[0]) for T in Ts] q = vectorops.div(vectorops.add(*qs), len(Ts)) return (so3.from_quaternion(q), t)
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 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 onMessage(self, msg): #print "Getting haptic message" #print msg self.numMessages += 1 if msg['type'] != 'MultiHapticState': print "Strange message type", msg['type'] return if len(self.deviceState) == 0: print "Adding", len(msg['devices']) - len( self.deviceState), "haptic devices" while len(self.deviceState) < len(msg['devices']): self.deviceState.append(HapticDeviceState()) if len(msg['devices']) != len(self.deviceState): print "Incorrect number of devices in message:", len( msg['devices']) return #read button presses for dstate in self.deviceState: dstate.buttonPressed[0] = dstate.buttonPressed[1] = False dstate.buttonReleased[0] = dstate.buttonReleased[1] = False if 'events' in msg: for e in msg['events']: #print e['type'] if e['type'] == 'b1_down': dstate = self.deviceState[e['device']] dstate.buttonPressed[0] = True elif e['type'] == 'b2_down': dstate = self.deviceState[e['device']] dstate.buttonPressed[1] = True elif e['type'] == 'b1_up': dstate = self.deviceState[e['device']] dstate.buttonReleased[0] = True elif e['type'] == 'b2_up': dstate = self.deviceState[e['device']] dstate.buttonReleased[1] = True for i in range(len(self.deviceState)): dmsg = msg['devices'][i] dstate = self.deviceState[i] if dstate.time == dmsg['time']: #no update... continue dstate.newupdate = True # ===== read from msg ===== dstate.position = dmsg['position'] dstate.rotationMoment = dmsg['rotationMoment'] dstate.velocity = dmsg['velocity'] dstate.angularVelocity = dmsg['angularVelocity'] dstate.jointAngles = dmsg['jointAngles'] #dstate.gimbalAngle = dmsg['gimbalAngle'] oldTime = dstate.time dstate.time = dmsg['time'] #print "position",dmsg['position'] #print "rotation moment",dmsg['rotationMoment'] #print "angular velocity",dmsg['angularVelocity'] dstate.widgetCurrentTransform = deviceToWidgetTransform( so3.from_moment(dmsg['rotationMoment']), dmsg['position']) if dmsg['button1'] == 1: oldButtonDown = dstate.buttonDown[0] dstate.buttonDown[0] = True if not oldButtonDown: dstate.widgetInitialTransform[ 0] = dstate.widgetPreviousTransform continue else: dstate.buttonDown[0] = False if dmsg['button2'] == 1: oldButtonDown = dstate.buttonDown[1] dstate.buttonDown[1] = True if not oldButtonDown: dstate.widgetInitialTransform[ 1] = dstate.widgetPreviousTransform continue else: dstate.buttonDown[1] = False newTime = dstate.time if newTime != oldTime and dstate.widgetPreviousTransform is not None: # print "previous position = ", dstate.widgetPreviousTransform[1] # print "current position = ", dstate.widgetCurrentTransform[1] timeInterval = newTime - oldTime #print "========================" #print "time = ", timeInterval delta_Pos = vectorops.sub(dstate.widgetCurrentTransform[1], dstate.widgetPreviousTransform[1]) vel = vectorops.div(delta_Pos, timeInterval) delta_Moment = so3.moment( so3.mul(dstate.widgetCurrentTransform[0], so3.inv(dstate.widgetPreviousTransform[0]))) angvel = vectorops.div(delta_Moment, timeInterval) # print "vel = [%2.4f %2.4f %2.4f]" % (vel[0], vel[1], vel[2]) # print "angvel = [%2.4f %2.4f %2.4f]" % (angvel[0], angvel[1], angvel[2]) dstate.linearVel = list(vel) dstate.angularVel = list(angvel) #end of loop, store previous transform dstate.widgetPreviousTransform = dstate.widgetCurrentTransform