def getSupportLCMFromListOfSupports(self, supportsList,ts):
supports = [0]*len(ts)
for i in xrange(len(ts)):
supportElement = lcmdrc.support_element_t()
supportElement.utime = getUtime()
numBodies = len(supportsList[i])
supportElement.num_bodies = numBodies
supportBodies = []
for j in xrange(numBodies):
name = supportsList[i][j]
if name is 'l_foot':
supportBodies.append(self.getFootSupportBodyMsg('left'))
elif name is 'r_foot':
supportBodies.append(self.getFootSupportBodyMsg('right'))
elif name is 'l_hand':
supportBodies.append(self.getHandSupportBodyMsg('left'))
elif name is 'r_hand':
supportBodies.append(self.getHandSupportBodyMsg('right'))
elif name is 'pelvis':
supportBodies.append(self.getPelvisSupportBodyMsg())
else:
print "passed a support that isn't allowed"
supportElement.support_bodies = supportBodies
supports[i] = supportElement
return supports
def getSupports(self):
if self.publishPlansWithSupports:
supportElement = lcmdrc.support_element_t()
supportElement.utime = getUtime()
numBodies = 0
supportBodies = []
if self.pelvisSupportEnabled:
numBodies += 1
supportBodies.append(self.getPelvisSupportBodyMsg())
if self.leftFootSupportEnabled:
numBodies += 1
supportBodies.append(self.getFootSupportBodyMsg('left'))
if self.rightFootSupportEnabled:
numBodies += 1
supportBodies.append(self.getFootSupportBodyMsg('right'))
if self.leftHandSupportEnabled:
#numBodies += 2
numBodies += 1
supportBodies.append(
self.getHandSupportBodyMsg('left', [0, 0, 1, 0]))
#supportBodies.append(self.getHandSupportBodyMsg('left', [0, 0, -1, 0]))
if self.rightHandSupportEnabled:
#numBodies += 2
numBodies += 1
supportBodies.append(
self.getHandSupportBodyMsg('right', [0, 0, 1, 0]))
#supportBodies.append(self.getHandSupportBodyMsg('right', [0, 0, -1, 0]))
supportElement.num_bodies = numBodies
supportElement.support_bodies = supportBodies
return [supportElement]
def getSupports(self):
if self.publishPlansWithSupports:
supportElement = lcmdrc.support_element_t()
supportElement.utime = getUtime()
numBodies = 0
supportBodies = []
if self.pelvisSupportEnabled:
numBodies += 1
supportBodies.append(self.getPelvisSupportBodyMsg())
if self.leftFootSupportEnabled:
numBodies += 1
supportBodies.append(self.getFootSupportBodyMsg('left'))
if self.rightFootSupportEnabled:
numBodies += 1
supportBodies.append(self.getFootSupportBodyMsg('right'))
if self.leftHandSupportEnabled:
#numBodies += 2
numBodies += 1
supportBodies.append(self.getHandSupportBodyMsg('left', [0, 0, 1, 0]))
#supportBodies.append(self.getHandSupportBodyMsg('left', [0, 0, -1, 0]))
if self.rightHandSupportEnabled:
#numBodies += 2
numBodies += 1
supportBodies.append(self.getHandSupportBodyMsg('right', [0, 0, 1, 0]))
#supportBodies.append(self.getHandSupportBodyMsg('right', [0, 0, -1, 0]))
supportElement.num_bodies = numBodies
supportElement.support_bodies = supportBodies
return [supportElement]
def getSupportLCMFromListOfSupports(self, supportsList, ts):
supports = [0] * len(ts)
for i in xrange(len(ts)):
supportElement = lcmdrc.support_element_t()
supportElement.utime = getUtime()
numBodies = len(supportsList[i])
supportElement.num_bodies = numBodies
supportBodies = []
for j in xrange(numBodies):
name = supportsList[i][j]
if name is 'l_foot':
supportBodies.append(self.getFootSupportBodyMsg('left'))
elif name is 'r_foot':
supportBodies.append(self.getFootSupportBodyMsg('right'))
elif name is 'l_hand':
supportBodies.append(self.getHandSupportBodyMsg('left'))
elif name is 'r_hand':
supportBodies.append(self.getHandSupportBodyMsg('right'))
elif name is 'pelvis':
supportBodies.append(self.getPelvisSupportBodyMsg())
else:
print "passed a support that isn't allowed"
supportElement.support_bodies = supportBodies
supports[i] = supportElement
return supports
