Commit cf17860d authored by Sébastien Kleff's avatar Sébastien Kleff
Browse files

fix conflicts upstream devel

parents bd1fd27d b6713925
Pipeline #20054 passed with stage
in 35 minutes and 59 seconds
......@@ -4,20 +4,15 @@ import numpy as np
from numpy.linalg import norm, pinv, inv, svd, eig # noqa: F401
import time
import numpy.random
import pybullet as pyb
# Local imports
import sobec
from sobec.walk_without_think.save_traj import save_traj
from sobec.walk_without_think.robot_wrapper import RobotWrapper
from sobec.walk_without_think import ocp
import mpcparams
from sobec.walk_without_think.config_mpc import configureMPCWalk
from sobec.pinbullet import SimuProxy
import sobec.viewer_multiple as viewer_multiple
from sobec.walk_without_think import miscdisp
import pybullet as pyb
import specific_params
import random
# from sobec.walk_without_think.talos_collections import jointToLockCollection
q_init = np.array(
......@@ -64,7 +59,7 @@ q_init = np.array(
]
)
q_init_robot = np.concatenate([q_init[:19], [q_init[24], q_init[24 + 8]]])
walkParams = mpcparams.PushParams("talos_low")
walkParams = specific_params.PushParams("talos_low")
# ## SIMU #############################################################################
# ## Load urdf model in pinocchio and bullet
......@@ -78,7 +73,7 @@ simu.setTalosDefaultFriction()
# ## OCP ########################################################################
# ## OCP ########################################################################
robot = RobotWrapper(simu.rmodel, contactKey="sole_link")
robot = sobec.wwt.RobotWrapper(simu.rmodel, contactKey="sole_link")
# robot.x0 = np.concatenate([q_init_robot, np.zeros(simu.rmodel.nv)])
assert len(walkParams.stateImportance) == robot.model.nv * 2
......@@ -111,19 +106,22 @@ contactPattern = (
)
# DDP for a full walk cycle, use as a standard pattern for the MPC.
ddp = ocp.buildSolver(robot, contactPattern, walkParams)
ddp = sobec.wwt.buildSolver(robot, contactPattern, walkParams)
problem = ddp.problem
x0s, u0s = ocp.buildInitialGuess(ddp.problem, walkParams)
x0s, u0s = sobec.wwt.buildInitialGuess(ddp.problem, walkParams)
ddp.setCallbacks([croc.CallbackVerbose()])
ddp.solve(x0s, u0s, 200)
with open("/tmp/pushreco-repr.ascii", "w") as f:
f.write(sobec.reprProblem(ddp.problem))
print("OCP described in /tmp/pushreco-repr.ascii")
mpcparams = sobec.MPCWalkParams()
configureMPCWalk(mpcparams, walkParams)
sobec.wwt.config_mpc.configureMPCWalk(mpcparams, walkParams)
mpc = sobec.MPCWalk(mpcparams, ddp.problem)
mpc.initialize(ddp.xs[: walkParams.Tmpc + 1], ddp.us[: walkParams.Tmpc])
# mpc.solver.setCallbacks([
# croc.CallbackVerbose(),
# miscdisp.CallbackMPCWalk(robot.contactIds)
# sobec.wwt.CallbackMPCWalk(robot.contactIds)
# ])
# #####################################################################################
# ### VIZ #############################################################################
......@@ -135,16 +133,14 @@ try:
viz.loadViewerModel()
gv = viz.viewer.gui
viz.display(simu.getState()[: robot.model.nq])
viz0 = viewer_multiple.GepettoGhostViewer(
simu.rmodel, simu.gmodel_col, simu.gmodel_vis, 0.8
)
viz0 = sobec.GepettoGhostViewer(simu.rmodel, simu.gmodel_col, simu.gmodel_vis, 0.8)
viz0.hide()
except (ImportError, AttributeError):
print("No viewer")
# ## MAIN LOOP ##################################################################
hx = []
hx = [simu.getState()]
hu = []
hxs = []
......@@ -178,11 +174,12 @@ nextSolve = -1
hiter = []
# FOR LOOP
for s in range(1500): # int(20.0 / walkParams.DT)):
mpcPeriod = int(walkParams.DT / 1e-3)
for s in range(walkParams.Tsimu):
# ###############################################################################
# # For timesteps without MPC updates
for k in range(int(walkParams.DT / 1e-3)):
for k in range(mpcPeriod):
# Get simulation state
x = simu.getState()
......@@ -240,14 +237,13 @@ for s in range(1500): # int(20.0 / walkParams.DT)):
print(
"{:4d} {} {:4d} reg={:.3} a={:.3} solveTime={:.3}".format(
s,
miscdisp.dispocp(mpc.problem, robot.contactIds),
sobec.wwt.dispocp(mpc.problem, robot.contactIds),
mpc.solver.iter,
mpc.solver.x_reg,
mpc.solver.stepLength,
solve_time,
)
)
# if not s % 10:
viz.display(simu.getState()[: robot.model.nq])
# Before each takeoff, the robot display the previewed movement (3 times)
......@@ -272,7 +268,7 @@ croc.stop_watch_report(3)
# #####################################################################################
if walkParams.saveFile is not None:
save_traj(np.array(hx), filename=walkParams.saveFile)
sobec.wwt.save_traj(np.array(hx), filename=walkParams.saveFile)
# #####################################################################################
# #####################################################################################
......@@ -280,9 +276,9 @@ if walkParams.saveFile is not None:
# The 2 next import must not be included **AFTER** pyBullet starts.
import matplotlib.pylab as plt # noqa: E402,F401
import sobec.walk_without_think.plotter as walk_plotter # noqa: E402
import sobec.walk_without_think.plotter # noqa: E402,F401
plotter = walk_plotter.WalkPlotter(robot.model, robot.contactIds)
plotter = sobec.wwt.plotter.WalkPlotter(robot.model, robot.contactIds)
plotter.setData(contactPattern, np.array(hx), None, None)
target = problem.terminalModel.differential.costs.costs[
......
import sobec.walk_without_think.params as swparams
import numpy as np
class WalkParams(swparams.WalkParams):
conePenaltyWeight = 20
flyHighWeight = 20
impactVelocityWeight = 200
refFootFlyingAltitude = 0.03
flyHighSlope = 5 / refFootFlyingAltitude
vcomRef = np.array([0.1, 0, 0])
baumgartGains = np.array([0, 50])
minimalNormalForce = 0.0
def __init__(self, name="talos_low"):
swparams.WalkParams.__init__(self, name)
# ### KEPT FOR REFERENCES ##################################################
# ### KEPT FOR REFERENCES ##################################################
# ### KEPT FOR REFERENCES ##################################################
basisQWeight = [0, 0, 0, 50, 50, 0]
legQWeight = [5, 5, 1, 2, 1, 1]
torsoQWeight = [10, 10]
armQWeight = [3, 3]
basisVWeight = [0, 0, 0, 3, 3, 1] # ## was 003331
legVWeight = [1] * 6
torsoVWeight = [20] * 2
armVWeight = [2] * 2
legUWeight = [1, 1, 1, 1, 1, 1]
torsoUWeight = [1, 1]
armUWeight = [1, 1]
class WalkParamsOld:
DT = 0.010
stateImportance = np.array(
basisQWeight
+ legQWeight
+ legQWeight
+ armQWeight
+ basisVWeight
+ legVWeight
+ legVWeight
+ armVWeight
)
stateTerminalImportance = np.array([3, 3, 0, 0, 0, 30] + [0] * 14 + [1] * 20)
controlImportance = np.array(legUWeight * 2 + armUWeight)
# ## Gains for force continuity: wfref for tracking the reference, wfcont for time
# difference
refTorqueWeight = 0
refStateWeight = 1e-1
flatBaseWeight = 0 # 20
forceImportance = np.array([1, 1, 0.1, 10, 10, 2])
coneAxisWeight = 2e-4
comWeight = 0 # 20
vcomImportance = np.array([0.0, 0, 1])
vcomWeight = 1
acomWeight = 0 # 16*DT
copWeight = 2
verticalFootVelWeight = 20
footVelWeight = 0 # 20
footAccWeight = 0 # 2
flyHighWeight = 20
groundColWeight = 200
conePenaltyWeight = 20
feetCollisionWeight = 1000
lowbandwidthweight = 0 # 2e-1
minTorqueDiffWeight = 0 # 2e-2
refForceWeight = 10
contiForceWeight = 0
impactAltitudeWeight = 20000
impactVelocityWeight = 200
impactRotationWeight = 200
refMainJointsAtImpactWeight = 0 # 2e2 # For avoinding crossing legs
stateTerminalWeight = 20 # 2000
terminalNoVelocityWeight = 2000
terminalXTargetWeight = 0 # ##DDP## 2000
enforceMinimalFootDistance = False
refFootFlyingAltitude = 3e-2
flyHighSlope = 5 / refFootFlyingAltitude
footMinimalDistance = 0.2 # (.17 is the max value wrt initial config)
soleCollision = True
towCollision = False
heelCollision = False
mainJointIds = [
"leg_%s_%s_joint" % (side, idx)
for side in ["left", "right"]
for idx in [1, 2, 4]
]
vcomRef = np.array([0.1, 0, 0])
# vcomSelection = [0, 1, 2]
vcomImportance = np.array([0.0, 0, 1])
footSize = 0.05
kktDamping = 0 # 1e-6
baumgartGains = np.array([0, 50])
solver_th_stop = 1e-3
guessFile = "/tmp/ddp.npy"
saveFile = "/tmp/ddp.npy"
This diff is collapsed.
Impact 48 at time 80
Impact 34 at time 140
No valid solution file, build quasistatic initial guess!
iter cost stop grad xreg ureg step ||ffeas||
0 3.54637e+01 3.47853e-01 1.93380e+02 1.00000e-09 1.00000e-09 0.5000 5.45697e-14
1 1.38786e+01 4.23981e+05 6.74195e+01 1.00000e-09 1.00000e-09 0.5000 2.72855e-14
2 7.93524e+00 2.22444e+05 2.43715e+01 1.00000e-09 1.00000e-09 1.0000 1.36557e-14
3 2.71820e+00 3.61193e+05 1.26412e+01 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
4 1.61051e+00 6.37229e+04 2.25905e+00 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
5 1.58274e+00 3.24962e+02 5.46976e-02 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
6 1.58129e+00 4.82504e+01 2.97934e-03 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
7 1.58030e+00 2.49521e+01 1.66924e-03 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
8 1.57997e+00 4.54509e+00 4.63392e-04 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
9 1.57982e+00 1.54359e+00 1.81043e-04 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
iter cost stop grad xreg ureg step ||ffeas||
10 1.57975e+00 2.15457e-01 6.32401e-05 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
11 1.57972e+00 3.89219e-02 2.56825e-05 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
12 1.57971e+00 8.10147e-03 1.08885e-05 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
13 1.57970e+00 1.47592e-03 4.65527e-06 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
14 1.57969e+00 2.86889e-04 2.01065e-06 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
OCP described in /tmp/bullet-repr.ascii
iter cost stop grad xreg ureg step ||ffeas||
0 1.01263e+00 1.08048e+02 2.09743e-01 1.00000e-06 1.00000e-06 0.5000 6.60142e-01
1 1.01190e+00 2.70421e+01 -1.48311e-02 1.00000e-06 1.00000e-06 1.0000 3.30071e-01
0 ==========================⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======_______ 2 reg=1e-06 a=1.0 solveTime=0.0461
iter cost stop grad xreg ureg step ||ffeas||
0 9.92022e-01 3.03798e+01 -3.56762e-02 1.00000e-06 1.00000e-06 1.0000 4.89623e-01
1 9.91203e-01 6.13486e+01 3.04722e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
1 =========================⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======________ 2 reg=1e-06 a=1.0 solveTime=0.0489
iter cost stop grad xreg ureg step ||ffeas||
0 9.81304e-01 2.38393e+01 2.81279e-01 1.00000e-06 1.00000e-06 1.0000 5.51224e-01
1 9.74100e-01 1.55683e+01 1.61112e-02 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
2 ========================⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======_________ 2 reg=1e-06 a=1.0 solveTime=0.061
iter cost stop grad xreg ureg step ||ffeas||
0 9.74937e-01 2.44712e+01 2.28492e-01 1.00000e-06 1.00000e-06 1.0000 5.46355e-01
1 9.66341e-01 4.59917e+00 1.87216e-02 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
3 =======================⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======__________ 2 reg=1e-06 a=1.0 solveTime=0.0607
iter cost stop grad xreg ureg step ||ffeas||
0 9.68828e-01 3.13958e+01 6.90985e-02 1.00000e-06 1.00000e-06 0.5000 5.42876e-01
1 9.66235e-01 8.81637e+00 -7.05940e-03 1.00000e-06 1.00000e-06 1.0000 2.71438e-01
4 ======================⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======___________ 2 reg=1e-06 a=1.0 solveTime=0.0651
iter cost stop grad xreg ureg step ||ffeas||
0 9.71284e-01 4.06299e+01 -6.71486e-03 1.00000e-06 1.00000e-06 0.5000 4.93534e-01
1 9.72550e-01 1.57821e+01 -1.47319e-02 1.00000e-06 1.00000e-06 1.0000 2.46767e-01
5 =====================⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======____________ 2 reg=1e-06 a=1.0 solveTime=0.0633
iter cost stop grad xreg ureg step ||ffeas||
0 9.83118e-01 7.08196e+01 3.96867e-03 1.00000e-06 1.00000e-06 1.0000 4.90897e-01
1 9.82005e-01 2.69268e+01 2.79743e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
6 ====================⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======_____________ 2 reg=1e-06 a=1.0 solveTime=0.0586
iter cost stop grad xreg ureg step ||ffeas||
0 9.98837e-01 8.29128e+01 7.62831e-02 1.00000e-06 1.00000e-06 1.0000 5.23039e-01
1 9.94027e-01 2.97644e+00 9.81268e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
7 ===================⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======______________ 2 reg=1e-06 a=1.0 solveTime=0.0716
iter cost stop grad xreg ureg step ||ffeas||
0 1.01189e+00 1.10478e+02 5.12232e-02 1.00000e-06 1.00000e-06 1.0000 5.10459e-01
1 1.00787e+00 2.37629e+00 8.03947e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
8 ==================⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======_______________ 2 reg=1e-06 a=1.0 solveTime=0.0749
iter cost stop grad xreg ureg step ||ffeas||
0 1.02634e+00 1.40334e+02 3.23870e-02 1.00000e-06 1.00000e-06 1.0000 4.97511e-01
1 1.02285e+00 2.07638e+00 6.90902e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
9 =================⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======________________ 2 reg=1e-06 a=1.0 solveTime=0.0857
iter cost stop grad xreg ureg step ||ffeas||
0 1.04177e+00 1.72770e+02 1.60023e-02 1.00000e-06 1.00000e-06 1.0000 4.82495e-01
1 1.03857e+00 2.19211e+00 6.37256e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
10 ================⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======_________________ 2 reg=1e-06 a=1.0 solveTime=0.0709
iter cost stop grad xreg ureg step ||ffeas||
0 1.05580e+00 2.02608e+02 -2.28441e-02 1.00000e-06 1.00000e-06 1.0000 4.68178e-01
1 1.05502e+00 6.31146e-01 1.62974e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
11 ===============⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======__________________ 2 reg=1e-06 a=1.0 solveTime=0.074
iter cost stop grad xreg ureg step ||ffeas||
0 1.07270e+00 2.30447e+02 -3.54617e-02 1.00000e-06 1.00000e-06 1.0000 4.64257e-01
1 1.07195e+00 3.03224e-01 1.53868e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
12 ==============⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======___________________ 2 reg=1e-06 a=1.0 solveTime=0.0609
iter cost stop grad xreg ureg step ||ffeas||
0 1.08999e+00 2.61493e+02 -4.43759e-02 1.00000e-06 1.00000e-06 1.0000 4.61450e-01
1 1.08926e+00 2.56709e-01 1.51616e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
13 =============⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======____________________ 2 reg=1e-06 a=1.0 solveTime=0.0641
iter cost stop grad xreg ureg step ||ffeas||
0 1.10758e+00 2.92090e+02 -5.29686e-02 1.00000e-06 1.00000e-06 1.0000 4.59310e-01
1 1.10685e+00 2.09476e-01 1.52459e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
14 ============⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻,=======_____________________ 2 reg=1e-06 a=1.0 solveTime=0.0671
*** PROFILING RESULTS [ms] min avg max lastTime nSamples totalTime ***
ShootingProblem::calc 2.615 3.622 4.307 3.947 15 54.324
ShootingProblem::calcDiff 12.222 18.761 27.035 20.138 30 562.841
SolverDDP::Qu 0.000 0.000 0.003 0.000 2790 0.902
SolverDDP::Quu 0.001 0.003 0.018 0.003 2790 9.655
SolverDDP::Quu_inv 0.001 0.001 0.015 0.001 2790 4.010
SolverDDP::Quu_inv_Qux 0.002 0.005 0.046 0.005 2790 12.994
SolverDDP::Qx 0.001 0.001 0.016 0.001 2790 2.671
SolverDDP::Qxu 0.004 0.008 0.060 0.008 2790 21.595
SolverDDP::Qxx 0.011 0.020 0.125 0.022 2790 55.496
SolverDDP::Vxx 0.003 0.007 0.048 0.007 2790 18.280
SolverDDP::backwardPass 4.930 7.505 11.717 8.025 30 225.161
SolverDDP::calcDiff 12.513 20.614 31.242 20.152 30 618.420
SolverDDP::computeDirection 17.924 28.125 39.155 28.182 30 843.756
SolverDDP::computeGains 0.004 0.008 0.076 0.007 2790 22.142
SolverDDP::resizeData 0.026 0.035 0.046 0.038 15 0.525
SolverDDP::tryStep 2.280 3.535 4.889 3.630 33 116.650
SolverFDDP::forwardPass 2.279 3.531 4.883 3.627 33 116.535
SolverFDDP::solve 45.906 64.632 85.142 66.913 15 969.484
Save "/tmp/sobec.npy"!
Run ```plt.ion(); plt.show()``` to display the plots.
Run ```play()``` to visualize the motion.
This diff is collapsed.
Impact 34 at time 121
Impact 48 at time 212
Impact 34 at time 80
Impact 48 at time 140
No valid solution file, build quasistatic initial guess!
iter cost stop grad xreg ureg step ||ffeas||
0 3.68184e+01 1.22241e-01 1.94542e+02 1.00000e-09 1.00000e-09 0.5000 3.63798e-14
1 1.46603e+01 1.88042e+05 7.01491e+01 1.00000e-09 1.00000e-09 0.5000 1.81938e-14
2 8.87914e+00 1.00641e+05 2.59557e+01 1.00000e-09 1.00000e-09 1.0000 9.10383e-15
3 2.91196e+00 1.87009e+05 1.45504e+01 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
4 1.59402e+00 3.47668e+04 2.66628e+00 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
5 1.57480e+00 7.97194e+01 4.13234e-02 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
6 1.57205e+00 6.69180e+01 7.34051e-03 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
7 1.57015e+00 3.46821e+01 3.98204e-03 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
8 1.56941e+00 1.08570e+01 1.23328e-03 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
9 1.56919e+00 1.29597e+00 2.86165e-04 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
iter cost stop grad xreg ureg step ||ffeas||
10 1.56909e+00 2.66630e-01 1.04232e-04 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
11 1.56904e+00 5.72569e-02 4.32459e-05 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
12 1.56902e+00 9.75377e-03 1.91227e-05 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
13 1.56901e+00 1.96185e-03 8.90809e-06 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
14 1.56900e+00 4.22866e-04 4.32065e-06 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
iter cost stop grad xreg ureg step ||ffeas||
0 1.19505e+00 1.12330e+02 -3.63077e-02 1.00000e-06 1.00000e-06 1.0000 5.03606e-01
1 1.19433e+00 1.66608e+00 1.37258e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
1 ========================================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.19618e+00 9.34204e+01 -2.72209e-02 1.00000e-06 1.00000e-06 1.0000 4.11720e-01
1 1.19584e+00 3.90513e-02 5.95847e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
2 =======================================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.19861e+00 9.84032e+01 -2.91107e-02 1.00000e-06 1.00000e-06 1.0000 4.00470e-01
1 1.19829e+00 2.00897e-02 5.50747e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
3 ======================================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.20185e+00 1.05868e+02 -3.22932e-02 1.00000e-06 1.00000e-06 1.0000 4.00524e-01
1 1.20154e+00 1.86375e-02 5.47519e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
4 =====================================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.20578e+00 1.13022e+02 -3.52815e-02 1.00000e-06 1.00000e-06 1.0000 4.00017e-01
1 1.20547e+00 1.71638e-02 5.49208e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
5 ====================================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.21032e+00 1.20013e+02 -3.81800e-02 1.00000e-06 1.00000e-06 1.0000 3.99489e-01
1 1.21000e+00 1.55768e-02 5.54926e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
6 ===================================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.21539e+00 1.26857e+02 -4.09903e-02 1.00000e-06 1.00000e-06 1.0000 4.07980e-01
1 1.21507e+00 1.42874e-02 5.63907e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
7 ==================================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.22095e+00 1.33527e+02 -4.36949e-02 1.00000e-06 1.00000e-06 1.0000 4.19331e-01
1 1.22062e+00 1.35904e-02 5.75691e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
8 =================================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.22692e+00 1.40004e+02 -4.62646e-02 1.00000e-06 1.00000e-06 1.0000 4.30101e-01
1 1.22659e+00 1.35868e-02 5.89821e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
9 ================================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.23328e+00 1.46275e+02 -4.86846e-02 1.00000e-06 1.00000e-06 1.0000 4.40308e-01
1 1.23294e+00 1.43375e-02 6.06530e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
10 ===============================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.23998e+00 1.52331e+02 -5.09365e-02 1.00000e-06 1.00000e-06 1.0000 4.49973e-01
1 1.23963e+00 1.56781e-02 6.25486e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
11 ==============================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.24698e+00 1.58164e+02 -5.29805e-02 1.00000e-06 1.00000e-06 1.0000 4.59118e-01
1 1.24663e+00 1.75159e-02 6.45710e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
12 =============================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.25425e+00 1.63767e+02 -5.47881e-02 1.00000e-06 1.00000e-06 1.0000 4.67760e-01
1 1.25389e+00 2.00438e-02 6.68112e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
13 ============================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.26176e+00 1.69136e+02 -5.63280e-02 1.00000e-06 1.00000e-06 1.0000 4.75918e-01
1 1.26138e+00 2.30777e-02 6.92104e-04 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
14 ===========================________________________________________________________________________________'===========⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
OCP described in /tmp/mpc-repr.ascii
iter cost stop grad xreg ureg step ||ffeas||
0 3.65041e+01 3.57450e-01 1.93371e+02 1.00000e-09 1.00000e-09 0.5000 5.45697e-14
1 1.42291e+01 4.34799e+05 6.94946e+01 1.00000e-09 1.00000e-09 0.5000 2.72855e-14
2 8.41443e+00 2.19950e+05 2.50679e+01 1.00000e-09 1.00000e-09 1.0000 1.36557e-14
3 2.68427e+00 3.90047e+05 1.35945e+01 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
4 1.61219e+00 6.23975e+04 2.18624e+00 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
5 1.58540e+00 3.16422e+02 5.32696e-02 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
6 1.58391e+00 6.45649e+01 3.79512e-03 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
7 1.58248e+00 4.58062e+01 2.56086e-03 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
8 1.58210e+00 6.27984e+00 5.83153e-04 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
9 1.58191e+00 2.63613e+00 2.38250e-04 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
iter cost stop grad xreg ureg step ||ffeas||
10 1.58184e+00 2.95329e-01 7.40624e-05 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
11 1.58180e+00 5.34363e-02 2.91721e-05 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
12 1.58178e+00 1.33164e-02 1.22721e-05 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
13 1.58177e+00 2.19507e-03 5.13547e-06 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
14 1.58177e+00 4.07855e-04 2.24506e-06 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
iter cost stop grad xreg ureg step ||ffeas||
0 1.03475e+00 1.18616e+02 2.34466e-01 1.00000e-06 1.00000e-06 0.5000 6.51157e-01
1 1.02646e+00 2.99760e+01 7.90355e-05 1.00000e-06 1.00000e-06 1.0000 3.25578e-01
0 ==========================_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.01315e+00 4.37125e+01 -2.45641e-02 1.00000e-06 1.00000e-06 1.0000 4.86912e-01
1 1.01272e+00 2.31063e+01 2.26915e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
1 =========================_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.00943e+00 2.60321e+01 2.75061e-01 1.00000e-06 1.00000e-06 1.0000 5.48504e-01
1 1.00403e+00 2.63885e+00 1.38473e-02 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
2 ========================_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.00364e+00 3.06609e+01 2.08549e-01 1.00000e-06 1.00000e-06 1.0000 5.42788e-01
1 9.98063e-01 4.16534e+00 1.36814e-02 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
3 =======================_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 9.97443e-01 3.26404e+01 7.19607e-02 1.00000e-06 1.00000e-06 0.5000 5.39475e-01
1 9.94680e-01 1.00684e+01 -9.17137e-03 1.00000e-06 1.00000e-06 1.0000 2.69737e-01
4 ======================_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 9.96376e-01 3.06922e+01 -1.03556e-03 1.00000e-06 1.00000e-06 1.0000 4.90068e-01
1 9.95660e-01 1.77537e+01 2.67429e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
5 =====================_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 9.97919e-01 5.32626e+01 2.87631e-02 1.00000e-06 1.00000e-06 0.2500 5.29491e-01
1 9.97679e-01 3.03224e+01 1.68792e-02 1.00000e-06 1.00000e-06 0.5000 3.97118e-01
6 ====================_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=0.5
iter cost stop grad xreg ureg step ||ffeas||
0 1.00552e+00 5.31687e+01 -3.51310e-02 1.00000e-06 1.00000e-06 1.0000 4.83838e-01
1 1.00356e+00 5.73408e+01 3.87095e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
7 ===================_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.00644e+00 9.92405e+01 1.57563e-02 1.00000e-05 1.00000e-05 0.0020 5.13893e-01
1 1.01208e+00 9.92400e+01 -2.29888e-02 1.00000e-06 1.00000e-06 1.0000 5.13893e-01
8 ==================_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.02143e+00 6.96356e+01 -1.72800e-02 1.00000e-06 1.00000e-06 1.0000 3.86753e-01
1 1.01998e+00 9.42466e+01 3.36807e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
9 =================_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.03443e+00 1.56445e+02 3.47339e-02 1.00000e-06 1.00000e-06 1.0000 4.92900e-01
1 1.02996e+00 2.67730e+00 9.04659e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
10 ================_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.04346e+00 1.87249e+02 3.49607e-03 1.00000e-06 1.00000e-06 1.0000 4.74408e-01
1 1.04120e+00 2.84629e+00 4.42082e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
11 ===============_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.05474e+00 2.11905e+02 -2.59522e-02 1.00000e-06 1.00000e-06 1.0000 4.62932e-01
1 1.05382e+00 4.50435e-01 1.77906e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
12 ==============_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.06833e+00 2.38781e+02 -3.76661e-02 1.00000e-06 1.00000e-06 1.0000 4.60276e-01
1 1.06747e+00 2.43054e-01 1.66879e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
13 =============_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
iter cost stop grad xreg ureg step ||ffeas||
0 1.08281e+00 2.67998e+02 -4.61808e-02 1.00000e-06 1.00000e-06 1.0000 4.58638e-01
1 1.08197e+00 2.19606e-01 1.63363e-03 1.00000e-06 1.00000e-06 1.0000 0.00000e+00
14 ============_____________________________________________________'=======⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻⎻ 2 reg=1e-06 a=1.0
Run ```plt.ion(); plt.show()``` to display the plots.
Save "/tmp/sobec.npy"!
This diff is collapsed.
iter cost stop grad xreg ureg step ||ffeas||
0 1.13127e+03 1.22089e+04 3.86899e+04 1.00000e-09 1.00000e-09 1.0000 3.63798e-14
1 1.01397e+03 4.74084e+06 2.21319e+03 1.00000e-09 1.00000e-09 0.2500 0.00000e+00
2 6.73485e+02 1.19856e+07 1.99642e+03 1.00000e-09 1.00000e-09 0.2500 0.00000e+00
3 5.77113e+02 4.08214e+04 1.32123e+03 1.00000e-09 1.00000e-09 0.2500 0.00000e+00
4 4.16121e+02 2.76617e+04 1.13557e+03 1.00000e-09 1.00000e-09 0.5000 0.00000e+00
5 7.21062e+01 1.31691e+04 8.17557e+02 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
6 5.12454e+01 2.29290e+03 1.38321e+02 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
7 2.89710e+01 2.21991e+04 9.87199e+01 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
8 4.49633e+00 1.34988e+07 5.44611e+01 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
9 1.82677e+00 8.93706e+03 6.03300e+00 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
iter cost stop grad xreg ureg step ||ffeas||
10 1.38179e+00 8.23424e+01 9.15747e-01 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
11 1.33603e+00 1.46793e-01 1.07233e-01 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
12 1.29803e+00 7.54751e-03 6.97905e-02 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
13 1.26107e+00 5.23950e-03 7.13683e-02 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
14 1.24276e+00 5.25361e-03 3.71647e-02 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
15 1.24276e+00 6.06484e-03 1.52731e-03 1.00000e-08 1.00000e-08 0.0020 0.00000e+00
16 1.24276e+00 6.06481e-03 1.52557e-03 1.00000e-07 1.00000e-07 0.0020 0.00000e+00
17 1.24276e+00 6.06462e-03 1.50924e-03 1.00000e-06 1.00000e-06 0.0020 0.00000e+00
18 1.24276e+00 6.06342e-03 1.39856e-03 1.00000e-06 1.00000e-06 0.0312 0.00000e+00
19 1.24276e+00 5.69586e-03 1.36367e-03 1.00000e-05 1.00000e-05 0.0020 0.00000e+00
iter cost stop grad xreg ureg step ||ffeas||
20 1.24274e+00 5.69127e-03 1.07634e-03 1.00000e-05 1.00000e-05 0.1250 0.00000e+00
21 1.24274e+00 4.37572e-03 9.48440e-04 1.00000e-04 1.00000e-04 0.0020 0.00000e+00
22 1.24274e+00 4.35871e-03 5.81744e-04 1.00000e-04 1.00000e-04 0.1250 0.00000e+00
23 1.24274e+00 3.33889e-03 5.10521e-04 1.00000e-03 1.00000e-03 0.0020 0.00000e+00
24 1.24273e+00 3.32545e-03 1.73777e-04 1.00000e-03 1.00000e-03 0.2500 0.00000e+00
25 1.24273e+00 1.87007e-03 1.44101e-04 1.00000e-02 1.00000e-02 0.0020 0.00000e+00
26 1.24273e+00 1.87151e-03 2.95659e-05 1.00000e-03 1.00000e-03 1.0000 0.00000e+00
27 1.24273e+00 1.87266e-07 1.21697e-04 1.00000e-02 1.00000e-02 0.0020 0.00000e+00
No viewer
Start from q0= half_sitting
Impact 48 at time 90
Impact 34 at time 151
Impact 48 at time 212
Impact 34 at time 273
Load "/tmp/ddp.npy"!
No valid solution file, build quasistatic initial guess!
Run ```plt.ion(); plt.show()``` to display the plots.
Save "/tmp/ddp.npy"!
OCP described in /tmp/ocp-repr.ascii
iter cost stop grad xreg ureg step ||ffeas||
0 1.12737e+03 1.22089e+04 3.86899e+04 1.00000e-09 1.00000e-09 1.0000 3.63798e-14
1 1.00429e+03 4.75178e+06 2.20931e+03 1.00000e-09 1.00000e-09 0.2500 0.00000e+00
2 6.58045e+02 1.19745e+07 1.97833e+03 1.00000e-09 1.00000e-09 0.2500 0.00000e+00
3 5.64038e+02 4.00759e+04 1.29120e+03 1.00000e-09 1.00000e-09 0.2500 0.00000e+00
4 4.08751e+02 2.71393e+04 1.10979e+03 1.00000e-09 1.00000e-09 0.5000 0.00000e+00
5 7.18488e+01 1.29395e+04 8.02931e+02 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
6 5.12686e+01 2.28830e+03 1.37822e+02 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
7 1.08923e+01 2.22097e+04 9.87402e+01 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
8 2.65649e+00 7.12187e+04 1.85206e+01 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
9 1.37696e+00 8.67129e+03 2.57081e+00 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
iter cost stop grad xreg ureg step ||ffeas||
10 1.28917e+00 8.04974e-01 1.89630e-01 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
11 1.25273e+00 4.75725e-02 6.87958e-02 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
12 1.24162e+00 5.68843e-03 2.41920e-02 1.00000e-09 1.00000e-09 1.0000 0.00000e+00
13 1.24162e+00 4.97979e-03 1.14112e-03 1.00000e-08 1.00000e-08 0.0020 0.00000e+00
14 1.24162e+00 4.97977e-03 1.13980e-03 1.00000e-07 1.00000e-07 0.0020 0.00000e+00
15 1.24162e+00 4.97960e-03 1.12744e-03 1.00000e-06 1.00000e-06 0.0020 0.00000e+00
16 1.24162e+00 4.97864e-03 1.04520e-03 1.00000e-05 1.00000e-05 0.0020 0.00000e+00
17 1.24162e+00 4.97685e-03 8.47647e-04 1.00000e-04 1.00000e-04 0.0020 0.00000e+00
18 1.24160e+00 4.96837e-03 5.97243e-04 1.00000e-04 1.00000e-04 0.2500 0.00000e+00
19 1.24160e+00 2.78989e-03 3.87498e-04 1.00000e-03 1.00000e-03 0.0020 0.00000e+00
iter cost stop grad xreg ureg step ||ffeas||
20 1.24160e+00 2.77886e-03 1.68515e-04 1.00000e-03 1.00000e-03 0.2500 0.00000e+00
21 1.24160e+00 1.56247e-03 1.19097e-04 1.00000e-02 1.00000e-02 0.0020 0.00000e+00
22 1.24159e+00 1.56125e-03 3.03242e-05 1.00000e-03 1.00000e-03 1.0000 0.00000e+00
23 1.24159e+00 1.42656e-07 8.47062e-05 1.00000e-02 1.00000e-02 0.0020 0.00000e+00
This diff is collapsed.