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