def getFootSupportBodyMsg(self, side):
linkname = 'l_foot' if side == 'left' else 'r_foot'
supportBody = lcmdrc.support_body_t()
supportBody.utime = getUtime()
supportBody.body_id = int(self.ikPlanner.ikServer.comm.getFloatArray('links.%s' % linkname)[0])
supportBody.contact_pts = np.array(self.ikPlanner.ikServer.comm.getFloatArray('%s_pts(:)' % linkname)).reshape((3,-1), order='F')
supportBody.num_contact_pts = len(supportBody.contact_pts[0])
supportBody.use_support_surface = False
supportBody.override_contact_pts = True
return supportBody
def getPelvisSupportBodyMsg(self):
linkname = 'pelvis'
supportBody = lcmdrc.support_body_t()
supportBody.utime = getUtime()
supportBody.body_id = int(self.ikPlanner.ikServer.comm.getFloatArray('links.%s' % linkname)[0])
supportBody.contact_pts = np.array(self.ikPlanner.ikServer.comm.getFloatArray('pelvis_pts(:)')).reshape((3,-1), order='F')
supportBody.num_contact_pts = len(supportBody.contact_pts[0])
supportBody.use_support_surface = False
supportBody.override_contact_pts = True
return supportBody
def getFootSupportBodyMsg(self, side):
linkname = 'l_foot' if side == 'left' else 'r_foot'
supportBody = lcmdrc.support_body_t()
supportBody.utime = getUtime()
supportBody.body_id = int(self.ikPlanner.ikServer.comm.getFloatArray('links.%s' % linkname)[0])
supportBody.contact_pts = np.array(self.ikPlanner.ikServer.comm.getFloatArray('%s_pts(:)' % linkname)).reshape((3,-1), order='F')
supportBody.num_contact_pts = len(supportBody.contact_pts[0])
supportBody.use_support_surface = False
supportBody.override_contact_pts = True
return supportBody
def getPelvisSupportBodyMsg(self):
linkname = 'pelvis'
supportBody = lcmdrc.support_body_t()
supportBody.utime = getUtime()
supportBody.body_id = int(self.ikPlanner.ikServer.comm.getFloatArray('links.%s' % linkname)[0])
supportBody.contact_pts = np.array(self.ikPlanner.ikServer.comm.getFloatArray('pelvis_pts(:)')).reshape((3,-1), order='F')
supportBody.num_contact_pts = len(supportBody.contact_pts[0])
supportBody.use_support_surface = False
supportBody.override_contact_pts = True
return supportBody
def getHandSupportBodyMsg(self, side, support_surface):
linkname = 'l_hand' if side == 'left' else 'r_hand'
supportBody = lcmdrc.support_body_t()
supportBody.utime = getUtime()
supportBody.body_id = int(self.ikPlanner.ikServer.comm.getFloatArray('links.%s' % linkname)[0])
supportBody.contact_pts = self.ikPlanner.getPalmPoint(side=side).reshape(3,1)
supportBody.num_contact_pts = 1
supportBody.use_support_surface = True
supportBody.override_contact_pts = True
supportBody.support_surface = support_surface
return supportBody
def getHandSupportBodyMsg(self, side, support_surface):
linkname = 'l_hand' if side == 'left' else 'r_hand'
supportBody = lcmdrc.support_body_t()
supportBody.utime = getUtime()
supportBody.body_id = int(self.ikPlanner.ikServer.comm.getFloatArray('links.%s' % linkname)[0])
supportBody.contact_pts = self.ikPlanner.getPalmPoint(side=side).reshape(3,1)
supportBody.num_contact_pts = 1
supportBody.use_support_surface = True
supportBody.override_contact_pts = True
supportBody.support_surface = support_surface
return supportBody
