def displayFeetTrajFromResult(gui, sceneName, res, Robot):
for eeName in res.eeNames:
name = "feet_traj_" + str(eeName)
offset = Robot.dict_offset[eeName].translation
traj = res.effector_references[eeName][:3, :].copy()
for i in range(traj.shape[1]):
traj[:, i] += offset
traj = numpy2DToList(traj)
color = Robot.dict_limb_color_traj[eeName]
gui.addCurve(name, traj, color)
gui.addToGroup(name, sceneName)
gui.refresh()
def displayCOMTrajForPhase(phase, gui, name, name_group, color, dt):
"""
Display a curve representing the CoM 3D position stored in the given phase
:param phase: the ContactPhase object storing the CoM trajectory
:param gui: a gepetto.corbaserver.GraphicalInterface instance
:param name: the node name of the graphical object
:param name_group: the name of the group where the new node will be added
:param color: the color of the trajectory (R, G, B, A)
:param dt: the time step used to discretize the CoM trajectory
"""
c = numpy2DToList(discretizeCurve(phase.c_t, dt)[0])
gui.addCurve(name, c, color)
gui.addToGroup(name, name_group)
def displayFeetTrajFromResult(gui, sceneName, res, Robot):
"""
Display all effector trajectories stored in a mlp.utils.wholebody_result.Result struct
:param gui: a gepetto.corbaserver.GraphicalInterface instance
:param sceneName: the node group used to add the new graphical objects
:param res: a mlp.utils.wholebody_result.Result
:param Robot: a Robot configuration class (eg. the class defined in talos_rbprm.talos.Robot)
:return:
"""
for eeName in res.eeNames:
name = "feet_traj_" + str(eeName)
offset = Robot.dict_offset[eeName].translation
traj = res.effector_references[eeName][:3, :].copy()
for i in range(traj.shape[1]):
traj[:, i] += offset
traj = numpy2DToList(traj)
color = Robot.dict_limb_color_traj[eeName]
gui.addCurve(name, traj, color)
gui.addToGroup(name, sceneName)
gui.refresh()
def comPosListFromState(state_traj):
state = stdVecToMatrix(state_traj)
c = state[:3, :]
return numpy2DToList(c)
def displayCOMTrajForPhase(phase, gui, name, name_group, color, dt):
c = numpy2DToList(discretizeCurve(phase.c_t, dt)[0])
gui.addCurve(name, c, color)
gui.addToGroup(name, name_group)