Exemple #1
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """
        #TODO: remove me
        left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
        right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
        qmin,qmax = self.robot.getJointLimits()
        for i in range(100):
            if self.active_limb == 'left':
                q = baxter_rest_config[:]
                for j in self.left_arm_indices:
                    q[j] = random.uniform(qmin[j],qmax[j])
                #goal = ik.objective(self.left_gripper_link,local=[vectorops.sub(left_gripper_center_xform[1],[0,0,0.1]),left_gripper_center_xform[1]],world=[vectorops.add(target,[0,0,0.1]),target])
                goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
                if ik.solve(goal,tol=0.1):
                    self.config = self.robot.getConfig()
                    return True
                self.config = self.robot.getConfig()
            else:
                q = baxter_rest_config[:]
                for j in self.right_arm_indices:
                    q[j] = random.uniform(qmin[j],qmax[j])
                #goal = ik.objective(self.right_gripper_link,local=[vectorops.sub(right_gripper_center_xform[1],[0,0,0.1]),right_gripper_center_xform[1]],world=[vectorops.add(target,[0,0,0.1]),target])
                goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
                if ik.solve(goal,tol=0.1):
                    self.config = self.robot.getConfig()
                    return True
                self.config = self.robot.getConfig()
        #TODO: put my code here
        return False
Exemple #2
0
 def getJacobian(self, q):
     self.robot.setConfig(q)
     J = None
     if self.taskType == 'po':
         J = self.link.getJacobian(self.localPosition)
     elif self.taskType == 'position':
         J = self.link.getPositionJacobian(self.localPosition)
     elif self.taskType == 'orientation':
         J = self.link.getOrientationJacobian()
     else:
         raise ValueError("Invalid taskType " + self.taskType)
     J = np.array(J)
     #check if relative transform task, modify Jacobian accordingly
     if self.baseLinkNo >= 0:
         T = self.link.getTransform()
         Tb = self.baseLink.getTransform()
         Tbinv = se3.inv(Tb)
         pb = se3.apply(Tbinv, se3.apply(T, self.localPosition))
         if self.taskType == 'po':
             Jb = self.baseLink.getJacobian(pb)
         elif self.taskType == 'position':
             Jb = self.baseLink.getPositionJacobian(pb)
         elif self.taskType == 'orientation':
             Jb = self.baseLink.getOrientationJacobian()
         Jb = np.array(Jb)
         #subtract out jacobian w.r.t. baseLink
         J -= Jb
     if self.activeDofs != None:
         J = select_cols(J, self.activeDofs)
     return J
Exemple #3
0
 def drawGL(self, q):
     x = self.getSensedValue(q)
     xdes = self.xdes
     if self.baseLinkNo >= 0:
         Tb = self.baseLink.getTransform()
         if self.taskType == "position":
             x = se3.apply(Tb, x)
             xdes = se3.apply(Tb, xdes)
         elif self.taskType == "po":
             x = se3.mul(Tb, x)
             xdes = se3.mul(Tb, xdes)
     glPointSize(6)
     glEnable(GL_POINT_SMOOTH)
     glBegin(GL_POINTS)
     if self.taskType == "position":
         glColor3f(1, 0, 0)  #red
         glVertex3fv(xdes)
         glColor3f(1, 0.5, 0)  #orange
         glVertex3fv(x)
     elif self.taskType == "po":
         glColor3f(1, 0, 0)  #red
         glVertex3fv(xdes[1])
         glColor3f(1, 0.5, 0)  #orange
         glVertex3fv(x[1])
     glEnd()
Exemple #4
0
 def display(self):
     #draw the world
     self.world.drawGL()
     lh = Hand('l')
     rh = Hand('r')
     glDisable(GL_LIGHTING)
     glPointSize(5.0)
     glDisable(GL_DEPTH_TEST)
     glBegin(GL_POINTS)
     glColor3f(0, 1, 0)
     glVertex3fv(
         se3.apply(
             self.world.robot(0).link(lh.link).getTransform(),
             lh.localPosition1))
     glVertex3fv(
         se3.apply(
             self.world.robot(0).link(rh.link).getTransform(),
             rh.localPosition1))
     glColor3f(0, 0, 1)
     glVertex3fv(
         se3.apply(
             self.world.robot(0).link(lh.link).getTransform(),
             lh.localPosition2))
     glVertex3fv(
         se3.apply(
             self.world.robot(0).link(rh.link).getTransform(),
             rh.localPosition2))
     glColor3f(1, 0, 0)
     glVertex3fv(self.world.rigidObject(0).getTransform()[1])
     glEnd()
     glEnable(GL_DEPTH_TEST)
Exemple #5
0
    def display(self):
        #Put your display handler here
        #the current example draws the simulated world in grey and the
        #commanded configurations in transparent green
        self.sim.updateWorld()
        self.world.drawGL()

        #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.getBody(self.handsim.model.robot.getLink(self.handsim.model.proximal_links[i]))
            b2 = self.sim.getBody(self.handsim.model.robot.getLink(self.handsim.model.distal_links[i]))
            glVertex3f(*se3.apply(b1.getTransform(),self.handsim.tendon1_local))
            glVertex3f(*se3.apply(b2.getTransform(),self.handsim.tendon2_local))
        glEnd()
        glLineWidth(1)
        glEnable(GL_DEPTH_TEST)
        glEnable(GL_LIGHTING)

        #draw commanded configurations
        glEnable(GL_BLEND)
        glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
        glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
        for i in xrange(self.world.numRobots()):
            r = self.world.robot(i)
            q = self.sim.getController(i).getCommandedConfig()
            r.setConfig(q)
            r.drawGL(False)
        glDisable(GL_BLEND)
	def getJacobian(self, q):
		self.robot.setConfig(q)
		J = None
		if self.taskType == 'po':
			J = self.link.getJacobian(self.localPosition)
		elif self.taskType == 'position':
			J = self.link.getPositionJacobian(self.localPosition)
		elif self.taskType == 'orientation':
			J = self.link.getOrientationJacobian()
		else:
			raise ValueError("Invalid taskType "+self.taskType)
		J = np.array(J)
		#check if relative transform task, modify Jacobian accordingly
		if self.baseLinkNo >= 0:
			T = self.link.getTransform()
			Tb = self.baseLink.getTransform()
			Tbinv = se3.inv(Tb)
			pb = se3.apply(Tbinv,se3.apply(T,self.localPosition))
			if self.taskType == 'po':
				Jb = self.baseLink.getJacobian(pb)
			elif self.taskType == 'position':
				Jb = self.baseLink.getPositionJacobian(pb)
			elif self.taskType == 'orientation':
				Jb = self.baseLink.getOrientationJacobian()
			Jb = np.array(Jb)
			#subtract out jacobian w.r.t. baseLink
			J -= Jb
		if self.activeDofs!=None:
			J = select_cols(J,self.activeDofs)
		return J
    def move_to_grasp_object(self,object):
        #Select appropriate limb and link
        self.active_limb = self.limb_selector("gripper")

        #Extract the contents of the bin
        binContents = self.knowledge.bin_contents[self.current_bin]

        #Extract the list of possible grasp points
        graspList = binContents[0].info.grasps

        #Loop through grasp list and attempt to solve inverse kinematics
        for graspIndex in range(0, len(graspList)):
            #Compute transform from grasp point to bin to world
            graspTransform = graspList[graspIndex].grasp_xform
            binCoordinate = se3.apply(graspTransform,[0,0,0])
            worldCoordinate = se3.apply(binContents[0].xform, binCoordinate)

            #Compute objective
            objective = ik.objective(self.active_limb, local=[0,0,0], world=worldCoordinate)

            #Attempt to solve inverse kinematics
            if ik.solve(objective):
                self.config = self.robot.getConfig()
                return True
            else:
                return False
Exemple #8
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, sends the motion to the low-level controller and
        returns True.

        Otherwise, does not modify the low-level controller and returns False.
        """
        left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
        right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
        qcmd = self.controller.getCommandedConfig()
        for i in range(100):
            if self.active_limb == 'left':
                goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
            else:
                goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
            #set IK solver initial configuration
            if i==0:
                self.robot.setConfig(qcmd)
            else:
                self.randomize_limb_position(self.active_limb)
            #solve
            if ik.solve(goal,tol=0.1):

                if self.planner.check_collision_free('left'):
                    self.controller.setMilestone(self.robot.getConfig())
                    self.active_limb = 'left'
                    return True

        return False
 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 drawGL(self,q):
		x = self.getSensedValue(q)
		xdes = self.xdes
		if self.baseLinkNo >= 0:
			Tb = self.baseLink.getTransform()
			if self.taskType == "position":
				x = se3.apply(Tb,x)
				xdes = se3.apply(Tb,xdes)
			elif self.taskType == "po":
				x = se3.mul(Tb,x)
				xdes = se3.mul(Tb,xdes)
		glPointSize(6)
		glEnable(GL_POINT_SMOOTH)
		glBegin(GL_POINTS)
		if self.taskType == "position":
			glColor3f(1,0,0)	#red 
			glVertex3fv(xdes)
			glColor3f(1,0.5,0)	#orange
			glVertex3fv(x)
		elif self.taskType == "po":
			glColor3f(1,0,0)	#red 
			glVertex3fv(xdes[1])
			glColor3f(1,0.5,0)	#orange
			glVertex3fv(x[1])
		glEnd()
    def goToOrderBin(self,limb,fromWhere):
        qcmd = self.controller.getCommandedConfig()        
        left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
        right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])

        if limb == 'left':
            placegoal = ik.objective(self.left_gripper_link,local=baxter.left_gripper_center_xform[1],world=left_target)
        else:
            placegoal = ik.objective(self.right_gripper_link,local=baxter.right_gripper_center_xform[1],world=right_target)

        sortedSolutions = self.get_ik_solutions([placegoal],[limb],qcmd,tol=0.1)
        if len(sortedSolutions) == 0:
            print "Failed to find placement config"
        for solution in sortedSolutions:
            if limb == 'left':
                path = self.planner.plan(qcmd, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3717782274123387, -0.674937510436548, -0.33948575074327003, 2.1672104893835455, -0.08734593872357976, 0.0, 0.12728394408809324, 9.587078444893871e-09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0600000000000005, 0.0, 2.31, 0.0, 0.0, 0.28, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
            else:
                path = self.planner.plan(qcmd, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.1399999999999997, 0.0, 2.32, 0.0, 0.0, 0.32, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.13866956622769833, -0.6983335926844152, 0.43471512229342457, 2.241413168181005, 0.09879187313355131, 0.0, 0.30929795433686946, 0.13997178733833623, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

                #path = self.planner.plan(qcmd,solution[0])
            if path != None:
                if(fromWhere == 'nofile'):
                    self.waitForMove()
                    self.sendPath(path)
                else:
                    self.waitForMove()
                    self.sendPath(path)
                    self.printPath(path,'cached_motions/'+limb+'_from_bin'+fromWhere+'_2home')
                return True
        print "Planning failed"        
        return False
Exemple #12
0
def icp(object,scene):
	#return se3.identity()
	"""Computes a rotation and translation of the object to the scene
	using the ICP algorithm.  Returns the transform (R,t) in Klamp't se3
	format.  
	"""
	kd = KDTree(scene)
	print "kd tree done"

	#TODO: implement me
	# Sample both maps to make the closest point computations faster

	# Compute the minimum distance between the points



	# Reject the outliers, for example, using a threshold

	# Compute the R and t matrices. The procedure can be similar as the one for the
	# 2D images.
	#ret = se3.identity()
	#return ret
	#ret[1][2]+=0.2
	#ret[1][1]+=0.3
	#return ret
	sampled = random.sample(object, 1000)
	threshold = 0.75
	net = se3.identity()
	#net =[net[0], (0.2,0.05,0)]
	#return net
	sampled = [se3.apply(net, o) for o in sampled]
	#print object[0]
	for _ in xrange(10):
		print "start finding correspondances"
		correspondings = []
		for i in sampled:
			try:
				(dist, idx) = kd.query(i)
			except:
				print i
				raise Exception()
			correspondings.append((i, scene[idx], dist))
		correspondings.sort(key=lambda x: x[2])
		correspondings = correspondings[0:int(len(correspondings)*threshold)]
		#print "\n".join(map(str,correspondings))
		print "done finding correspondances"
		object_points = [i[0] for i in correspondings]
		scene_points = [i[1] for i in correspondings]
		(R,t) = one_round_icp(scene_points, object_points, 'list')
		#(R,t) = one_round_icp(object_points, scene_points, 'matrix')
		sampled = [se3.apply((R,t),i) for i in sampled]
		net = se3.mul((R,t), net)
		#print R,t
		#print net
	return net
Exemple #13
0
def ApplyICP(object, scene):

    outR = [];
    outT = [];

    SSEm1 = 1000000000

    for i in range(0,5):

        SSE = 1000000
        SSE_min = 10000000
        SSE_max = 10000000

        #run the ICP
        R,t,mR,mt = icp(object, scene)

        if len(R) > 1:
            #apply the ICP results to the model
            maxModel = [se3.apply((R,t),p) for p in object]
            maxSample = random.sample(object,int(float(len(object))*0.01))
            SSE_max = sum_of_squared_errors(maxSample,scene)
        elif R == -1:
            SSE_max = 10000000

        if len(mR) > 1:
            minModel = [se3.apply((mR,mt),p) for p in object]
            minSample = random.sample(object,int(float(len(object))*0.01))
            SSE_min = sum_of_squared_errors(minSample,scene)
        else:
            SSE_min = 10000000

        if SSE_min == 10000000 and SSE_max == 10000000:
            SSE = 1000000
            break
        elif SSE_max <= SSE_min:
            object = maxModel
            print SSE_max
            SSE = SSE_max
            tempR = R
            tempT = t
        else:
            object = minModel
            print SSE_min
            SSE = SSE_min
            tempR = mR
            tempT = mt

        if SSEm1 > SSE:
            SSEm1 = SSE
            outR = tempR
            outT = tempT
            print SSE

    return outR, outT
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """

        # find a point a little (0.5 meters) above the order bin)
        aboveOrderBinLocation = se3.apply(order_bin_xform, [0, 0, 0])
        aboveOrderBinLocation[2] = aboveOrderBinLocation[2] + 0.5

        oldConfig = self.robot.getConfig()

        # figure out what gripper is holding the object
        activeGripper = self.left_gripper_link
        if self.active_limb == "Right":
            activeGripper = self.right_gripper_link

        goal = ik.objective(activeGripper, local=[0, 0, 0], world=aboveOrderBinLocation)
        if ik.solve(goal):
            self.config = self.robot.getConfig();
            return True


        #self.robot.setConfig(oldConfig)
        print "IK solver failed to find a a point above the order bin."
        return False
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """
        #TODO: put my code here
		#gohere
	'''
	bin_name = object.bin_name
	pointInFrontOfBin = self.knowledge.bin_front_center(bin_name)
	goal = ik.objective(self.active_limb,R=[0,1,0,0,0,1,1,0,0],t=pointInFrontOfBin)
	self.config = self.robot.getConfig()
	return ik.solve(goal)
	'''
	self.robot.setConfig(self.originalConfig)
	self.config = self.originalConfig

	orderBinLocalPoint = [(order_bin_bounds[0][i] + order_bin_bounds[1][i])/2 for i in range(3)]
	orderBinLocalPoint[2] = .7
	gripper = self.active_limb
	orderBinWorldPoint = se3.apply(order_bin_xform,orderBinLocalPoint)
	goal = ik.objective(gripper,R=[0,1,0,1,0,0,0,0,-1],t=orderBinWorldPoint)
	iterations = 0
	while iterations!=1000:
		iterations+=1
		if ik.solve(goal,tol=.1):
			self.config = self.robot.getConfig()
			return True
		print "having trouble putting in order bin"
		pointAroundBinWorldPoint = [orderBinWorldPoint[i] + random.random()*.1 for i in range(3)]
		goal = ik.objective(gripper,R=[0,1,0,1,0,0,0,0,-1],t=pointAroundBinWorldPoint)
		self.robot.setConfig(self.originalConfig)
		self.config = self.originalConfig
    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.

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """
    #TODO: put my code here
	graspToWorldTransforms = self.knowledge.grasp_xforms(object)
	grasps_in_world_coords = [se3.apply(transform,[0,0,0]) for transform in graspToWorldTransforms]
	if self.active_limb == self.left_camera_link:
		gripper = self.left_gripper_link
		print "left gripper"
	if self.active_limb == self.right_camera_link:
		gripper = self.left_gripper_link
		print "right gripper"
	goals = [ik.objective(gripper,R=graspToWorldTransforms[i][0],t=grasps_in_world_coords[i]) for i in range(len(graspToWorldTransforms))]
	
	iterations = 0
	while iterations!= 1000:
		iterations += 1
		for goal in goals:
			if ik.solve(goal,tol=.01):
				self.config = self.robot.getConfig()
				return True
		print "having trouble grasping"
		if iterations % 500 == 0:
			a = 5
		goals = [ik.objective(gripper,R=graspToWorldTransforms[i][0],t=grasps_in_world_coords[i]) for i in range(len(graspToWorldTransforms))]
	return False
Exemple #17
0
def main():
    """
    Main loop.  Run ICP on the given model and scene file, display the results.
    """
    model_full = get_reconstructed_model(model_file, True)
    scene_full = get_raw_depth(depth_file)

    # segment the scene
    print
    print ".................Segmenting data................."
    sampled_scene = segmentScene(model_full["positions"], scene_full)
    # run the ICP
    print ".................Done segmenting................."
    print ".................Running ICP....................."
    if len(sampled_scene) > 0:
        R, t = icp(sampled_scene[0], model_full["positions"], 0)
    else:
        R, t = icp(scene_full, model_full["positions"], 0.05)
    print ".................ICP finished...................."
    # apply the ICP results to the model
    transformed_points = [se3.apply((R, t), p) for p in model_full["positions"]]
    model_full["positions"] = transformed_points

    # visualize the results
    opengl_plot = OpenGLPlot(model_full, scene_full)
    opengl_plot.initialize_main_loop()
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """
        print
        print 'Trying to move over order bin'
        world_goal_xyz = se3.apply(order_bin_xform,[0,0,0.75])
        
        cnt = 0;
        while(cnt < 100):
        #pick a random starting config for the left link and try to solve the ik problem
            config = self.robot.getConfig()
            for ind in self.left_arm_indices:
                config[ind] = random.uniform(0,3);
        
            self.robot.setConfig(config);
                
            goal = ik.objective(self.left_gripper_link,local=([0,0,0]),world=(world_goal_xyz))

            if ik.solve(goal,tol = 0.05): #lower tol because the bin is huge
                self.active_limb = 'Left'
                self.config = self.robot.getConfig()
                return True

            cnt = cnt + 1



        
        return False
Exemple #19
0
def main():


    for x in range (0,len(obs)):

        model_files = ["data/models/"+o+"_model.json" for o in objects]
        depth_file = "data/processed_depth/"+obs[x]+".json"
        print depth_file
        models = [get_reconstructed_model(f,True) for f in model_files]
        scene_full = get_raw_depth(depth_file)
        errors = []

        for i in range(len(objects)):
            print model_files[i]
            R,t = ApplyICP(models[i]['positions'],scene_full)
            #apply the ICP results to the model
            if len(R) != 0:
                transformed_points = [se3.apply((R,t),p) for p in models[i]['positions']]
                errors.append(matching_error(transformed_points,scene_full))
                print "Object",objects[i],"matching error:",errors[-1]
                models[i]['positions'] = transformed_points
            else:
                print "ERROR"
        print depth_file
        print "END"
        print ""

    #pick one of the models to visualize
    visualization_model = 2

    opengl_plot = OpenGLPlot(models[visualization_model], scene_full)
    opengl_plot.initialize_main_loop()
Exemple #20
0
 def drawGL(self, q):
     x = self.getSensedValue(q)
     xdes = self.xdes
     #invert the transformations from task to world coordinates
     if self.baseLinkNo >= 0:
         Tb = self.robot.getLink(self.baseLinkNo).getTransform()
         xdes = se3.apply(Tb, self.xdes)
         x = se3.apply(Tb, x)
     glPointSize(10)
     glEnable(GL_POINT_SMOOTH)
     glBegin(GL_POINTS)
     glColor3f(0, 1, 0)  #green
     glVertex3fv(xdes)
     glColor3f(0, 1, 1)  #cyan
     glVertex3fv(x)
     glEnd()
    def bin_front_center(self,bin_name):
        #you may want to implement this.  Returns the world position of the
        #front-center of the bin.  The visualizer will draw these points if
        #'draw_bins' is turned to True.
        #TODO:
	point = [(apc.bin_bounds[bin_name][0][0]+apc.bin_bounds[bin_name][1][0])/2,(apc.bin_bounds[bin_name][0][1]+apc.bin_bounds[bin_name][1][1])/2,.42]
	return se3.apply(self.shelf_xform,point)
	def drawGL(self,q):
		x = self.getSensedValue(q)
		xdes = self.xdes
		#invert the transformations from task to world coordinates
		if self.baseLinkNo >= 0:
			Tb = self.robot.getLink(self.baseLinkNo).getTransform()
			xdes = se3.apply(Tb,self.xdes)
			x = se3.apply(Tb,x)
		glPointSize(10)
		glEnable(GL_POINT_SMOOTH)
		glBegin(GL_POINTS)
		glColor3f(0,1,0)	#green 
		glVertex3fv(xdes)
		glColor3f(0,1,1)	#cyan
		glVertex3fv(x)
		glEnd()
Exemple #23
0
def main():
    models = [get_reconstructed_model(f,True) for f in model_files]
    scene_full = get_raw_depth(depth_file)
    errors = []
    for i in range(len(objects)):
        R,t = icp(models[i]['positions'],scene_full,True)
        #apply the ICP results to the model
        transformed_points = [se3.apply((R,t),p) for p in models[i]['positions']]
        errors.append(matching_error(transformed_points,scene_full))
        print "Object",objects[i],"matching error:",errors[-1]
        models[i]['positions'] = transformed_points

    bestIndex = 0
    bestError = min(errors)
    for i in range(len(objects)):
        if errors[i] == bestError:
            bestIndex = i
            break

    #pick one of the models to visualize
    visualization_model = bestIndex
    print scene, " scene's best match was the object: ", objects[bestIndex]

    opengl_plot = OpenGLPlot(models[visualization_model], scene_full)
    opengl_plot.initialize_main_loop()
Exemple #24
0
 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')
Exemple #25
0
    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 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
Exemple #27
0
 def order_bin_top(self):
     (a, b) = order_bin_bounds
     # compute the order bin center in the x-y plane
     # return the top as 10% beyond the bin top in z
     top = [ (a[0] + b[0])/2, (a[1] + b[1])/2, max([ a[2], b[2] ])*1.1 ]
     
     # transform the order bin into the appropriate location
     return se3.apply(order_bin_xform, top)
 def bin_front_center(self,bin_name):
     #you may want to implement this.  Returns the world position of the
     #front-center of the bin.  The visualizer will draw these points if
     #'draw_bins' is turned to True.
 
     boundingBox = apc.bin_bounds[bin_name]
     binFrontCenterShelfFrame = [(boundingBox[0][0] + boundingBox[1][0]) / 2, (boundingBox[0][1] + boundingBox[1][1]) / 2, boundingBox[1][2] ]
     return se3.apply(self.shelf_xform, binFrontCenterShelfFrame)
    def bin_front_center(self,bin_name):
        #you may want to implement this. Returns the world position of the
        #front-center of the bin.  The visualizer will draw these points if
        #'draw-bins' is turned to True.

        bins_xyz = apc.bin_bounds[bin_name]
        Bin = [bins_xyz[0][0] + (bins_xyz[1][0] - bins_xyz[0][0])/2 ,bins_xyz[0][1] + (bins_xyz[1][1] - bins_xyz[0][1])/2, bins_xyz[1][2]]
        Bin_World = se3.apply(self.shelf_xform, Bin)
        return Bin_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.

        bins_xyz = apc.bin_bounds[bin_name]
        Bin = [bins_xyz[0][0] + (bins_xyz[1][0] - bins_xyz[0][0])/2 ,bins_xyz[0][1] + (bins_xyz[1][1] - bins_xyz[0][1])/2, bins_xyz[1][2] + 0.2]
        Bin_World_Vantage = se3.apply(self.shelf_xform, Bin)
        return Bin_World_Vantage
    def order_bin_vantage_point(self):

        #Compute the local vantage point from the order bin bounds
        localVantagePoint = [order_bin_bounds[0][0] + (order_bin_bounds[1][0] - order_bin_bounds[0][0])/2 ,order_bin_bounds[0][1] + (order_bin_bounds[1][1] - order_bin_bounds[0][1])/2, order_bin_bounds[1][2]]

        #Apply the xform transform to convert the local vantage point to the world vantage point
        worldVantagePoint = se3.apply(order_bin_xform, localVantagePoint)

        return worldVantagePoint
Exemple #32
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, sends the motion to the low-level controller and
        returns True.
        
        Otherwise, does not modify the low-level controller and returns False.
        """
        collisionTries = 0
        left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
        right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
        qcmd = self.controller.getCommandedConfig()
        for i in range(100):
            if self.active_limb == 'left':
                goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
            else:
                goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
            #set IK solver initial configuration
            if i==0:
                self.robot.setConfig(qcmd)
            else:
                self.randomize_limb_position(self.active_limb)
            #solve
            if ik.solve(goal,tol=0.1):
                #TODO: plan a path

                # check that this solution is self collision free
                # we call the self collision function of WorldCollider
                # and see how many items the generator returns. If it is 0
                # then we don't collide (if it is not zero, we don't care what
                # they are, we throw the solution away and retry with a different
                # initial arm configuration).
                collisionTries += 1
                collisions = self.collider.robotSelfCollisions(robot = self.robot)
                numCols = 0
                for cols in collisions:
                   numCols += 1

                self.controller.setMilestone(self.robot.getConfig())
                if numCols == 0:
                    self.controller.setMilestone(self.robot.getConfig())
                    print "Found self collision free IK solution after " + str(collisionTries) + " tries"
                    return True
        return False
    def move_to_order_bin(self,object):
        # apply the order_bin_xform to the given point
        left_target_point  = se3.apply(order_bin_xform, [0.0, 0.2, order_bin_bounds[1][2]+0.1])
        right_target_point = se3.apply(order_bin_xform, [0.0, -0.2, order_bin_bounds[1][2]+0.1])

        # Setup ik objectives for both arms
        left_goal  = ik.objective(self.left_gripper_link,  local=left_gripper_center_xform[1],  world=left_target_point)
        right_goal = ik.objective(self.right_gripper_link, local=right_gripper_center_xform[1], world=right_target_point)

        qmin, qmax = self.robot.getJointLimits()

        for i in range(100):
            # initialize to the resting pose
            # NOTE: that this (without [:]) doesn't work because the list
            #       must be copied by value, not reference.
            #       See python shallow copy vs. deep copy for more details
            #       ( q = baxter_rest_config ) doesn't work!
            q = baxter_rest_config[:]

            # use left hand
            if self.active_limb == 'left':
                # randomly initialize each joint in the arm within joint limits
                for jointIndex in self.left_arm_indices:
                    q[jointIndex] = random.uniform(qmin[jointIndex], qmax[jointIndex])
                self.robot.setConfig(q)

                # attempt to solve ik objective
                if ik.solve(left_goal):
                    self.config = self.robot.getConfig()
                    return True

            # use right hand
            else:
                # randomly initialize each joint in the arm within joint limits
                for jointIndex in self.right_arm_indices:
                    q[jointIndex] = random.uniform(qmin[jointIndex], qmax[jointIndex])
                self.robot.setConfig(q)

                # attempt to solve ik objective
                if ik.solve(right_goal):
                    self.config = self.robot.getConfig()
                    return True
        self.config = self.robot.getConfig()
        return False
    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 getSensedValue(self, q):
		"""Get link x, which is rotation matrix and/or translation
		"""
		self.robot.setConfig(q)
		T = self.link.getTransform()
		#check if relative transform task, modify T to local transform
		if self.baseLinkNo >= 0:
			Tb = self.baseLink.getTransform()
			Tbinv = se3.inv(Tb)
			T = se3.mul(Tbinv,T)
		if self.taskType == 'po':
			x = (T[0],se3.apply(T,self.localPosition))
		elif self.taskType == 'position':
			x = se3.apply(T,self.localPosition)
		elif self.taskType == 'orientation':
			x = T[0]
		else:
			raise ValueError("Invalid taskType "+self.taskType)
		return x
	def getSensedValue(self, q):
		"""Returns CoM position
		"""
		self.robot.setConfig(q)
		com = self.robot.getCom()
		if self.baseLinkNo >= 0:
			Tb = self.robot.getLink(self.baseLinkNo).getTransform()
			Tbinv = se3.inv(Tb)
			com = se3.apply(Tbinv,com)
		return com
Exemple #37
0
 def display(self):
     #draw the world
     self.world.drawGL()
     lh = Hand('l')
     rh = Hand('r')
     glDisable(GL_LIGHTING)
     glPointSize(5.0)
     glDisable(GL_DEPTH_TEST)
     glBegin(GL_POINTS)
     glColor3f(0,1,0)
     glVertex3fv(se3.apply(self.world.robot(0).link(lh.link).getTransform(),lh.localPosition1))
     glVertex3fv(se3.apply(self.world.robot(0).link(rh.link).getTransform(),rh.localPosition1))
     glColor3f(0,0,1)
     glVertex3fv(se3.apply(self.world.robot(0).link(lh.link).getTransform(),lh.localPosition2))
     glVertex3fv(se3.apply(self.world.robot(0).link(rh.link).getTransform(),rh.localPosition2))
     glColor3f(1,0,0)
     glVertex3fv(self.world.rigidObject(0).getTransform()[1])
     glEnd()
     glEnable(GL_DEPTH_TEST)
Exemple #38
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, sends the motion to the low-level controller and
        returns True.

        Otherwise, does not modify the low-level controller and returns False.
        """

        qcmd = self.controller.getCommandedConfig()
        left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
        right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
        #retraction goal -- maintain vertical world axis
        liftVector = [0,0,0.04]
        retractVector = [-0.2,0,0]
        self.planner.rebuild_dynamic_objects()
        self.robot.setConfig(qcmd)
        if self.move_gripper_upright(self.active_limb,liftVector):
            self.controller.setMilestone(self.robot.getConfig())
            print "Moved to lift goal"
            self.waitForMove()
        collisionchecker = lambda x:self.planner.check_collision_free_with_object(x,object.info.geometry,self.active_grasp)
        if self.move_gripper_upright(self.active_limb,retractVector,collisionchecker):
            self.controller.setMilestone(self.robot.getConfig())
            print "Moved to retract goal"
            self.waitForMove()
        retractConfig = self.robot.getConfig()

        if self.active_limb == 'left':
            placegoal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
        else:
            placegoal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
        sortedSolutions = self.get_ik_solutions([placegoal],[self.active_limb],retractConfig,tol=0.1)
        if len(sortedSolutions) == 0:
            print "Failed to find placement config"
        for solution in sortedSolutions:
            path = self.planner.plan_transfer(retractConfig,solution[0],self.active_limb,object,self.active_grasp)
            if path != None:
                self.waitForMove()
                self.sendPath(path)
                return True
        print "Planning failed"
        return False
Exemple #39
0
    def getSensedValue(self, q):
        """Returns CoM position
		"""
        self.robot.setConfig(q)
        com = self.robot.getCom()
        if self.baseLinkNo >= 0:
            Tb = self.robot.getLink(self.baseLinkNo).getTransform()
            Tbinv = se3.inv(Tb)
            com = se3.apply(Tbinv, com)
        return com
Exemple #40
0
    def getSensedValue(self, q):
        """Get link x, which is rotation matrix and/or translation
		"""
        self.robot.setConfig(q)
        T = self.link.getTransform()
        #check if relative transform task, modify T to local transform
        if self.baseLinkNo >= 0:
            Tb = self.baseLink.getTransform()
            Tbinv = se3.inv(Tb)
            T = se3.mul(Tbinv, T)
        if self.taskType == 'po':
            x = (T[0], se3.apply(T, self.localPosition))
        elif self.taskType == 'position':
            x = se3.apply(T, self.localPosition)
        elif self.taskType == 'orientation':
            x = T[0]
        else:
            raise ValueError("Invalid taskType " + self.taskType)
        return x
    def display(self):
        #Put your display handler here
        #the current example draws the simulated world in grey and the
        #commanded configurations in transparent green
        GLSimulationProgram.display(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.handsim.model.robot.link(self.handsim.model.proximal_links[i]))
            b2 = self.sim.body(self.handsim.model.robot.link(self.handsim.model.distal_links[i]))
            glVertex3f(*se3.apply(b1.getTransform(),self.handsim.tendon1_local))
            glVertex3f(*se3.apply(b2.getTransform(),self.handsim.tendon2_local))
        glEnd()
        glLineWidth(1)
        glEnable(GL_DEPTH_TEST)
        glEnable(GL_LIGHTING)
    def display(self):
        #Put your display handler here
        #the current example draws the simulated world in grey and the
        #commanded configurations in transparent green
        self.sim.updateWorld()
        self.world.drawGL()

        #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.getBody(
                self.handsim.model.robot.getLink(
                    self.handsim.model.proximal_links[i]))
            b2 = self.sim.getBody(
                self.handsim.model.robot.getLink(
                    self.handsim.model.distal_links[i]))
            glVertex3f(
                *se3.apply(b1.getTransform(), self.handsim.tendon1_local))
            glVertex3f(
                *se3.apply(b2.getTransform(), self.handsim.tendon2_local))
        glEnd()
        glLineWidth(1)
        glEnable(GL_DEPTH_TEST)
        glEnable(GL_LIGHTING)

        #draw commanded configurations
        glEnable(GL_BLEND)
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
        for i in xrange(self.world.numRobots()):
            r = self.world.robot(i)
            q = self.sim.getController(i).getCommandedConfig()
            r.setConfig(q)
            r.drawGL(False)
        glDisable(GL_BLEND)
Exemple #43
0
	def getJacobian(self, q):
		"""Returns axis-weighted CoM Jacobian by averaging
		mass-weighted Jacobian of each link.
		"""
		self.robot.setConfig(q)
		numLinks = self.robot.numLinks()
		Jcom = np.array(self.robot.getComJacobian())
		#if relative positioning task, subtract out COM jacobian w.r.t. base
		if self.baseLinkNo >= 0:
			Tb = self.robot.link(self.baseLinkNo).getTransform()
			pb = se3.apply(se3.inv(Tb),self.robot.getCom())
			Jb = np.array(self.robot.link(self.baseLinkNo).getPositionJacobian(pb))
			Jcom -= Jb
		if self.activeDofs != None:
			Jcom = select_cols(Jcom,self.activeDofs)
		return Jcom
Exemple #44
0
 def emulate(self, sim):
     """Given a Simulator instance, emulates the sensor.
     Result is a CameraColorDetectorOutput structure."""
     global objectColors
     xscale = math.tan(math.radians(self.fov * 0.5)) * self.w / 2
     blobs = []
     for i in range(sim.world.numRigidObjects()):
         o = sim.world.rigidObject(i)
         body = sim.body(o)
         Tb = body.getTransform()
         plocal = se3.apply(se3.inv(self.Tsensor), Tb[1])
         x, y, z = plocal
         if z < self.dmin or z > self.dmax:
             continue
         c = objectColors[i % len(objectColors)]
         o.geometry().setCurrentTransform(
             so3.mul(so3.inv(self.Tsensor[0]), Tb[0]), [0] * 3)
         bmin, bmax = o.geometry().getBB()
         err = self.pixelError
         dscale = xscale / z
         xim = dscale * x + self.w / 2
         yim = dscale * y + self.h / 2
         xmin = int(xim - dscale * (bmax[0] - bmin[0]) * 0.5 +
                    random.uniform(-err, err))
         ymin = int(yim - dscale * (bmax[1] - bmin[1]) * 0.5 +
                    random.uniform(-err, err))
         xmax = int(xim + dscale * (bmax[0] - bmin[0]) * 0.5 +
                    random.uniform(-err, err))
         ymax = int(yim + dscale * (bmax[1] - bmin[1]) * 0.5 +
                    random.uniform(-err, err))
         if xmin < 0: xmin = 0
         if ymin < 0: ymin = 0
         if xmax >= self.w: xmax = self.w - 1
         if ymax >= self.h: ymax = self.h - 1
         if ymax <= ymin: continue
         if xmax <= xmin: continue
         #print "Actual x,y,z",x,y,z
         #print "blob color",c[0:3],"dimensions",xmin,xmax,"x",ymin,ymax
         blob = CameraBlob(c[0:3], 0.5 * (xmin + xmax), 0.5 * (ymin + ymax),
                           xmax - xmin, ymax - ymin)
         blobs.append(blob)
     return CameraColorDetectorOutput(blobs)
 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.tendon0_local))
         glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local))
         glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local))
         glVertex3f(*se3.apply(b2.getTransform(), self.tendon2_local))
     glEnd()
     glLineWidth(1)
     glColor3f(1, 0.5, 0)
     glBegin(GL_LINES)
     fscale = 0.01
     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]))
         if self.forces[i][0] != None:
             p = se3.apply(b1.getTransform(), self.tendon0_local)
             glVertex3f(*p)
             glVertex3f(*vectorops.madd(p, self.forces[i][0], fscale))
         if self.forces[i][1] != None:
             p = se3.apply(b1.getTransform(), self.tendon1_local)
             glVertex3f(*p)
             glVertex3f(*vectorops.madd(p, self.forces[i][1], fscale))
         if self.forces[i][2] != None:
             p = se3.apply(b2.getTransform(), self.tendon2_local)
             glVertex3f(*p)
             glVertex3f(*vectorops.madd(p, self.forces[i][2], fscale))
     glEnd()
     glEnable(GL_DEPTH_TEST)
     glEnable(GL_LIGHTING)
Exemple #46
0
    unionPoints = []
    unionTris = []
    for i in range(robot.numLinks()):
        T = robot.getLink(i).getTransform()
        geom = robot.getLink(i).getGeometry()
        t = geom.type()
        if t=="TriangleMesh":
            trimesh = geom.getTriangleMesh()
            tris = []
            for index in xrange(0,len(trimesh.indices),3):
                tris.append(trimesh.indices[index:index+3])
            points = []
            for index in xrange(0,len(trimesh.vertices),3):
                points.append(trimesh.vertices[index:index+3])
            offset = len(unionPoints)
            #apply transformation
            points = [se3.apply(T,pt) for pt in points]
            unionPoints += points
            #offset triangle indices by number of existing points
            unionTris += [(a+offset,b+offset,c+offset) for (a,b,c) in tris]
    print "Writing to",meshfn,"..."
    f = open(meshfn,'w')
    f.write(str(len(unionPoints))+'\n')
    for pt in unionPoints:
        f.write('%g %g %g\n'%tuple(pt))
    f.write(str(len(unionTris))+'\n')
    for tri in unionTris:
        f.write('%d %d %d\n'%tuple(tri))
    f.close()
Exemple #47
0
def calibrate_xform_camera(camera_link_transforms,
                           marker_link_transforms,
                           marker_observations,
                           marker_ids,
                           observation_relative_errors=None,
                           camera_initial_guess=None,
                           marker_initial_guess=None,
                           regularizationFactor=0,
                           maxIters=100,
                           tolerance=1e-7):
    """Single camera calibration function for a camera and markers on some
    set of rigid bodies.
    
    Given body transforms and a list of estimated calibration marker observations
    in the camera frame, estimates both the camera transform relative to the
    camera link as well as the marker transforms relative to their links.

    M: is the set of m markers. By default there is at most one marker per link.
       Markers can either be point markers (e.g., a mocap ball), or transform
       markers (e.g., an AR tag or checkerboard pattern).
    O: is the set of n observations, consisting of a reading (Tc_i,Tm_i,o_i,l_i)
       where Tc_i is the camera link's transform, Tm_i is the marker link's
       transform, o_i is the reading which consists of either a point or transform
       estimate in the camera frame, and l_i is the ID of the marker (by default,
       just its link)

    Output: a tuple (err,Tc,marker_dict) where err is the norm of the
    reconstruction residual, Tc is the estimated camera transform relative to the
    camera's link, and marker_dict is a dict mapping each marker id to its
    estimated position or transform on the marker's link.

    Arguments:
    - camera_link: an integer index or a RobotModelLink instance on which
      the camera lies.
    - calibration_configs: a list of the RobotModel configurations q_1,...,q_n
      that generated the marker_observations list.
    - marker_observations: a list of estimated positions or transformations
      of calibration markers o_1,...,o_n, given in the camera's reference
      frame (z forward, x right, y down).

      If o_i is a 3-vector, the marker is considered to be a point marker.
      If a se3 element (R,t) is given, the marker is considered to be
      a transform marker.  You may not mix point and transform observations
      for a single marker ID.
    - marker_ids: a list of marker ID #'s l_1,...,l_n corresponding to
      each observation, e.g., the link index on which each marker lies.
    - observation_relative_errors: if you have an idea of the magnitude of
      each observation error, it can be placed into this list.  Must be
      a list of n floats, 3-lists (point markers), or 6-lists (transform
      markers).  By default, errors will be set proportionally to the
      observed distance between the camera and marker.
    - camera_initial_guess: if not None, an initial guess for the camera transform
    - marker_initial_guess: if not None, a dictionary containing initial guesses
      for the marker transforms
    - regularizationFactor: if nonzero, the optimization penalizes deviation
      of the estimated camera transform and marker transforms from zero
      proportionally to this factor.
    - maxIters: maximum number of iterations for optimization.
    - tolerance: optimization convergence tolerance.  Stops when the change of
      estimates falls below this threshold
    """
    if len(camera_link_transforms) != len(marker_ids):
        raise ValueError("Must provide the same number of marker IDs as camera transforms")
    if len(marker_link_transforms) != len(marker_ids):
        raise ValueError("Must provide the same number of marker IDs as marker transforms")
    if len(marker_observations) != len(marker_ids):
        raise ValueError("Must provide the same number of marker observations as marker transforms")
    #get all unique marker ids
    marker_id_list = list(set(marker_ids))
    #detect marker types
    marker_types = dict((v,None) for v in marker_id_list)
    for i,(obs,id) in enumerate(zip(marker_observations,marker_ids)):
        if len(obs)==3:
            if marker_types[id] == 't':
                raise ValueError("Provided both point and transform observations for observation #%d, id %s\n"%(i,str(id)))
            marker_types[id] = 'p'
        elif len(obs)==2 and len(obs[0])==9 and len(obs[1])==3:
            if marker_types[id] == 'p':
                raise ValueError("Provided both point and transform observations for observation #%d, id %s\n"%(i,str(id)))
            marker_types[id] = 't'
        else:
            raise ValueError("Invalid observation for observation #%d, id %s\n"%(i,str(id)))

    n = len(marker_observations)
    m = len(marker_id_list)

    #get all the observation weights
    observation_weights = []
    if observation_relative_errors is None:
        #default weights: proportional to distance
        for obs in marker_observations:
            if len(obs) == 3:
                observation_weights.append(1.0/vectorops.norm(obs))
            else:
                observation_weights.append(1.0/vectorops.norm(obs[1]))
        observation_weights = [1.0]*len(observation_weights)
    else:
        if len(observation_relative_errors) != n:
            raise ValueError("Invalid length of observation errors")
        for err in observation_relative_errors:
            if hasattr(err,'__iter__'):
                observation_weights.append([1.0/v for v in err])
            else:
                observation_weights.append(1.0/err)

    #initial guesses
    if camera_initial_guess == None:
        camera_initial_guess = se3.identity()
        if any(v == 't' for v in marker_types.itervalues()):
            #estimate camera rotation from point estimates because rotations are more prone to initialization failures
            point_observations = []
            marker_point_rel = []
            for i,obs in enumerate(marker_observations):
                if len(obs)==2:
                    point_observations.append(obs[1])
                else:
                    point_observations.append(obs)
                marker_point_rel.append(se3.mul(se3.inv(camera_link_transforms[i]),marker_link_transforms[i])[1])
            camera_initial_guess = (point_fit_rotation_3d(point_observations,marker_point_rel),[0.0]*3)
            print "Estimated camera rotation from points:",camera_initial_guess
    if marker_initial_guess == None:
        marker_initial_guess = dict((l,(se3.identity() if marker_types[l]=='t' else [0.0]*3)) for l in marker_id_list)
    else:
        marker_initial_guess = marker_initial_guess.copy()
        for l in marker_id_list:
            if l not in marker_initial_guess:
                marker_initial_guess[l] = (se3.identity() if marker_types[l]=='t' else [0.0]*3)
    camera_transform = camera_initial_guess
    marker_transforms = marker_initial_guess.copy()

    if DO_VISUALIZATION:
        rgroup = coordinates.addGroup("calibration")
        rgroup.addFrame("camera link",worldCoordinates=camera_link_transforms[-1])
        rgroup.addFrame("marker link",worldCoordinates=marker_link_transforms[-1])
        rgroup.addFrame("camera estimate",parent="camera link",relativeCoordinates=camera_transform)
        rgroup.addFrame("marker estimate",parent="marker link",relativeCoordinates=marker_transforms.values()[0])
        for i,obs in enumerate(marker_observations):
            rgroup.addFrame("obs"+str(i)+" estimate",parent="camera estimate",relativeCoordinates=obs)
        vis.add("coordinates",rgroup)
        vis.dialog()

    point_observations_only = all(marker_types[marker] == 'p' for marker in marker_id_list)
    xform_observations_only = all(marker_types[marker] == 't' for marker in marker_id_list)
    if not point_observations_only and not xform_observations_only:
        raise NotImplementedError("Can't calibrate camera from mixed point/transform markers yet")
    for iters in range(maxIters):
        #attempt to minimize the error on the following over all observations i
        #camera_link_transform(q_i)*camera_transform*observation_i = marker_link_transform(l_i,q_i)*marker_transform(l_i)
        #first, we'll assume the camera transform is fixed and then optimize the marker transforms.
        #then, we'll assume the marker transforms are fixed and then optimize the camera transform.
        #finally we'll check the error to detect convergence
        #1. Estimate marker transforms from current camera transform
        new_marker_transforms = dict((l,(TransformStats(zero=marker_initial_guess[l],prior=regularizationFactor) if marker_types[l]=='t' else VectorStats(value,zero=[0.0]*3,prior=RegularizationFactor))) for l in marker_id_list)
        for i in xrange(n):
            marker = marker_ids[i]
            Tclink = camera_link_transforms[i]
            Tmlink = marker_link_transforms[i]
            obs = marker_observations[i]
            Trel = se3.mul(se3.inv(Tmlink),se3.mul(Tclink,camera_transform))
            if marker_types[marker] == 't':
                estimate = se3.mul(Trel,obs)
            else:
                estimate = se3.apply(Trel,obs)
            new_marker_transforms[marker].add(estimate,observation_weights[i])
        print "ITERATION",iters
        #print "  ESTIMATED MARKER TRANSFORMS:",dict((k,v.average) for (k,v) in new_marker_transforms.iteritems())
        #2. Estimate camera transform from current marker transforms
        new_camera_transform = TransformStats(zero=camera_initial_guess,prior=regularizationFactor)
        if point_observations_only:
            #TODO: weighted point fitting
            relative_points = []
            for i in xrange(n):
                marker = marker_ids[i]
                Tclink = camera_link_transforms[i]
                Tmlink = marker_link_transforms[i]
                obs = marker_observations[i]
                pRel = se3.apply(se3.inv(Tclink),se3.apply(Tmlink,new_marker_transforms[marker].average))
                relative_points.append(pRel)
            new_camera_transform.add(point_fit_xform_3d(marker_observations,relative_points),sum(observation_weights))
        else:
            for i in xrange(n):
                marker = marker_ids[i]
                Tclink = camera_link_transforms[i]
                Tmlink = marker_link_transforms[i]
                obs = marker_observations[i]
                Trel = se3.mul(se3.inv(Tclink),se3.mul(Tmlink,new_marker_transforms[marker].average))
                estimate = se3.mul(Trel,se3.inv(obs))
                new_camera_transform.add(estimate,observation_weights[i])
        #print "  ESTIMATED CAMERA TRANSFORMS:",new_camera_transform.average
        #3. compute difference between last and current estimates
        diff = 0.0
        diff += vectorops.normSquared(se3.error(camera_transform,new_camera_transform.average))
        for marker in marker_id_list:
            if marker_types[marker]=='t':
                diff += vectorops.normSquared(se3.error(marker_transforms[marker],new_marker_transforms[marker].average))
            else:
                diff += vectorops.distanceSquared(marker_transforms[marker],new_marker_transforms[marker].average)
        camera_transform = new_camera_transform.average
        for marker in marker_id_list:
            marker_transforms[marker] = new_marker_transforms[marker].average
        if math.sqrt(diff) < tolerance:
            #converged!
            print "Converged with diff %g on iteration %d"%(math.sqrt(diff),iters)
            break
        print "  ESTIMATE CHANGE:",math.sqrt(diff)
        error = 0.0
        for i in xrange(n):
            marker = marker_ids[i]
            Tclink = camera_link_transforms[i]
            Tmlink = marker_link_transforms[i]
            obs = marker_observations[i]
            Tc = se3.mul(Tclink,camera_transform)
            if marker_types[marker] == 't':
                Tm = se3.mul(Tmlink,marker_transforms[marker])
                error += vectorops.normSquared(se3.error(se3.mul(Tc,obs),Tm))
            else:
                Tm = se3.apply(Tmlink,marker_transforms[marker])
                error += vectorops.distanceSquared(se3.apply(Tc,obs),Tm)
        print "  OBSERVATION ERROR:",math.sqrt(error)
        #raw_input()
        if DO_VISUALIZATION:
            rgroup.setFrameCoordinates("camera estimate",camera_transform)
            rgroup.setFrameCoordinates("marker estimate",marker_transforms.values()[0])
            for i,obs in enumerate(marker_observations):
                rgroup.setFrameCoordinates("obs"+str(i)+" estimate",obs)
            vis.add("coordinates",rgroup)
            vis.dialog()
    if math.sqrt(diff) >= tolerance:
        print "Max iters reached"
    error = 0.0
    for i in xrange(n):
        marker = marker_ids[i]
        Tclink = camera_link_transforms[i]
        Tmlink = marker_link_transforms[i]
        obs = marker_observations[i]
        Tc = se3.mul(Tclink,camera_transform)
        if marker_types[marker] == 't':
            Tm = se3.mul(Tmlink,marker_transforms[marker])
            error += vectorops.normSquared(se3.error(se3.mul(Tc,obs),Tm))
        else:
            Tm = se3.apply(Tmlink,marker_transforms[marker])
            error += vectorops.distanceSquared(se3.apply(Tc,obs),Tm)
    return (math.sqrt(error),camera_transform,marker_transforms)