示例#1
0
文件: nav.py 项目: janfrs/kwc-ros-pkg
	def calc(self,t):
		mag  = 0.0
		ang  = 0.0

		total_weights = 0.0
		for n, w in self.nodes:
			polar = n.val(t)

			#Don't count this node as outputing anything if it does not output a command
			if not (ut.approx_equal(polar[0,0], 0.0) and ut.approx_equal(polar[1,0], 0.0)): 
				mag   += w * polar[0,0]
				ang   += w * polar[1,0]
				total_weights += w
		#print "total_weights", total_weights
		return np.matrix([mag, ang]).T / total_weights
示例#2
0
    def calc(self, t):
        mag = 0.0
        ang = 0.0

        total_weights = 0.0
        for n, w in self.nodes:
            polar = n.val(t)

            #Don't count this node as outputing anything if it does not output a command
            if not (ut.approx_equal(polar[0, 0], 0.0)
                    and ut.approx_equal(polar[1, 0], 0.0)):
                mag += w * polar[0, 0]
                ang += w * polar[1, 0]
                total_weights += w
        #print "total_weights", total_weights
        return np.matrix([mag, ang]).T / total_weights
示例#3
0
 def test_approx_equal_false(self):
     self.assertFalse(approx_equal(200, 200.6))
示例#4
0
 def test_approx_equal_true(self):
     self.assertTrue(approx_equal(200, 200.5))
示例#5
0
 def test_score_group(self):
     regionSNPs = {'foo': [0] * 10, 'bar': [0,0]}
     regionXS = {'foo': 0.2, 'bar': 1.}
     groups = {'1':('foo', 'bar')}
     s = analyze.score_groups_pooled(regionSNPs, groups, regionXS)
     assert util.approx_equal(s[0][0], 6.17e-09)
示例#6
0
 def test_score_pooled(self):
     regionSNPs = {'foo': [0] * 10}
     regionXS = {'foo': 0.2}
     s = analyze.score_pooled(regionSNPs, regionXS)
     assert util.approx_equal(s[0][0], 2.35e-14)
示例#7
0
 def ankle_different_width(self, l_ankle, r_ankle):
     l_width = abs(self.init_l_shoulder.x - l_ankle.x)
     r_width = abs(self.init_r_shoulder.x - r_ankle.x)
     return not approx_equal(l_width, r_width)
示例#8
0
 def hip_different_height(self, l_hip, r_hip):
     return not approx_equal(l_hip.y, r_hip.y + self.hip_offset)
示例#9
0
 def shoulder_different_height(self, l_shoulder, r_shoulder):
     return not approx_equal(l_shoulder.y,
                             r_shoulder.y + self.shoulder_offset)
示例#10
0
 def __cmp__(self, other):
   if util.approx_equal(self.x, other.x) and\
           util.approx_equal(self.y, other.y):
     return 0
   else:
     return -1