def plotZMPdifferences(cs_ref_iters, cs_iters, dt):
fig, ax = plt.subplots(len(cs_ref_iters), 2)
fig.canvas.set_window_title("Difference between the ZMP trajectories")
plt.suptitle("Difference between the ZMP trajectories from the centroidal solver and the wholebody after iterations of the dynamic filter.")
labels = ["X", "Y"]
colors = ['r', 'g', 'b']
max_values = [0, 0] # store the maximal value for each axis, used to set the y_axis range
min_values = [0, 0]
for i, cs in enumerate(cs_iters):
ref, timeline = discretizeCurve(cs_ref_iters[i].concatenateZMPtrajectories(), dt)
zmp = discretizeCurve(cs.concatenateZMPtrajectories(), dt)[0]
diff = zmp - ref
for j in range(2):
ax_sub = ax[i, j]
ax_sub.plot(timeline.T, diff[j,:], color = colors[j])
ax_sub.set_xlabel('time (s)')
ax_sub.set_ylabel(labels[j]+" values for iter " + str(i))
max_values[j] = max(np.amax(diff[j,:]), max_values[j])
min_values[j] = min(np.amin(diff[j,:]), min_values[j])
addVerticalLineContactSwitch(cs, ax_sub)
# set the ranges of each subplot
for i in range(len(cs_iters)):
for j in range(2):
ax_sub = ax[i, j]
ax_sub.set_ylim([min_values[j]*1.1, max_values[j]*1.1])
def plotZMP(cs_ref_iters, cs_iters, dt, circle_radius = 0.):
fig = plt.figure("ZMP(xy)")
ax = fig.gca()
plt.suptitle("ZMP(xy); dashed = ZMP reference ; lines = ZMP from wholebody ; green = feet placements")
ZMPs_t = []
ZMPs_ref = []
for cs in cs_iters:
ZMPs_t += [discretizeCurve(cs.concatenateZMPtrajectories(), dt)[0]]
for cs_ref in cs_ref_iters:
ZMPs_ref += [discretizeCurve(cs_ref.concatenateZMPtrajectories(), dt)[0]]
colors = ['b', 'g','r', 'c', 'm', 'y']
for k, ZMP_t in enumerate(ZMPs_t):
plt.plot(ZMP_t[0, :].T, ZMP_t[1, :].T, color=colors[k], label="iter "+str(k))
for k, ZMP_ref in enumerate(ZMPs_ref):
plt.plot(ZMP_ref[0, :].T, ZMP_ref[1, :].T, color=colors[k], linestyle='dashed')
plt.xlabel("x position (m)")
plt.ylabel("y position (m)")
plt.axis('equal')
plt.grid(False)
colors_circles = ['g', 'r', 'b', 'y']
effector_list = cs.getAllEffectorsInContact()
for p in cs.contactPhases:
# plot x for the center of the feets contact,
# and a circle of 1cm of radius around it (size of the flexibility) :
for eeName in p.effectorsInContact():
pos = p.contactPatch(eeName).placement.translation
color = colors_circles[effector_list.index(eeName)]
plt.plot(pos[0], pos[1], marker="x", markersize=20, color=color)
circle_r = plt.Circle((pos[0], pos[1]), circle_radius, color='g', fill=False)
ax.add_artist(circle_r)
ax.legend()
def plotZMP_t(cs_ref_iters, cs_iters, dt):
fig, ax = plt.subplots(len(cs_ref_iters), 2)
fig.canvas.set_window_title("Comparison between the ZMP trajectories")
plt.suptitle("Comparison between the ZMP trajectories after iterations of the dynamic filter. Dashed = from the centroidal solver ; line = from the wholebody")
labels = ["X", "Y"]
colors = ['r', 'g', 'b']
max_values = [0, 0] # store the maximal value for each axis, used to set the y_axis range
min_values = [0, 0]
for i, cs in enumerate(cs_iters):
ref, timeline = discretizeCurve(cs_ref_iters[i].concatenateZMPtrajectories(), dt)
zmp = discretizeCurve(cs.concatenateZMPtrajectories(), dt)[0]
for j in range(2):
if len(cs_ref_iters) > 1:
ax_sub = ax[i, j]
else:
ax_sub = ax[j]
ax_sub.plot(timeline.T, zmp[j,:], color = colors[j])
ax_sub.plot(timeline.T, ref[j,:], linestyle=':', color = colors[j])
ax_sub.set_xlabel('time (s)')
ax_sub.set_ylabel(labels[j]+" values for iter " + str(i))
max_value = max(np.amax(zmp[j,:]), np.amax(ref[j,:]))
max_values[j] = max(max_value, max_values[j])
min_value = min(np.amin(zmp[j,:]), np.amin(ref[j,:]))
min_values[j] = min(min_value, min_values[j])
addVerticalLineContactSwitch(cs, ax_sub)
# set the ranges of each subplot
for i in range(len(cs_iters)):
for j in range(2):
if len(cs_ref_iters) > 1:
ax_sub = ax[i, j]
else:
ax_sub = ax[j]
ax_sub.set_ylim([min_values[j]*1.1, max_values[j]*1.1])
def plotAMTrajChanges(cs0, cs1, dt):
labels = ["x", "y", "z", "dx", "dy", "dz"]
colors = ['r', 'g', 'b']
fig, ax = plt.subplots(2, 3)
fig.canvas.set_window_title("Comparison of AM trajectories")
fig.suptitle(
"Comparison of AM trajectories. dashed: before dynamic filter, line: after",
fontsize=20)
L0_t, timeline = discretizeCurve(cs0.concatenateLtrajectories(), dt)
dL0_t = discretizeCurve(cs0.concatenateDLtrajectories(), dt)[0]
values0 = np.vstack([L0_t, dL0_t])
L1_t = discretizeCurve(cs1.concatenateLtrajectories(), dt)[0]
dL1_t = discretizeCurve(cs1.concatenateDLtrajectories(), dt)[0]
values1 = np.vstack([L1_t, dL1_t])
for i in range(2): # line = L,dL
for j in range(3): # col = x,y,z
ax_sub = ax[i, j]
ax_sub.plot(timeline.T,
values0[i * 3 + j, :].T,
color=colors[j],
linestyle="dashed")
ax_sub.plot(timeline.T, values1[i * 3 + j, :].T, color=colors[j])
ax_sub.set_xlabel('time (s)')
ax_sub.set_ylabel(labels[i * 3 + j])
ax_sub.yaxis.grid()
addVerticalLineContactSwitch(cs0, ax_sub)
def plotAMTrajWithReferences(cs_ref, cs, dt):
labels = ["x", "y", "z", "dx", "dy", "dz"]
colors = ['r', 'g', 'b']
fig, ax = plt.subplots(2, 3)
fig.canvas.set_window_title("AM trajectory (dashed = reference)")
fig.suptitle("AM trajectory (dashed = reference)", fontsize=20)
L_t, timeline = discretizeCurve(cs.concatenateLtrajectories(), dt)
dL_t = discretizeCurve(cs.concatenateDLtrajectories(), dt)[0]
L_ref = discretizeCurve(cs_ref.concatenateLtrajectories(), dt)[0]
dL_ref = discretizeCurve(cs_ref.concatenateDLtrajectories(), dt)[0]
values = np.vstack([L_t, dL_t])
values_ref = np.vstack([L_ref, dL_ref])
for i in range(2): # line = L,dL
for j in range(3): # col = x,y,z
ax_sub = ax[i, j]
ax_sub.plot(timeline.T, values[i * 3 + j, :].T, color=colors[j])
ax_sub.plot(timeline.T,
values_ref[i * 3 + j, :].T,
color=colors[j],
linestyle="dashed")
ax_sub.set_xlabel('time (s)')
ax_sub.set_ylabel(labels[i * 3 + j])
ax_sub.yaxis.grid()
addVerticalLineContactSwitch(cs, ax_sub)
def plotZMPdifferences(cs_ref_iters, cs_iters, dt):
fig, ax = plt.subplots(len(cs_ref_iters), 2)
fig.canvas.set_window_title("Difference between the ZMP trajectories")
plt.suptitle(
"Difference between the ZMP trajectories from the centroidal solver and the wholebody after iterations of the dynamic filter."
)
labels = ["X", "Y"]
for i, cs in enumerate(cs_iters):
ref, timeline = discretizeCurve(
cs_ref_iters[i].concatenateZMPtrajectories(), dt)
zmp = discretizeCurve(cs.concatenateZMPtrajectories(), dt)[0]
diff = zmp - ref
for j in range(2):
ax_sub = ax[i, j]
ax_sub.plot(timeline.T, diff[j, :])
ax_sub.set_xlabel('time (s)')
ax_sub.set_ylabel(labels[j] + " values for iter " + str(i))
def plotAMTraj(cs, dt, name_suffixe = ""):
labels = ["x", "y", "z", "dx", "dy", "dz"]
colors = ['r', 'g', 'b']
fig, ax = plt.subplots(2, 3)
fig.canvas.set_window_title("AM trajectory"+name_suffixe)
fig.suptitle("AM trajectory"+name_suffixe, fontsize=20)
L_t, timeline = discretizeCurve(cs.concatenateLtrajectories(), dt)
dL_t = discretizeCurve(cs.concatenateDLtrajectories(), dt)[0]
values = np.vstack([L_t, dL_t])
for i in range(2): # line = L,dL
for j in range(3): # col = x,y,z
ax_sub = ax[i, j]
ax_sub.plot(timeline.T, values[i * 3 + j, :].T, color=colors[j])
ax_sub.set_xlabel('time (s)')
ax_sub.set_ylabel(labels[i * 3 + j])
ax_sub.yaxis.grid()
addVerticalLineContactSwitch(cs, ax_sub)
def plotAMdifference(cs_ref, cs, dt, name):
labels = ["x", "y", "z"]
colors = ['r', 'g', 'b']
fig, ax = plt.subplots(3, 1)
fig.canvas.set_window_title(name)
fig.suptitle(name, fontsize=20)
traj = cs.concatenateLtrajectories()
traj_ref = cs_ref.concatenateLtrajectories()
pos, timeline = discretizeCurve(traj, dt)
pos_ref = discretizeCurve(traj_ref, dt)[0]
error = pos - pos_ref
for i in range(3): # line = x,y,z
ax_sub = ax[i]
ax_sub.plot(timeline.T, error[i, :].T, color=colors[i])
ax_sub.set_xlabel('time (s)')
ax_sub.set_ylabel(labels[i])
ax_sub.yaxis.grid()
addVerticalLineContactSwitch(cs, ax_sub)
def plotCOMTraj(cs, dt, name_suffixe = ""):
labels = [
"x (m)", "y (m)", "z (m)", "dx (m/s)", "dy (m/s)", "dz (m/s)", "ddx (m/s^2)", "ddy (m/s^2)", "ddz (m/s^2)"
]
colors = ['r', 'g', 'b']
fig, ax = plt.subplots(3, 3)
fig.canvas.set_window_title("COM trajectory"+name_suffixe)
fig.suptitle("COM trajectory"+name_suffixe, fontsize=20)
c_t, timeline = discretizeCurve(cs.concatenateCtrajectories(), dt)
dc_t = discretizeCurve(cs.concatenateDCtrajectories(), dt)[0]
ddc_t = discretizeCurve(cs.concatenateDDCtrajectories(), dt)[0]
values = np.vstack([c_t, dc_t, ddc_t])
for i in range(3): # line = pos,vel,acc
for j in range(3): # col = x,y,z
ax_sub = ax[i, j]
ax_sub.plot(timeline.T, values[i * 3 + j, :].T, color=colors[j])
ax_sub.set_xlabel('time (s)')
ax_sub.set_ylabel(labels[i * 3 + j])
ax_sub.yaxis.grid()
addVerticalLineContactSwitch(cs, ax_sub)
def plotCOMTrajChanges(cs0, cs1, dt):
labels = [
"x (m)", "y (m)", "z (m)", "dx (m/s)", "dy (m/s)", "dz (m/s)", "ddx (m/s^2)", "ddy (m/s^2)", "ddz (m/s^2)"
]
colors = ['r', 'g', 'b']
fig, ax = plt.subplots(3, 3)
fig.canvas.set_window_title("Comparison of CoM trajectories")
fig.suptitle("Comparison of CoM trajectories. dashed: before dynamic filter, line: after", fontsize=20)
c0_t, timeline = discretizeCurve(cs0.concatenateCtrajectories(), dt)
dc0_t = discretizeCurve(cs0.concatenateDCtrajectories(), dt)[0]
ddc0_t = discretizeCurve(cs0.concatenateDDCtrajectories(), dt)[0]
values0 = np.vstack([c0_t, dc0_t, ddc0_t])
c1_t = discretizeCurve(cs1.concatenateCtrajectories(), dt)[0]
dc1_t = discretizeCurve(cs1.concatenateDCtrajectories(), dt)[0]
ddc1_t = discretizeCurve(cs1.concatenateDDCtrajectories(), dt)[0]
values1 = np.vstack([c1_t, dc1_t, ddc1_t])
for i in range(3): # line = pos,vel,acc
for j in range(3): # col = x,y,z
ax_sub = ax[i, j]
ax_sub.plot(timeline.T, values0[i * 3 + j, :].T, color=colors[j], linestyle="dashed")
ax_sub.plot(timeline.T, values1[i * 3 + j, :].T, color=colors[j])
ax_sub.set_xlabel('time (s)')
ax_sub.set_ylabel(labels[i * 3 + j])
ax_sub.yaxis.grid()
addVerticalLineContactSwitch(cs0, ax_sub)
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 plotKneeTorque(cs, dt, kneeIds, offset):
colors = ['r', 'g', 'b', 'y']
fig = plt.figure("Knee torque")
plt.suptitle("Knee torque")
ax = fig.gca()
ax.set_xlabel('time (s)')
ax.set_ylabel("Torque")
ax.yaxis.grid()
tau, timeline = discretizeCurve(cs.concatenateTauTrajectories(), dt)
for k, name in enumerate(kneeIds.keys()):
ax.plot(timeline.T, tau[kneeIds[name] - offset, :].T, color=colors[k], label=name)
# - 1 because it's the index in the configuration space, and here we are in the velocity space
addVerticalLineContactSwitch(cs, ax)
ax.legend()
def plotContactForces(cs, dt):
colors = ['r', 'g', 'b', 'y']
fig = plt.figure("Contact normal force")
plt.suptitle("Contact normal force")
ax = fig.gca()
ax.set_xlabel('time (s)')
ax.set_ylabel("Contact normal force (N)")
ax.yaxis.grid()
sum_f = None
for i, eeName in enumerate(cs.getAllEffectorsInContact()):
force, timeline = discretizeCurve(cs.concatenateNormalForceTrajectories(eeName), dt)
ax.plot(timeline.T, force.T, color=colors[i], label=eeName)
if sum_f is None:
sum_f = force
else:
sum_f += force
ax.plot(timeline.T, sum_f.T, color="k", label="sum")
addVerticalLineContactSwitch(cs, ax)
ax.legend()
def generateLimbRRTPath(q_init, q_end, phase_previous, phase, phase_next,
fullBody):
assert fullBody and "Cannot use limb-rrt method as fullBody object is not defined."
extraDof = int(fullBody.client.robot.getDimensionExtraConfigSpace())
q_init = q_init.tolist() + [0] * extraDof
q_end = q_end.tolist() + [0] * extraDof
# create nex states in fullBody corresponding to given configuration and set of contacts
s0 = createStateFromPhase(fullBody, phase_previous, q_init)
s1 = createStateFromPhase(fullBody, phase_next, q_end)
if not fullBody.isConfigValid(q_init)[0]:
logger.error("q_init invalid in limb-rrt : %s", q_init)
raise ValueError("init config is invalid in limb-rrt.")
if not fullBody.isConfigValid(q_end)[0]:
logger.error("q_end invalid in limb-rrt : %s", q_end)
raise ValueError("goal config is invalid in limb-rrt.")
logger.debug("New state added, q_init = %s", q_init)
logger.debug("New state added, q_end = %s", q_end)
if logger.getEffectiveLevel() >= logging.DEBUG:
contacts = fullBody.getAllLimbsInContact(s0)
fullBody.setCurrentConfig(fullBody.getConfigAtState(s0))
logger.debug("contact at init state : %s", contacts)
for contact in contacts:
effName = fullBody.dict_limb_joint[contact]
logger.debug("contact position for joint %s = %s", effName,
fullBody.getJointPosition(effName)[0:3])
contacts = fullBody.getAllLimbsInContact(s1)
fullBody.setCurrentConfig(fullBody.getConfigAtState(s1))
logger.debug("contact at end state : %s", contacts)
for contact in contacts:
effName = fullBody.dict_limb_joint[contact]
logger.debug("contact position for joint %s = %s", effName,
fullBody.getJointPosition(effName)[0:3])
# create a path in hpp corresponding to the discretized trajectory in phase :
dt = HPP_DT
c_t = discretizeCurve(phase.c_t, dt)[0]
v_t = discretizeCurve(phase.dc_t, dt)[0][:, :-1]
a_t = discretizeCurve(phase.ddc_t, dt)[0][:, :-1]
logger.debug("c shape : %s", c_t.shape)
logger.debug("v shape : %s", v_t.shape)
logger.debug("a shape : %s", a_t.shape)
logger.debug("c_t = %s", c_t.T.tolist())
logger.debug("dc_t = %s", v_t.T.tolist())
logger.debug("ddc_t = %s", a_t.T.tolist())
fullBody.setCurrentConfig(fullBody.getConfigAtState(s0))
com0_fb = fullBody.getCenterOfMass()
fullBody.setCurrentConfig(fullBody.getConfigAtState(s1))
com1_fb = fullBody.getCenterOfMass()
## TEST, FIXME (force com path to start/end in the com position found from q_init and q_end. :
c_t[:, 0] = np.array(com0_fb)
c_t[:, -1] = np.array(com1_fb)
com0 = c_t[:, 0].tolist()
com1 = c_t[:, -1].tolist()
logger.debug("init com : %s", com0_fb)
logger.debug("init ref : %s", com0)
logger.debug("end com : %s", com1_fb)
logger.debug("end ref : %s", com1)
path_com_id = fullBody.generateComTraj(c_t.T.tolist(), v_t.T.tolist(),
a_t.T.tolist(), dt)
logger.info("add com reference as hpp path with id : %d", path_com_id)
# project this states to the new COM position in phase :
"""
successProj=fullBody.projectStateToCOM(s0,com0)
assert successProj and "Error during projection of state"+str(s0)+" to com position : "+str(com0)
successProj=fullBody.projectStateToCOM(s1,com1)
assert successProj and "Error during projection of state"+str(s1)+" to com position : "+str(com1)
q_init = fullBody.getConfigAtState(s0)
q_end = fullBody.getConfigAtState(s1)
if extraDof:
q_init[-extraDof:] = [0]*extraDof #TODO : fix this in the projection method
q_end[-extraDof:] = [0]*extraDof
fullBody.setConfigAtState(s0,q_init)
fullBody.setConfigAtState(s1,q_end)
"""
# run limb-rrt in hpp :
logger.info("start limb-rrt ... ")
paths_rrt_ids = fullBody.comRRTOnePhase(s0, s1, path_com_id, 1)
logger.info("Limb-rrt returned path(s) : %s", paths_rrt_ids)
path_rrt_id = int(paths_rrt_ids[0])
return path_rrt_id
def FromContactSequenceWB(cs_ref, cs, dt):
p0 = cs.contactPhases[0]
nq = p0.q_t.dim()
nv = p0.dq_t.dim()
nu = p0.tau_t.dim()
eeNames = cs.getAllEffectorsInContact()
t_begin = p0.timeInitial
res = Result(nq, nv, dt, eeNames, cs=cs, t_begin=t_begin, nu=nu)
# fill all the array with discretization of the curves
res.q_t = discretizeCurve(cs.concatenateQtrajectories(), dt)[0]
res.dq_t = discretizeCurve(cs.concatenateDQtrajectories(), dt)[0]
res.ddq_t = discretizeCurve(cs.concatenateDDQtrajectories(), dt)[0]
res.tau_t = discretizeCurve(cs.concatenateTauTrajectories(), dt)[0]
res.c_t = discretizeCurve(cs.concatenateCtrajectories(), dt)[0]
res.dc_t = discretizeCurve(cs.concatenateDCtrajectories(), dt)[0]
res.ddc_t = discretizeCurve(cs.concatenateDDCtrajectories(), dt)[0]
res.L_t = discretizeCurve(cs.concatenateLtrajectories(), dt)[0]
res.dL_t = discretizeCurve(cs.concatenateDLtrajectories(), dt)[0]
res.c_reference = discretizeCurve(cs_ref.concatenateCtrajectories(), dt)[0]
res.dc_reference = discretizeCurve(cs_ref.concatenateDCtrajectories(),
dt)[0]
res.ddc_reference = discretizeCurve(cs_ref.concatenateDDCtrajectories(),
dt)[0]
res.L_reference = discretizeCurve(cs_ref.concatenateLtrajectories(), dt)[0]
res.dL_reference = discretizeCurve(cs_ref.concatenateDLtrajectories(),
dt)[0]
res.zmp_t = discretizeCurve(cs.concatenateZMPtrajectories(), dt)[0]
res.wrench_t = discretizeCurve(cs.concatenateWrenchTrajectories(), dt)[0]
res.zmp_reference = discretizeCurve(cs_ref.concatenateZMPtrajectories(),
dt)[0]
res.wrench_reference = discretizeCurve(
cs_ref.concatenateWrenchTrajectories(), dt)[0]
root_t = cs_ref.concatenateRootTrajectories()
res.waist_orientation_reference = discretizeSE3CurveQuaternion(root_t,
dt)[0]
res.d_waist_orientation_reference = discretizeDerivateCurve(root_t, dt,
1)[0][3:, :]
res.dd_waist_orientation_reference = discretizeDerivateCurve(
root_t, dt, 2)[0][3:, :]
for ee in eeNames:
res.contact_forces[ee] = discretizeCurve(
cs.concatenateContactForceTrajectories(ee), dt)[0]
res.contact_normal_force[ee] = discretizeCurve(
cs.concatenateNormalForceTrajectories(ee), dt)[0]
eff = cs.concatenateEffectorTrajectories(ee)
# append init/end constant trajectorie if required :
if eff.min() < t_begin:
eff_begin = constantSE3curve(eff.evaluateAsSE3(eff.min()), t_begin,
eff.min())
eff_mid = eff
eff = piecewise_SE3(eff_begin)
eff.append(eff_mid)
if eff.max() < cs.contactPhases[-1].timeFinal:
eff_end = constantSE3curve(eff.evaluateAsSE3(eff.max()), eff.max(),
cs.contactPhases[-1].timeFinal)
eff.append(eff_end)
res.effector_trajectories[ee] = discretizeSE3CurveToVec(eff, dt)[0]
res.d_effector_trajectories[ee] = discretizeDerivateCurve(eff, dt,
1)[0]
res.dd_effector_trajectories[ee] = discretizeDerivateCurve(eff, dt,
2)[0]
eff = cs_ref.concatenateEffectorTrajectories(ee)
# append init/end constant trajectorie if required :
if eff.min() < t_begin:
eff_begin = constantSE3curve(eff.evaluateAsSE3(eff.min()), t_begin,
eff.min())
eff_mid = eff
eff = piecewise_SE3(eff_begin)
eff.append(eff_mid)
if eff.max() < cs.contactPhases[-1].timeFinal:
eff_end = constantSE3curve(eff.evaluateAsSE3(eff.max()), eff.max(),
cs.contactPhases[-1].timeFinal)
eff.append(eff_end)
res.effector_references[ee] = discretizeSE3CurveToVec(eff, dt)[0]
res.d_effector_references[ee] = discretizeDerivateCurve(eff, dt, 1)[0]
res.dd_effector_references[ee] = discretizeDerivateCurve(eff, dt, 2)[0]
return res
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)
