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 drawContactForces(self): contacted=False; for i in range(self.simworld.numIDs()): for j in range(i+1, self.simworld.numIDs()): if(self.sim.hadContact(i,j)): if(not contacted): # print "Touching bodies:\n" contacted=True f = self.sim.meanContactForce(i,j) contacts = self.sim.getContacts(i,j) # print self.simworld.getName(i),self.simworld.getName(j), len(contacts) x=[0,0,0] n=[0,0,0] k=0 scale = 5 if len(contacts) != 0: for numContact in range(len(contacts)): x = vectorops.add(x,contacts[numContact][0:3]) n = vectorops.add(n,contacts[numContact][3:6]) k += contacts[numContact][6] x = vectorops.div(x,len(contacts)) n = vectorops.div(n,len(contacts)) k = k/(len(contacts)*scale) draw_vector(x,n,k)
def moveRobot(self, movement=None): print 'Moving robot' global LEFT_ARM_INDICES global RIGHT_ARM_INDICES joint_ones = ['1', '!'] joint_twos = ['2', '@'] joint_threes = ['3', '#'] joint_fours = ['4', '$'] joint_fives = ['5', '%'] joint_sixes = ['6', '^'] joint_sevens = ['7', '&'] plus = ['1','2','3','4','5','6','7'] minus = ['!','@','#','$','%','^','&'] index = 0 factor = 0 adjustment = [0,0,0,0,0,0,0] if movement in joint_ones: index = 0 elif movement in joint_twos: index = 1 elif movement in joint_threes: index = 2 elif movement in joint_fours: index = 3 elif movement in joint_fives: index = 4 elif movement in joint_sixes: index = 5 elif movement in joint_sevens: index = 6 if movement in plus: factor = 1 elif movement in minus: factor = -1 adjustment[index] = factor*self.degreeChange*math.pi/180.0 currentSetup = self.controller.controller.getCommandedConfig() if self.currentLimb == LEFT: leftMilestone = [currentSetup[v] for v in LEFT_ARM_INDICES] leftMilestone = vectorops.add(leftMilestone, adjustment) adjustedMilestone = [currentSetup[i] for i in range(len(currentSetup))] for i in range(len(LEFT_ARM_INDICES)): adjustedMilestone[LEFT_ARM_INDICES[i]] = leftMilestone[i] self.controller.controller.setLinear(adjustedMilestone, .1) elif self.currentLimb == RIGHT: rightMilestone = [currentSetup[v] for v in RIGHT_ARM_INDICES] rightMilestone = vectorops.add(rightMilestone, adjustment) adjustedMilestone = [currentSetup[i] for i in range(len(currentSetup))] for i in range(len(RIGHT_ARM_INDICES)): adjustedMilestone[RIGHT_ARM_INDICES[i]] = rightMilestone[i] self.controller.controller.setLinear(adjustedMilestone, .1) return
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 right_arm_ik_near_seed(self, right_target, ignore_elbow_up_constraint=True): """Solves IK to move the right arm to the specified right_target ([x, y, z] in world space) """ self.load_real_robot_state() self.set_model_right_arm(eval("Q_IK_SEED_" + self.current_bin)) oldRSquared = -1 q_ik = None qmin, qmax = self.robotModel.getJointLimits() for i in range(1000): point2_local = vectorops.add(VACUUM_POINT_XFORM[1], [0.5, 0, 0]) point2_world = vectorops.add(right_target, [0, 0, -0.5]) goal1 = ik.objective(self.robotModel.link("right_wrist"), local=VACUUM_POINT_XFORM[1], world=right_target) goal2 = ik.objective(self.robotModel.link("right_wrist"), local=point2_local, world=point2_world) if ik.solve([goal1, goal2], tol=0.0001) and (self.elbow_up() or ignore_elbow_up_constraint): q_vals = [self.robotModel.getConfig()[v] for v in self.right_arm_indices] rSquared = vectorops.norm(vectorops.sub(q_vals, eval("Q_IK_SEED_" + self.current_bin))) if oldRSquared < 0 or oldRSquared > rSquared: oldRSquared = rSquared q_ik = q_vals print FAIL_COLOR + "right_arm_ik failed for " + str(right_target) + END_COLOR if not (self.elbow_up() or ignore_elbow_up_constraint): print FAIL_COLOR + str([self.robotModel.getConfig()[v] for v in self.right_arm_indices]) + END_COLOR print FAIL_COLOR + "IK found but elbow wasn't up" + END_COLOR if oldRSquared >= 0: self.set_model_right_arm(q_ik) return True return False
def saveStep(self,extra=[]): sim = self.sim world = sim.world sim.updateWorld() values = [] values.append(sim.getTime()) for i in xrange(world.numRobots()): robot = world.robot(i) values += robot.getCom() values += robot.getConfig() values += sim.getActualTorques(i) """ j = 0 while True: s = self.sim.controller(i).getSensor(j) if len(s.name())==0: break meas = s.measurements() values += meas j += 1 """ for i in xrange(world.numRigidObjects()): obj = world.rigidObject(i) T = obj.getTransform() values += se3.apply(T,obj.getMass().getCom()) values += T[1] values += so3.moment(T) values += sim.body(obj).getVelocity()[1] values += sim.body(obj).getVelocity()[0] if self.f_contact: for i,id in enumerate(self.colliding): for j in range(i+1,len(self.colliding)): id2 = self.colliding[j] if sim.hadContact(id,id2): clist = sim.getContacts(id,id2); f = sim.contactForce(id,id2) m = sim.contactTorque(id,id2) pavg = [0.0]*3 navg = [0.0]*3 for c in clist: pavg = vectorops.add(pavg,c[0:3]) navg = vectorops.add(navg,c[3:6]) if len(clist) > 0: pavg = vectorops.div(pavg,len(clist)) navg = vectorops.div(navg,len(clist)) body1 = world.getName(id) body2 = world.getName(id2) values = [sim.getTime(),body1,body2,len(clist)] values += pavg values += navg values += f values += m self.f_contact.write(','.join(str(v) for v in values)) self.f_contact.write('\n') if extra: values += extra self.f.write(','.join([str(v) for v in values])) self.f.write('\n')
def saveStep(self, extra=[]): sim = self.sim world = sim.world sim.updateWorld() values = [] values.append(sim.getTime()) for i in xrange(world.numRobots()): robot = world.robot(i) values += robot.getCom() values += robot.getConfig() values += robot.getVelocity() values += sim.getActualTorques(i) j = 0 while True: s = self.sim.controller(i).sensor(j) if s is None or len(s.name()) == 0: break meas = s.getMeasurements() values += meas j += 1 for i in xrange(world.numRigidObjects()): obj = world.rigidObject(i) T = obj.getTransform() values += se3.apply(T, obj.getMass().getCom()) values += T[1] values += so3.moment(T[0]) values += sim.body(obj).getVelocity()[1] values += sim.body(obj).getVelocity()[0] if self.f_contact: for i, id in enumerate(self.colliding): for j in range(i + 1, len(self.colliding)): id2 = self.colliding[j] if sim.hadContact(id, id2): clist = sim.getContacts(id, id2) f = sim.contactForce(id, id2) m = sim.contactTorque(id, id2) pavg = [0.0] * 3 navg = [0.0] * 3 for c in clist: pavg = vectorops.add(pavg, c[0:3]) navg = vectorops.add(navg, c[3:6]) if len(clist) > 0: pavg = vectorops.div(pavg, len(clist)) navg = vectorops.div(navg, len(clist)) body1 = world.getName(id) body2 = world.getName(id2) cvalues = [sim.getTime(), body1, body2, len(clist)] cvalues += pavg cvalues += navg cvalues += f cvalues += m self.f_contact.write(','.join(str(v) for v in cvalues)) self.f_contact.write('\n') if extra: values += extra if not (self.f is None): self.f.write(','.join([str(v) for v in values])) self.f.write('\n')
def sendPath(path,maxSmoothIters =0, INCREMENTAL=False, limb = None, readConstants=False, internalSpeed=SPEED): # interpolate path linearly between two endpoints if path == None: print "sending empty path" return False for smoothIter in range(maxSmoothIters): # path = path smoothePath = [0]*(len(path)*2-1) for i in range(len(path)-1): smoothePath[i*2] = path[i] smoothePath[i*2 +1] = vectorops.div(vectorops.add(path[i],path[i+1]), 2) smoothePath[-1] = path[-1] path = smoothePath while i <endIndex-1: # print i, endIndex q = path[i] qNext = path[i+1] dt = vectorops.distance(q,qNext) # smooth trajectory by interpolating between two consecutive configurations # if the distance between the two is big # Note: might want to make dt bigger for arm movements (only 7 configurations vs 52) if dt>0.1: qInterp = vectorops.div(vectorops.add(q, qNext), 2) path.insert(i+1, qInterp) endIndex +=1 continue else: i += 1 waitForMove() if counter%internalSpeed == 0 or INCREMENTAL: if limb == 'left': #CONTROLLER.appendMilestoneLeft(q) pass elif limb == 'right': #CONTROLLER.appendMilestoneRight(q) pass else: #print 'milestone #', i, q #CONTROLLER.appendMilestone(q) pass counter +=1 if limb == 'left': #CONTROLLER.appendMilestoneLeft(path[-1]) pass elif limb == 'right': #CONTROLLER.appendMilestoneRight(path[-1]) pass else: pass
def solve_3R_forward_kinematics(q1,q2,q3,L1=1,L2=1,L3=1): """Returns a list of (x,y,theta) triples for each link for a planar, 3R manipulator with link lengths L1, L2, L3. It also returns the end effector transform.""" T1 = (0,0,q1) dx1 = (L1*math.cos(T1[2]),L1*math.sin(T1[2])) T2 = vectorops.add((T1[0],T1[1]),dx1)+[T1[2]+q2] dx2 = (L2*math.cos(T2[2]),L2*math.sin(T2[2])) T3 = vectorops.add((T2[0],T2[1]),dx2)+[T2[2]+q3] dx3 = (L3*math.cos(T3[2]),L2*math.sin(T3[2])) T4 = vectorops.add((T3[0],T3[1]),dx3)+[T3[2]] return [T1,T2,T3,T4]
def solve_3R_forward_kinematics(q1, q2, q3, L1=1, L2=1, L3=1): """Returns a list of (x,y,theta) triples for each link for a planar, 3R manipulator with link lengths L1, L2, L3. It also returns the end effector transform.""" T1 = (0, 0, q1) dx1 = (L1 * math.cos(T1[2]), L1 * math.sin(T1[2])) T2 = vectorops.add((T1[0], T1[1]), dx1) + [T1[2] + q2] dx2 = (L2 * math.cos(T2[2]), L2 * math.sin(T2[2])) T3 = vectorops.add((T2[0], T2[1]), dx2) + [T2[2] + q3] dx3 = (L3 * math.cos(T3[2]), L2 * math.sin(T3[2])) T4 = vectorops.add((T3[0], T3[1]), dx3) + [T3[2]] return [T1, T2, T3, T4]
def putSensedObjectInWorld(self,item): #put this obejct in the world obj = self.world.makeRigidObject(item.info.name) bmin,bmax = item.info.bmin,item.info.bmax center = vectorops.div(vectorops.add(bmin,bmax),2.0) m = obj.getMass() m.setMass(item.info.mass) m.setCom([0,0,0]) m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],item.info.mass/12.0)) obj.setMass(m) c = obj.getContactParameters() c.kFriction = 0.6 c.kRestitution = 0.1; c.kStiffness = 100000 c.kDamping = 100000 obj.setContactParameters(c) cube = obj.geometry() if not cube.loadFile(os.path.join(model_dir,"cube.tri")): print "Error loading cube file",os.path.join(model_dir,"cube.tri") exit(1) scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]] translate = vectorops.sub(bmin,center) cube.transform(scale,translate) mesh = cube.getTriangleMesh() obj.setTransform(item.xform[0],item.xform[1])
def spawn_item_in_world(item,world): """Given an ItemInBin instance, spawns a RigidObject in the Klamp't world with its same geometry / size / mass properties. Returns the new object.""" obj = world.makeRigidObject(item.info.name) bmin,bmax = item.info.bmin,item.info.bmax center = vectorops.div(vectorops.add(bmin,bmax),2.0) m = obj.getMass() m.setMass(item.info.mass) m.setCom([0,0,0]) m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],item.info.mass/12.0)) obj.setMass(m) c = obj.getContactParameters() #TODO: make these parameters dynamic c.kFriction = 0.6 c.kRestitution = 0.1; c.kStiffness = 100000 c.kDamping = 100000 obj.setContactParameters(c) simgeometry = obj.geometry() #either copy directly (this line)... simgeometry.set(item.info.geometry) #or reload from file (this line) #load_item_geometry(item.info,simgeometry) obj.setTransform(item.xform[0],item.xform[1]) return obj
def load_item_geometry(item,geometry_ptr = None): """Loads the geometry of the given item and returns it. If geometry_ptr is provided, then it is assumed to be a Geometry3D object and the object geometry is loaded into it.""" if geometry_ptr == None: geometry_ptr = Geometry3D() if item.info.geometryFile == None: return None elif item.info.geometryFile == 'box': fn = "../klampt_models/cube.tri" if not geometry_ptr.loadFile(fn): print "Error loading cube file",fn exit(1) bmin,bmax = item.info.bmin,item.info.bmax center = vectorops.mul(vectorops.add(bmin,bmax),0.5) scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]] translate = vectorops.sub(bmin,center) geometry_ptr.transform(scale,translate) geometry_ptr.setCurrentTransform(item.xform[0],item.xform[1]) return geometry_ptr else: if not geometry_ptr.loadFile(item.info.geometryFile): print "Error loading geometry file",item.info.geometryFile exit(1) return geometry_ptr
def spawn_objects_from_ground_truth(world): """For all ground_truth_items, spawns RigidObjects in the world according to their sizes / mass properties""" global ground_truth_items print "Initializing world objects" for item in ground_truth_items: obj = world.makeRigidObject(item.info.name) bmin,bmax = item.info.bmin,item.info.bmax center = vectorops.div(vectorops.add(bmin,bmax),2.0) m = obj.getMass() m.setMass(item.info.mass) m.setCom([0,0,0]) m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],item.info.mass/12.0)) obj.setMass(m) c = obj.getContactParameters() c.kFriction = 0.6 c.kRestitution = 0.1; c.kStiffness = 100000 c.kDamping = 100000 obj.setContactParameters(c) cube = obj.geometry() if not cube.loadFile(os.path.join(model_dir,"cube.tri")): print "Error loading cube file",os.path.join(model_dir,"cube.tri") exit(1) scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]] translate = vectorops.sub(bmin,center) cube.transform(scale,translate) mesh = cube.getTriangleMesh() obj.setTransform(item.xform[0],item.xform[1]) return
def update_world_with_knowledge(self): '''Create objects in the planning for those added to the knowledge base.''' # look at all the bins for (name, contents) in self.knowledge.bin_contents.items(): # only care above perceived for full bins if contents: # look at all the items in the bin for item in contents: # check which ones are missing planning objects if item.klampt_index is None: # create an object for it # code borrowed from spawn_objects_from_ground_truth obj = self.world.makeRigidObject(item.info.name) bmin,bmax = item.info.bmin,item.info.bmax center = vectorops.div(vectorops.add(bmin,bmax),2.0) cube = obj.geometry() if not cube.loadFile(os.path.join(model_dir,"cube.tri")): print "Error loading cube file",os.path.join(model_dir,"cube.tri") exit(1) scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]] translate = vectorops.sub(bmin,center) cube.transform(scale,translate) obj.setTransform(item.xform[0],item.xform[1]) # note this item's corresponding object item.klampt_index = obj.getID() # update the collider self.collider = WorldCollider(self.world) print 'spawned planning model', obj.getName()
def move_gripper_upright(self,limb,moveVector,collisionchecker = None): vertical = [0,0,0.1] if self.active_limb == 'left': gripperlink = self.left_gripper_link else: gripperlink = self.right_gripper_link qcur = self.robot.getConfig() vertical_in_gripper_frame = so3.apply(so3.inv(gripperlink.getTransform()[0]),vertical) centerpos = se3.mul(gripperlink.getTransform(),left_gripper_center_xform)[1] move_target = vectorops.add(centerpos,moveVector) movegoal = ik.objective(gripperlink,local=[left_gripper_center_xform[1],vectorops.add(left_gripper_center_xform[1],vertical_in_gripper_frame)],world=[move_target,vectorops.add(move_target,vertical)]) sortedSolutions = self.get_ik_solutions([movegoal],[self.active_limb],qcur,validity_checker=collisionchecker,maxResults=1) if len(sortedSolutions) == 0: print "No upright-movement config found" return False self.robot.setConfig(sortedSolutions[0][0]) return True
def reward(self,state,action=None,actionargs={},actionresult=None): """Assesses the reward of the given action""" res = None for r in self.rewards: if r.applicable(state,action,actionargs,actionresult): if res == None: res = r.value(state,action,actionargs,actionresult) else: res = vectorops.add(res,r.value(state,action,actionargs,actionresult)) return res
def right_arm_ik(self, right_target, ignore_elbow_up_constraint=True): """Solves IK to move the right arm to the specified right_target ([x, y, z] in world space) """ self.load_real_robot_state() self.set_model_right_arm(eval('Q_IK_SEED_' + self.current_bin)) qmin,qmax = self.robotModel.getJointLimits() for i in range(1000): point2_local = vectorops.add(VACUUM_POINT_XFORM[1], [.5, 0, 0]) point2_world = vectorops.add(right_target, [0, 0, -.5]) goal1 = ik.objective(self.robotModel.link('right_wrist'),local=VACUUM_POINT_XFORM[1],world=right_target) goal2 = ik.objective(self.robotModel.link('right_wrist'),local=point2_local,world=point2_world) if ik.solve([goal1, goal2],tol=0.0001) and (self.elbow_up() or ignore_elbow_up_constraint): return True print FAIL_COLOR + "right_arm_ik failed for " + str(right_target) + END_COLOR if not (self.elbow_up() or ignore_elbow_up_constraint): print FAIL_COLOR + str([self.robotModel.getConfig()[v] for v in self.right_arm_indices]) + END_COLOR print FAIL_COLOR + "IK found but elbow wasn't up" + END_COLOR return False
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 bin_vantage_point(self,bin_name): #TODO: remove me world_center = self.bin_front_center(bin_name) #20cm offset world_offset = so3.apply(self.shelf_xform[0],[0,0,0.2]) return vectorops.add(world_center,world_offset) #you may want to implement this. Returns the world position of the #vantage point for viewing the bin. The visualizer will draw these points if #'draw_bins' is turned to True. #TODO: return None
def gen_add(a,b): """a and b can be floats, integers, lists of floats/integers, or None. If both are lists, must be of same length""" if a == None: return b if b == None: return a if isinstance(a,(list,tuple)): if isinstance(b,(list,tuple)): return vectorops.add(a,b) else: return [ai + b for ai in a] elif isinstance(b,(list,tuple)): return [bi + a for bi in b] else: return a+b
def updateAllObjectives(self, frameIdx): for i in range(0, len(self.objectives)): obj = self.objectives[i] frame1pos = self.data.getMarkerPosAtFrame( frameIdx - 1, i ) #self.data.getFrame(frameIdx-1).viconMarkers[i].markerFramePos frame2pos = self.data.getMarkerPosAtFrame( frameIdx, i ) #self.data.getFrame(frameIdx).viconMarkers[i].markerFramePos posChange = utils.calcChangeBetwFrames(frame1pos, frame2pos) pos = vectorops.add(obj.globalpos, posChange) obj.globalpos = pos
def planTransfer(world, objectIndex, hand, shift): """Plan a transfer path for the robot given in world, which is currently holding the object indexed by objectIndex in the hand hand. The desired motion should translate the object by shift without rotating the object. """ globals = Globals(world) obj = world.rigidObject(objectIndex) cspace = TransferCSpace(globals, hand, obj) robot = world.robot(0) qmin, qmax = robot.getJointLimits() #get the start config q0 = robot.getConfig() q0arm = [q0[i] for i in hand.armIndices] if not cspace.feasible(q0arm): print "Warning, arm start configuration is infeasible" print "TODO: Complete 2.a to bypass this error" raw_input() return None #TODO: get the ungrasp config using an IK solver qungrasp = None qungrasparm = None print "TODO: Complete 2.b to find a feasible ungrasp config" raw_input() solver = hand.ikSolver(robot, vectorops.add(obj.getTransform()[1], shift), (0, 0, 1)) #plan the transfer path between q0arm and qungrasparm print "Planning transfer motion to ungrasp config..." MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5) planner = MotionPlan(cspace, 'sbl') planner.setEndpoints(q0arm, qungrasparm) #TODO: do the planning print "TODO: Complete 2.c to find a feasible transfer path" raw_input() #lift arm path to whole configuration space path path = [] for qarm in planner.getPath(): path.append(q0[:]) for qi, i in zip(qarm, hand.armIndices): path[-1][i] = qi qpostungrasp = hand.open(qungrasp, 1.0) return path + [qpostungrasp]
def planTransfer(world,objectIndex,hand,shift): """Plan a transfer path for the robot given in world, which is currently holding the object indexed by objectIndex in the hand hand. The desired motion should translate the object by shift without rotating the object. """ globals = Globals(world) obj = world.rigidObject(objectIndex) cspace = TransferCSpace(globals,hand,obj) robot = world.robot(0) qmin,qmax = robot.getJointLimits() #get the start config q0 = robot.getConfig() q0arm = [q0[i] for i in hand.armIndices] if not cspace.feasible(q0arm): print "Warning, arm start configuration is infeasible" print "TODO: Complete 2.a to bypass this error" raw_input() return None #TODO: get the ungrasp config using an IK solver qungrasp = None qungrasparm = None print "TODO: Complete 2.b to find a feasible ungrasp config" raw_input() solver = hand.ikSolver(robot,vectorops.add(obj.getTransform()[1],shift),(0,0,1)) #plan the transfer path between q0arm and qungrasparm print "Planning transfer motion to ungrasp config..." MotionPlan.setOptions(connectionThreshold=5.0,perturbationRadius=0.5) planner = MotionPlan(cspace,'sbl') planner.setEndpoints(q0arm,qungrasparm) #TODO: do the planning print "TODO: Complete 2.c to find a feasible transfer path" raw_input() #lift arm path to whole configuration space path path = [] for qarm in planner.getPath(): path.append(q0[:]) for qi,i in zip(qarm,hand.armIndices): path[-1][i] = qi qpostungrasp = hand.open(qungrasp,1.0) return path + [qpostungrasp]
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 load_world(): world = robotsim.WorldModel() print "Loading reflex hands" # world.loadElement(os.path.join(model_dir,"reflex.rob")) world.loadElement(os.path.join(model_dir,"reflex_col_with_moving_base.rob")) print "Loading plane model..." world.loadElement(os.path.join(model_dir,"plane.env")) R,t = world.robot(0).link(0).getParentTransform() R_reorient = so3.rotation([1,0,0], math.pi/2) offset = (0,0.15,0.1) R = so3.mul(R,R_reorient) t = vectorops.add(t, offset) world.robot(0).link(0).setParentTransform(R,t) for i in range(world.robot(0).numLinks()): world.robot(0).link(i).geometry().setCollisionMargin(0) return world
def bin_vantage_point(self,bin_name): #you may want to implement this. Returns the world position of the #vantage point for viewing the bin. The visualizer will draw these points if #'draw_bins' is turned to True. # determine the vantage point by moving the front center point back from the shelf # start by getting the front center in world coordinates front_center_world = self.bin_front_center(bin_name) if not front_center_world: return None # move back into local coordinates since the local z frame is aligned with the offset direction front_center_local = se3.apply(se3.inv(self.shelf_xform), front_center_world) # now offset the the front center point along z to get the vantage point vantage_point_local = vectorops.add(front_center_local, [ 0, 0, vantage_point_offset ]) # transform back into world coordinates vantage_point_world = se3.apply(self.shelf_xform, vantage_point_local) return vantage_point_world
def add(self,value,weight=1.0): """Accumulates a new value, with an optional weight factor. The weight can either be a float or a list. In the latter case it is a per-element weight.""" assert len(value) == len(self.average) if not hasattr(weight,'__iter__'): weight = [weight]*len(value) newcount = vectorops.add(self.count,weight) oldweight = [a/b for (a,b) in zip(self.count,newcount)] newweight = [1.0/b for b in newcount] #oldaverage = self.average self.average = [(1.0-w)*a + w*b for (a,b,w) in zip(self.average,value,newweight)] #variance = E[vv^T] - E[v]E[v^T] #variance(n) = 1/n sum_{1 to n+1} v_i*v_i^T - average(n)*average(n)^T #variance(n+1) = 1/(n+1)sum_{1 to n+1} v_i*v_i^T - average(n+1)*average(n+1)^T # = 1/(n+1) sum_{1 to n} v_i*v_i^T + 1/(n+1) v_{n+1}*v_{n+1}^T - average(n+1)*average(n+1)^T # = 1/(n+1)(n variance(n) + average(n)*average(n)^T) - average(n+1)*average(n+1)^T + 1/(n+1) v_{n+1}*v_{n+1}^T # = n/(n+1) variance(n) + 1/(n+1)(v_{n+1}*v_{n+1}^T + average(n)*average(n)^T) - average(n+1)*average(n+1)^T #temp1 = matrixops.add(matrixops.outer(oldaverage,oldaverage),matrixops.outer(value,value)) #temp2 = matrixops.madd(matrixops.mul(oldweight,self.variance),temp1,newweight) #self.variance = matrixops.add(temp2,matrixops.outer(self.average,self.average) self.count = newcount
def load_item_geometry(bmin,bmax,geometry_ptr = None): """Loads the geometry of the given item and returns it. If geometry_ptr is provided, then it is assumed to be a Geometry3D object and the object geometry is loaded into it.""" if geometry_ptr == None: geometry_ptr = Geometry3D() # fn = model_dir + "cylinder.tri" fn = model_dir + "cube.tri" # fn = model_dir + "/objects/teapot/pots.tri" # bmin = [0,0,0] # bmax = [1.2,1.5,1.25] # fn = model_dir + "items/rollodex_mesh_collection_jumbo_pencil_cup/textured_meshes/optimized_poisson_textured_mesh.ply" if not geometry_ptr.loadFile(fn): print "Error loading cube file",fn exit(1) center = vectorops.mul(vectorops.add(bmin,bmax),0.5) scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]] translate = vectorops.sub(bmin,center) geometry_ptr.transform(so3.mul(scale,so3.rotation([0,0,1],math.pi/180*0)),translate) geometry_ptr.setCurrentTransform(so3.identity(),[0,0,0]) return geometry_ptr
def generate_trajectory(qi, qf): i = 0 endIndex = 2 path = [0.0, 0.0] path[0] = qi path[1] = qf print qi, qf while i < endIndex-1: # print i, endIndex q = path[i] qNext = path[i+1] dt = vectorops.distance(q,qNext) # smooth trajectory by interpolating between two consecutive configurations # if the distance between the two is big if dt>0.1: qInterp = vectorops.div(vectorops.add(q, qNext), 2) path.insert(i+1, qInterp) endIndex +=1 continue else: i += 1 print len(path) return path
def spawn_objects_from_ground_truth(world): """For all ground_truth_items, spawns RigidObjects in the world according to their sizes / mass properties""" print "Initializing world objects" for item in ground_truth_items: obj = world.makeRigidObject(item.info.name) bmin,bmax = item.info.bmin,item.info.bmax center = vectorops.div(vectorops.add(bmin,bmax),2.0) m = obj.getMass() m.setMass(item.info.mass) m.setCom([0,0,0]) m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],item.info.mass/12.0)) obj.setMass(m) c = obj.getContactParameters() c.kFriction = 0.6 c.kRestitution = 0.1; c.kStiffness = 100000 c.kDamping = 100000 obj.setContactParameters(c) simgeometry = obj.geometry() load_item_geometry(item,simgeometry) obj.setTransform(item.xform[0],item.xform[1]) return
calibrationConfigs = [] trueObservations = [] observations = [] for obs in xrange(numObs): q0 = [random.uniform(a,b) for (a,b) in zip(qmin,qmax)] #don't move head for i in range(13): q0[i] = 0 trueCalibrationConfigs.append(q0) trueCalibrationConfigs=resource.get("calibration.configs",default=trueCalibrationConfigs,type="Configs",description="Calibration configurations",world=world) for q0 in trueCalibrationConfigs: robot.setConfig(q0) obs0 = se3.mul(se3.inv(lc.getTransform()),lm.getTransform()) dq = [random.uniform(-jointEncoderError,jointEncoderError) for i in range(len(q0))] dobs = (so3.from_moment([random.uniform(-sensorErrorRads,sensorErrorRads) for i in range(3)]),[random.uniform(-sensorErrorMeters,sensorErrorMeters) for i in range(3)]) calibrationConfigs.append(vectorops.add(q0,dq)) observations.append(se3.mul(obs0,dobs)) trueObservations.append(obs0) if DO_VISUALIZATION: rgroup = coordinates.addGroup("calibration ground truth") rgroup.addFrame("camera link",worldCoordinates=pc.getTransform()) rgroup.addFrame("marker link",worldCoordinates=pm.getTransform()) rgroup.addFrame("camera (ground truth)",parent="camera link",relativeCoordinates=Tc0) rgroup.addFrame("marker (ground truth)",parent="marker link",relativeCoordinates=Tm0) for i,(obs,obs0) in enumerate(zip(observations,trueObservations)): rgroup.addFrame("obs"+str(i)+" (ground truth)",parent="camera (ground truth)",relativeCoordinates=obs0) rgroup.addFrame("obs"+str(i)+" (from camera)",parent="camera (ground truth)",relativeCoordinates=obs) vis.add("world",world) for i,q in enumerate(calibrationConfigs): vis.add("config"+str(i),q)
cmd = parts[0] args = parts[1:] if cmd == 'load' and len(args) == 1: kb.target_object = args[0] kb.object_xforms[kb.target_object] = se3.identity() elif cmd == 'move' and len(args) <= 3: (x, y, z) = map(float, args) + ([0] * (3 - len(args))) xform = kb.object_xforms[kb.target_object] kb.object_xforms[kb.target_object] = (xform[0], [ x, y, z ]) elif cmd == 'mover' and len(args) <= 3: (x, y, z) = map(float, args) + ([0] * (3 - len(args))) xform = kb.object_xforms[kb.target_object] kb.object_xforms[kb.target_object] = (xform[0], vectorops.add(xform[1], [ x, y, z ])) elif cmd == 'rot' and len(args) <= 3: (rx, ry, rz) = map(lambda a: float(a) * 3.141592/180, args) + ([0] * (3 - len(args))) xform = kb.object_xforms[kb.target_object] kb.object_xforms[kb.target_object] = ( reduce(so3.mul, [ so3.rotation(v, a) for (v, a) in zip([ [1,0,0], [0,1,0], [0,0,1] ], [ rx, ry, rz ]) ]), xform[1] ) elif cmd == 'rotl' and len(args) <= 3: (rx, ry, rz) = map(float, args) + ([0] * (3 - len(args))) xform = kb.object_xforms[kb.target_object] kb.object_xforms[kb.target_object] = ( reduce(so3.mul, [ xform[0] ] + [ so3.rotation(v, a) for (v, a) in zip([ [1,0,0], [0,1,0], [0,0,1] ], [ rx, ry, rz ]) ]), xform[1]
def power_grasp_medial_axis(pc,bin,object,knowledgeBase): """Returns a power grasp for the Reflex gripper to grasp the medial axis of the point cloud given in the Baxter's frame (i.e., x is forward, z is up). Input: - pc: a klampt.PointCloud object - bin: a bin name - object: an object name - knowledgeBase: an apc.KnowledgeBase object Returns a pair (g,s): - g: an apc.ItemGrasp member defining the grasp. Its xform member is given in the Baxter's local frame. - s: a confidence score, with 0 = unconfident, 1 = confident """ global gripper,depthmax,powerwidthmax points = [] for i in xrange(0,len(pc.vertices),3): points.append([pc.vertices[i],pc.vertices[i+1],pc.vertices[i+2]]) mean = [0,0,0] for p in points: mean = vectorops.add(mean,p) mean = vectorops.mul(mean,1.0/len(points)) bmin,bmax = points[0][:],points[0][:] for p in points: if p[0] < bmin[0]: bmin[0]=p[0] elif p[0] > bmax[0]: bmax[0]=p[0] if p[1] < bmin[1]: bmin[1]=p[1] elif p[1] > bmax[1]: bmax[1]=p[1] if p[2] < bmin[2]: bmin[2]=p[2] elif p[2] > bmax[2]: bmax[2]=p[2] median = vectorops.mul(vectorops.add(bmin,bmax),0.5) width = bmax[1]-bmin[1] depth = bmax[0]-bmin[0] height = bmax[2]-bmin[2] #determine an adequate center graspcenter = median[:] if depth*0.5 > depthmax - center_to_finger: graspcenter[0] = bmin[0] + depthmax - center_to_finger graspfile = os.path.join("../resources",gripper,object,'default_power_grasp.json') try: f = open(graspfile,'r') contents = ''.join(f.readlines()) g = apc.ItemGrasp() g.readFromJson(contents) #translate to center of point cloud g.grasp_xform = (g.grasp_xform[0],vectorops.add(g.grasp_xform[1],graspcenter)) return (g,1) except IOError: print "No default grasp defined for object",object,"trying default" graspfile = os.path.join("../resources",gripper,'default_power_grasp.json') try: f = open(graspfile,'r') contents = ''.join(f.readlines()) g = apc.ItemGrasp() g.readFromJson(contents) #translate to center of point cloud g.grasp_xform = (g.grasp_xform[0],vectorops.add(g.grasp_xform[1],graspcenter)) #adjust grasp to size of object ratio = width/powerwidthmax if ratio > 1.0: g.gripper_close_command[0] = g.gripper_close_command[1] = g.gripper_close_command[2] = 0.7 g.gripper_open_command[0] = g.gripper_open_command[1] = g.gripper_open_command[2] = g.gripper_close_command[2] = 1 return (g,0.1) else: #simple approximation: angular interpolation angle = math.asin(ratio) g.gripper_close_command[0] = g.gripper_close_command[1] = g.gripper_close_command[2] = 0.7*angle/(math.pi*0.5) g.gripper_open_command[0] = g.gripper_open_command[1] = g.gripper_open_command[2] = g.gripper_open_command[2] = g.gripper_close_command[0]+0.2 return (g,0.5) except IOError: print "Default power grasp file",graspfile,"not found" return (None,0)
def drawRobotGL(self, q): glColor3f(0, 0, 1) newc = vectorops.add(self.robot.center, q) c = Circle(newc[0], newc[1], self.robot.radius) c.drawGL()
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 integrate(self,x,d): """For Lie groups, returns the point that would be arrived at via integrating the difference vector d starting from x. Must satisfy the relationship a = integrate(b,difference(a,b)). In Cartesian spaces it is x+d""" return vectorops.add(x,d)
def move_to_grasp_object(self,object): """Sets the robot's configuration so the gripper grasps object at one of its potential grasp locations. Might change self.active_limb to the appropriate limb. Must change self.active_grasp to the selected grasp. If successful, sends the motion to the low-level controller and returns True. Otherwise, does not modify the low-level controller and returns False. """ self.waitForMove() self.controller.commandGripper(self.active_limb,[1]) grasps = self.knowledge.grasp_xforms(object) qmin,qmax = self.robot.getJointLimits() #get the end of the commandGripper motion qcmd = self.controller.getCommandedConfig() self.robot.setConfig(qcmd) set_model_gripper_command(self.robot,self.active_limb,[1]) qcmd = self.robot.getConfig() #solve an ik solution to one of the grasps grasp_goals = [] pregrasp_goals = [] pregrasp_shift = [0,0,0.05] for (grasp,gxform) in grasps: if self.active_limb == 'left': Tg = se3.mul(gxform,se3.inv(left_gripper_center_xform)) goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1]) Tpg = se3.mul(gxform,se3.inv((left_gripper_center_xform[0],vectorops.add(left_gripper_center_xform[1],pregrasp_shift)))) pregoal = ik.objective(self.left_gripper_link,R=Tpg[0],t=Tpg[1]) else: Tg = se3.mul(gxform,se3.inv(right_gripper_center_xform)) goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1]) Tpg = se3.mul(gxform,se3.inv((right_gripper_center_xform[0],vectorops.add(right_gripper_center_xform[1],pregrasp_shift)))) pregoal = ik.objective(self.right_gripper_link,R=Tpg[0],t=Tpg[1]) grasp_goals.append(goal) pregrasp_goals.append(pregoal) sortedSolutions = self.get_ik_solutions(pregrasp_goals,[self.active_limb]*len(grasp_goals),qcmd) if len(sortedSolutions)==0: return False #prototyping hack: move straight to target if SKIP_PATH_PLANNING: for pregrasp in sortedSolutions: graspIndex = pregrasp[1] gsolns = self.get_ik_solutions([grasp_goals[graspIndex]],[self.active_limb],pregrasp[0],maxResults=1) if len(gsolns)==0: print "Couldn't find grasp config for pregrasp? Trying another" else: self.waitForMove() self.sendPath([pregrasp[0],gsolns[0][0]]) self.active_grasp = grasps[graspIndex][0] return True print "Planning failed" return False for solution in sortedSolutions: path = self.planner.plan(qcmd,solution[0]) graspIndex = solution[1] if path != None: #now solve for grasp gsolns = self.get_ik_solutions([grasp_goals[graspIndex]],[self.active_limb],path[-1],maxResults=1) if len(gsolns)==0: print "Couldn't find grasp config??" else: self.waitForMove() self.sendPath(path+[gsolns[0][0]]) self.active_grasp = grasps[graspIndex][0] return True print "Planning failed" return False
def drawRobotGL(self,q): glColor3f(0,0,1) newc = vectorops.add(self.robot.center,q) c = Circle(newc[0],newc[1],self.robot.radius) c.drawGL()
def bin_vantage_point(self,bin_name): world_center = self.bin_front_center(bin_name) # Vantage point has 20cm offset from bin center world_offset = so3.apply(ground_truth_shelf_xform[0],[0,0,0.2]) return vectorops.add(world_center,world_offset)
def bin_vantage_point(self,bin_name): world_center = self.bin_front_center(bin_name) #20cm offset world_offset = so3.apply(self.shelf_xform[0],[0,0,0.2]) return vectorops.add(world_center,world_offset)