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))
