def displayWBconfig(viewer, q_matrix): """ display a joint configuration stored as a numpy.array in the viewer :param viewer: An instance of hpp.gepetto.Viewer :param q_matrix: a numpy.array representing the robot configuration """ viewer(hppConfigFromMatrice(viewer.robot, q_matrix))
def displayWBatT(viewer, cs_wb, t): """ Display the configuration of the robot stored in the given contact sequence at the given time :param viewer: An instance of hpp.gepetto.Viewer :param cs_wb: the ContactSequence storing the joint trajectories :param t: the desired time index """ q = cs_wb.phaseAtTime(t).q_t(t) viewer(hppConfigFromMatrice(viewer.robot, q))
def createStateFromPhase(fullBody, phase, q=None): """ Create and add an RBPRM state to fullBody corresponding to the contacts defined in the given phase :param fullBody: an rbprm.FullBody object :param phase: a ContactPhase object, defining the active contacts :param q: if given, set the state wholebody configuration :return: the Id of the state in fullbody """ if q is None: q = utils.hppConfigFromMatrice(fullBody.client.robot, phase.q_init) effectorsInContact = phase.effectorsInContact() contacts = [] # contacts should contains the limb names, not the effector names list_effector = list(fullBody.dict_limb_joint.values()) for eeName in effectorsInContact: contacts += [list(fullBody.dict_limb_joint.keys())[list_effector.index(eeName)]] # FIXME : check if q is consistent with the contacts, and project it if not. return fullBody.createState(q, contacts)
def displayWBatT(viewer, res, t): viewer(hppConfigFromMatrice(viewer.robot, res.qAtT(t)))
def displayWBconfig(viewer, q_matrix): viewer(hppConfigFromMatrice(viewer.robot, q_matrix))
def displayWBatT(viewer, cs_wb, t): q = cs_wb.phaseAtTime(t).q_t(t) viewer(hppConfigFromMatrice(viewer.robot, q))