(
sim: simple.Simulator,
args: SimulationArgs,
t: int,
q: np.ndarray,
v: np.ndarray,
)
| 91 | |
| 92 | |
| 93 | def plotContactSolver( |
| 94 | sim: simple.Simulator, |
| 95 | args: SimulationArgs, |
| 96 | t: int, |
| 97 | q: np.ndarray, |
| 98 | v: np.ndarray, |
| 99 | ): |
| 100 | if args.debug or t == args.debug_step: |
| 101 | stats: pin.SolverStats = sim.admm_constraint_solver.getStats() |
| 102 | if args.contact_solver == "ADMM": |
| 103 | solver = sim.admm_constraint_solver |
| 104 | if args.contact_solver == "PGS": |
| 105 | solver = sim.pgs_constraint_solver |
| 106 | stats = solver.getStats() |
| 107 | abs_res = solver.getAbsoluteConvergenceResidual() |
| 108 | rel_res = solver.getRelativeConvergenceResidual() |
| 109 | it = solver.getIterationCount() |
| 110 | if stats.size() > 0: |
| 111 | plt.cla() |
| 112 | title = ( |
| 113 | f"Step {t}, it = {it}, abs res = {abs_res:.2e}, rel res = {rel_res:.2e}" |
| 114 | ) |
| 115 | if args.contact_solver == "ADMM": |
| 116 | title += f", cholesky count: {stats.cholesky_update_count}" |
| 117 | plt.title(title) |
| 118 | plt.plot(stats.primal_feasibility, label="primal feas") |
| 119 | plt.plot(stats.dual_feasibility, label="dual feas") |
| 120 | plt.plot(stats.dual_feasibility_ncp, label="dual feas NCP") |
| 121 | if args.contact_solver == "ADMM": |
| 122 | plt.plot(stats.dual_feasibility_admm, label="dual feas ADMM") |
| 123 | plt.plot( |
| 124 | stats.dual_feasibility_constraint, label="dual feas constraint" |
| 125 | ) |
| 126 | plt.plot(stats.rho, label="rho") |
| 127 | if args.contact_solver == "PGS": |
| 128 | plt.plot(stats.complementarity, label="complementarity") |
| 129 | plt.yscale("log") |
| 130 | plt.legend() |
| 131 | plt.ion() |
| 132 | plt.show() |
| 133 | print(f"{t=}") |
| 134 | print(f"{q=}") |
| 135 | print(f"{v=}") |
| 136 | print(f"{sim.qnew=}") |
| 137 | print(f"{sim.vnew=}") |
| 138 | print(f"{sim.admm_constraint_solver.getIterationCount()=}") |
| 139 | print(f"{sim.pgs_constraint_solver.getIterationCount()=}") |
| 140 | print(f"{sim.constraints_problem.constraints_forces=}") |
| 141 | print(f"{sim.constraints_problem.constraints_problem_size=}") |
| 142 | print(f"{sim.constraints_problem.joint_friction_constraint_size=}") |
| 143 | print(f"{sim.constraints_problem.joint_limit_constraint_size=}") |
| 144 | print(f"{sim.constraints_problem.bilateral_constraints_size=}") |
| 145 | print(f"{sim.constraints_problem.weld_constraints_size=}") |
| 146 | print(f"{sim.constraints_problem.frictional_point_constraints_size=}") |
| 147 | print("Constraint solver timings: ", sim.getConstraintSolverCPUTimes().user) |
| 148 | input("[Press enter to continue]") |
| 149 | |
| 150 |
no test coverage detected