def scaleToBounds(val, bmin, bmax, origin=None): """Cap the ray starting origin in the direction val against bounding box limits. """ if origin != None: if isinstance(bmin, np.ndarray): bmin -= origin else: bmin = vectorops.sub(bmin, origin) if isinstance(bmax, np.ndarray): bmax -= origin else: bmax = vectorops.sub(bmax, origin) return scaleToBounds(val, bmin, bmax) umax = 1.0 for vali, ai, bi in zip(val, bmin, bmax): assert bi >= ai if vali < ai: umax = ai / vali if vali > bi: umax = bi / vali if umax == 1.0: return val assert umax >= 0 print "Scaling to velocity maximum, factor", umax if isinstance(val, np.ndarray): return val * umax else: return vectorops.mul(val, umax)
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 insideGeometryBB(self, point, bb, bbMargin): #t1 = time.time() mins = vectorops.sub(bb[0], bbMargin) maxes = vectorops.sub(bb[1], -bbMargin) #print "bounding box checked first two lines in time: ",time.time()-t1 insideX = point[0] >= mins[0] and point[0] <= maxes[0] insideY = point[1] >= mins[1] and point[1] <= maxes[1] insideZ = point[2] >= mins[2] and point[2] <= maxes[2] #print "bounding box checked in time: ",time.time()-t1 #print maxes[0], maxes[1] #print "point:", point[0], point[1] return insideX and insideY and insideZ
def point_fit_xform_3d(apts,bpts): """Finds a 3D rigid transform that maps the list of points apts to the list of points bpts. Return value is a klampt.se3 element that minimizes the sum of squared errors ||T*ai-bi||^2. """ assert len(apts)==len(bpts) ca = vectorops.div(reduce(apts,vectorops.add),len(apts)) cb = vectorops.div(reduce(bpts,vectorops.add),len(bpts)) arel = [vectorops.sub(a,ca) for a in apts] brel = [vectorops.sub(b,cb) for b in bpts] R = point_fit_rotation_3d(arel,brel) #R minimizes sum_{i=1,...,n} ||R(ai-ca) - (bi-cb)||^2 t = so3.sub(cb,so3.apply(R,ca)) return (R,t)
def drawGL(self): #draw tendons glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) glLineWidth(4.0) glColor3f(0, 1, 1) glBegin(GL_LINES) for i in range(3): b1 = self.sim.body( self.model.robot.link(self.model.proximal_links[i])) b2 = self.sim.body( self.model.robot.link(self.model.distal_links[i])) glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local)) glVertex3f(*se3.apply(b2.getTransform(), self.tendon2_local)) if self.ext_forces[i]: glColor3f(0, 1, 0) b = self.sim.body( self.model.robot.link(self.model.proximal_links[i])) print "Applying ext force", self.EXT_F, "to", i glVertex3f(*se3.apply(b.getTransform(), self.EXT_F_DISP)) glVertex3f( *se3.apply(b.getTransform(), vectorops.sub(self.EXT_F_DISP, self.EXT_F))) glEnd() glLineWidth(1) glEnable(GL_DEPTH_TEST) glEnable(GL_LIGHTING)
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.getLink(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 taskDifference(self, a, b): if self.taskType == 'po': return se3.error(a, b) elif self.taskType == 'position': return vectorops.sub(a, b) elif self.taskType == 'orientation': return so3.error(a, b) else: raise ValueError("Invalid taskType " + self.taskType)
def apply_tendon_forces(self, i, link1, link2, rest_length): tendon_c2 = 30000.0 tendon_c1 = 10000.0 b0 = self.sim.body( self.model.robot.link(self.model.proximal_links[0] - 3)) b1 = self.sim.body(self.model.robot.link(link1)) b2 = self.sim.body(self.model.robot.link(link2)) p0w = se3.apply(b1.getTransform(), self.tendon0_local) p1w = se3.apply(b1.getTransform(), self.tendon1_local) p2w = se3.apply(b2.getTransform(), self.tendon2_local) d = vectorops.distance(p1w, p2w) if d > rest_length: #apply tendon force direction = vectorops.unit(vectorops.sub(p2w, p1w)) f = tendon_c2 * (d - rest_length)**2 + tendon_c1 * (d - rest_length) #print d,rest_length #print "Force magnitude",self.model.robot.link(link1).getName(),":",f f = min(f, 100) #tendon routing force straight = vectorops.unit(vectorops.sub(p2w, p0w)) pulley_direction = vectorops.unit(vectorops.sub(p1w, p0w)) pulley_axis = so3.apply(b1.getTransform()[0], (0, 1, 0)) tangential_axis = vectorops.cross(pulley_axis, pulley_direction) cosangle = vectorops.dot(straight, tangential_axis) #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle base_direction = so3.apply(b0.getTransform()[0], [0, 0, -1]) b0.applyForceAtLocalPoint( vectorops.mul(base_direction, -f), vectorops.madd(p0w, base_direction, 0.04)) b1.applyForceAtLocalPoint( vectorops.mul(tangential_axis, cosangle * f), self.tendon1_local) b1.applyForceAtLocalPoint( vectorops.mul(tangential_axis, -cosangle * f), self.tendon0_local) b2.applyForceAtLocalPoint(vectorops.mul(direction, -f), self.tendon2_local) self.forces[i][1] = vectorops.mul(tangential_axis, cosangle * f) self.forces[i][2] = vectorops.mul(direction, f) else: self.forces[i] = [None, None, None] return
def sphere_collision_tests(x, radius, grid): bmin = vectorops.sub(x, [radius] * len(x)) bmax = vectorops.add(x, [radius] * len(x)) imin = [max(0, int(v)) for v in bmin] imax = [min(int(v), b - 1) for v, b in zip(bmax, workspace_size)] for i in range(imin[0], imax[0]): for j in range(imin[1], imax[1]): for k in range(imin[2], imax[2]): if vectorops.distanceSquared((i, j, k), x) < radius**2: yield (i, j, k) return
def control_loop(self): sim = self.sim world = self.world controller = sim.getController(0) robotModel = world.robot(0) t = self.ttotal #Problem 1: tune the values in this line if problem == "1a" or problem == "1b": kP = 1 kI = 1 kD = 1 controller.setPIDGains([kP], [kI], [kD]) #Problem 2: use setTorque to implement a control loop with feedforward #torques if problem == "2a" or problem == "2b": qcur = controller.getSensedConfig() dqcur = controller.getSensedVelocity() qdes = [0] dqdes = [0] robotModel.setConfig(qcur) robotModel.setVelocity(dqcur) while qcur[0] - qdes[0] > math.pi: qcur[0] -= 2 * math.pi while qcur[0] - qdes[0] < -math.pi: qcur[0] += 2 * math.pi ddq = vectorops.mul(vectorops.sub(qdes, qcur), 100.0) ddq = vectorops.madd(ddq, vectorops.sub(dqdes, dqcur), 20.0) #TODO: solve the dynamics equation to fill this in T = [3] controller.setTorque(T) #Problem 3: drive the robot so its end effector goes #smoothly toward the target point if problem == "3": target = [1.0, 0.0, 0.0] qdes = [0.7, -2.3] dqdes = [0, 0] controller.setPIDCommand(qdes, dqdes)
def apply_tendon_forces(self,link1,link2,rest_length): tendon_c2 = 30000.0 tendon_c1 = 100000.0 b1 = self.sim.body(self.model.robot.link(link1)) b2 = self.sim.body(self.model.robot.link(link2)) p1w = se3.apply(b1.getTransform(),self.tendon1_local) p2w = se3.apply(b2.getTransform(),self.tendon2_local) d = vectorops.distance(p1w,p2w) if d > rest_length: #apply tendon force direction = vectorops.unit(vectorops.sub(p2w,p1w)) f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length) b1.applyForceAtPoint(vectorops.mul(direction,f),p1w) b2.applyForceAtPoint(vectorops.mul(direction,-f),p2w) else: pass
def keyboardfunc(self, key, x, y): h = 0.932 targets = { 'a': (0.35, 0.25, h), 'b': (0.35, 0.05, h), 'c': (0.35, -0.1, h), 'd': (0.35, -0.1, h) } if key in targets: dest = targets[key] shift = vectorops.sub(dest, self.Tstart[1]) self.robot.setConfig(self.qstart) self.object.setTransform(*self.Tstart) self.hand = Hand('l') #plan transit path to grasp object self.transitPath = planTransit(self.world, 0, self.hand) if self.transitPath: #plan transfer path self.robot.setConfig(self.transitPath[-1]) self.transferPath = planTransfer(self.world, 0, self.hand, shift) if self.transferPath: self.Tgoal = graspedObjectTransform( self.robot, self.hand, self.transferPath[0], self.Tstart, self.transferPath[-1]) #plan free path to retract arm self.robot.setConfig(self.transferPath[-1]) self.object.setTransform(*self.Tgoal) self.retractPath = planFree(self.world, self.hand, self.qstart) #reset the animation self.animationTime = self.ttotal glutPostRedisplay() if key == 'n': print "Moving to next action" if self.transitPath and self.transferPath and self.retractPath: #update start state self.qstart = self.retractPath[-1] self.Tstart = self.Tgoal self.robot.setConfig(self.qstart) self.object.setTransform(*self.Tstart) self.transitPath = None self.transferPath = None self.hand = None glutPostRedisplay()
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 nextState(self, x, u): pos = [x[0], x[1]] fwd = [math.cos(x[2]), math.sin(x[2])] right = [-fwd[1], fwd[0]] phi = u[1] d = u[0] if abs(phi) < 1e-8: newPos = vectorops.madd(pos, fwd, d) return newpos + [x[2]] else: #rotate about a center of rotation, with radius 1/phi cor = vectorops.madd(pos, right, 1.0 / phi) sign = cmp(d * phi, 0) d = abs(d) phi = abs(phi) theta = 0 thetaMax = d * phi newpos = vectorops.add( so2.apply(sign * thetaMax, vectorops.sub(pos, cor)), cor) return newpos + [so2.normalize(x[2] + sign * thetaMax)]
def diff(x, y): return vectorops.sub(x, y)
def difference(self,a,b): """For Lie groups, returns a difference vector that, when integrated would get to a from b. In Cartesian spaces it is a-b.""" return vectorops.sub(a,b)
def getVector(p1, p2): ''' Get vector formed by two points ''' return vectorops.sub(p1, p2)
def interpolate_linear(a, b, u): """Interpolates linearly in cartesian space between a and b.""" return vectorops.madd(a, vectorops.sub(b, a), u)
def onMessage(self, msg): global deviceState, defaultDeviceState #print "Getting haptic message" #print msg if msg['type'] != 'MultiHapticState': print "Strange message type", msg['type'] return if len(deviceState) == 0: print "Adding", len( msg['devices']) - len(deviceState), "haptic devices" while len(deviceState) < len(msg['devices']): deviceState.append(defaultDeviceState.copy()) if len(msg['devices']) != len(deviceState): print "Incorrect number of devices in message:", len( msg['devices']) return #change overall state depending on button 2 if 'events' in msg: for e in msg['events']: #print e['type'] if e['type'] == 'b2_down': dstate = deviceState[e['device']] toggleMode(dstate) print 'Changing device', e['device'], 'to state', dstate[ 'mode'] for i in range(len(deviceState)): dmsg = msg['devices'][i] dstate = deviceState[i] if dmsg['button1'] == 1: #drag widget if button 1 is down oldButtonDown = dstate['buttonDown'] dstate['buttonDown'] = True #moving if dstate['mode'] == 'normal': if not oldButtonDown: dstate['moveStartDeviceTransform'] = (so3.from_moment( dmsg['rotationMoment']), dmsg['position']) if self.widgetGetters == None: dstate['moveStartWidgetTransform'] = dstate[ 'widgetTransform'] else: Twidget = self.widgetGetters[i]() if Twidget != None: dstate['moveStartWidgetTransform'] = Twidget else: dstate['moveStartWidgetTransform'] = dstate[ 'widgetTransform'] #================================================================================================ # #perform transformation of widget # deviceTransform = (so3.from_moment(dmsg['rotationMoment']),dmsg['position']) # #compute relative motion # Tdev = se3.mul(deviceToWidgetTransform,deviceTransform) # TdevStart = se3.mul(deviceToWidgetTransform,dstate['moveStartDeviceTransform']) # relDeviceTransform = (so3.mul(Tdev[0],so3.inv(TdevStart[0])),vectorops.sub(Tdev[1],TdevStart[1])) # #scale relative motion # Rrel,trel = relDeviceTransform # wrel = so3.moment(Rrel) # wrel = vectorops.mul(wrel,dstate['rotationScale']) # trel = vectorops.mul(trel,dstate['positionScale']) # #print "Moving",trel,"rotating",wrel # relDeviceTransform = (so3.from_moment(wrel),trel) # #perform transform # dstate['widgetTransform'] = se3.mul(dstate['moveStartWidgetTransform'],relDeviceTransform) #================================================================================================ #- perform transformation of widget deviceTransform = (so3.from_moment(dmsg['rotationMoment']), dmsg['position']) # delta_device = vectorops.sub(deviceTransform[1],dstate['moveStartDeviceTransform'][1]) # print "haptic moves: ", delta_device delta_device_R_matrix = so3.mul( deviceTransform[0], so3.inv(dstate['moveStartDeviceTransform'][0])) # delta_device_R = so3.moment(delta_device_R_matrix) # norm_delta_R = vectorops.norm(delta_device_R) # if norm_delta_R != 0: # vec_delta_R = vectorops.div(delta_device_R, norm_delta_R) # else: # vec_delta_R = [0,0,0] # # print "haptic rotate: norm = ", norm_delta_R, ", vec = ", vec_delta_R #- compute relative motion Tdev = se3.mul(deviceToBaxterBaseTransform, deviceTransform) TdevStart = se3.mul(deviceToBaxterBaseTransform, dstate['moveStartDeviceTransform']) # delta_widget = vectorops.sub(Tdev[1],TdevStart[1]) # print "Baxter moves: ", delta_widget # delta_widget_R_matrix = so3.mul(Tdev[0],so3.inv(TdevStart[0])) # delta_widget_R = so3.moment(delta_widget_R_matrix) # norm_widget_R = vectorops.norm(delta_widget_R) # if norm_widget_R != 0: # vec_widget_R = vectorops.div(delta_widget_R, norm_widget_R) # else: # vec_widget_R = [0,0,0] # print "Baxter rotate: norm = ", norm_widget_R, ", vec = ", vec_widget_R relDeviceTransform = (so3.mul(Tdev[0], so3.inv(TdevStart[0])), vectorops.sub(Tdev[1], TdevStart[1])) #- scale relative motion Rrel, trel = relDeviceTransform wrel = so3.moment(Rrel) wrel = vectorops.mul(wrel, dstate['rotationScale']) trel = vectorops.mul(trel, dstate['positionScale']) #- print "Moving",trel,"rotating",wrel relDeviceTransform = (so3.from_moment(wrel), trel) #- perform transform # dstate['widgetTransform'] = se3.mul(dstate['moveStartWidgetTransform'],relDeviceTransform) dstate['widgetTransform'] = ( so3.mul(dstate['moveStartWidgetTransform'][0], relDeviceTransform[0]), vectorops.add(dstate['moveStartWidgetTransform'][1], relDeviceTransform[1])) #================================================================================================ elif dstate['mode'] == 'scaling': if not oldButtonDown: dstate['moveStartDeviceTransform'] = (so3.from_moment( dmsg['rotationMoment']), dmsg['position']) #perform scaling deviceTransform = (so3.from_moment(dmsg['rotationMoment']), dmsg['position']) oldDeviceTransform = dstate['moveStartDeviceTransform'] znew = deviceTransform[1][1] zold = oldDeviceTransform[1][1] posscalerange = [1e-2, 20] rotscalerange = [0.5, 2] newposscale = min( max( dstate['positionScale'] * math.exp( (znew - zold) * 10), posscalerange[0]), posscalerange[1]) newrotscale = min( max( dstate['rotationScale'] * math.exp( (znew - zold) * 4), rotscalerange[0]), rotscalerange[1]) if any(newposscale == v for v in posscalerange) or any( newrotscale == v for v in rotscalerange): pass #dont change the scale else: dstate['positionScale'] = newposscale dstate['rotationScale'] = newrotscale print "Setting position scale to", dstate[ 'positionScale'], "rotation scale to", dstate[ 'rotationScale'] #update start device transform dstate['moveStartDeviceTransform'] = deviceTransform elif dstate['mode'] == 'gripper': print "Gripper mode not available" dstate['mode'] = 'normal' dstate['buttonDown'] = False else: dstate['buttonDown'] = False if self.viewer: self.viewer.update(deviceState) else: print "No viewer..."
def interpolate(a, b, u): """Interpolates linearly between a and b""" return vectorops.madd(a, vectorops.sub(b, a), u)
def taskDifference(self, a, b): """Default: assumes a Cartesian space""" return vectorops.sub(a, b)