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)