Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
S
sobec
Manage
Activity
Members
Labels
Plan
Issues
0
Issue boards
Milestones
Wiki
Code
Merge requests
0
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package Registry
Container Registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Nicolas Mansard
sobec
Commits
6ab7b5b2
Commit
6ab7b5b2
authored
2 years ago
by
Nicolas Mansard
Committed by
nmansard
2 years ago
Browse files
Options
Downloads
Patches
Plain Diff
First MPC, dirty copied from walk.py.
parent
9505cd14
Branches
main
No related tags found
No related merge requests found
Pipeline
#19269
failed with stage
Stage: test
in 7 minutes and 26 seconds
Changes
2
Pipelines
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
mpc/mpc.py
+451
-0
451 additions, 0 deletions
mpc/mpc.py
mpc/mpcparams.py
+3
-0
3 additions, 0 deletions
mpc/mpcparams.py
with
454 additions
and
0 deletions
mpc/mpc.py
0 → 100644
+
451
−
0
View file @
6ab7b5b2
import
pinocchio
as
pin
import
crocoddyl
as
croc
import
numpy
as
np
import
example_robot_data
as
robex
import
matplotlib.pyplot
as
plt
;
#plt.ion()
from
numpy.linalg
import
norm
,
inv
,
pinv
,
svd
,
eig
# Local imports
import
talos_low
from
weight_share
import
computeReferenceForces
import
sobec
import
time
pin
.
SE3
.
__repr__
=
pin
.
SE3
.
__str__
np
.
set_printoptions
(
precision
=
2
,
linewidth
=
300
,
suppress
=
True
,
threshold
=
10000
)
### HYPER PARAMETERS
# Hyperparameters defining the optimal control problem.
T_START
=
30
T_SINGLE
=
50
T_DOUBLE
=
11
T_END
=
30
### LOAD AND DISPLAY SOLO
# Load the robot model from example robot data and display it if possible in Gepetto-viewer
robot
=
talos_low
.
load
()
contactIds
=
[
i
for
i
,
f
in
enumerate
(
robot
.
model
.
frames
)
if
"
sole_link
"
in
f
.
name
]
ankleToTow
=
0.1
ankleToHeel
=-
0.1
for
cid
in
contactIds
:
f
=
robot
.
model
.
frames
[
cid
]
robot
.
model
.
addFrame
(
pin
.
Frame
(
f
'
{
f
.
name
}
_tow
'
,
f
.
parent
,
f
.
previousFrame
,
f
.
placement
*
pin
.
SE3
(
np
.
eye
(
3
),
np
.
array
([
ankleToTow
,
0
,
0
])),
pin
.
FrameType
.
OP_FRAME
))
robot
.
model
.
addFrame
(
pin
.
Frame
(
f
'
{
f
.
name
}
_heel
'
,
f
.
parent
,
f
.
previousFrame
,
f
.
placement
*
pin
.
SE3
(
np
.
eye
(
3
),
np
.
array
([
ankleToHeel
,
0
,
0
])),
pin
.
FrameType
.
OP_FRAME
))
try
:
viz
=
pin
.
visualize
.
GepettoVisualizer
(
robot
.
model
,
robot
.
collision_model
,
robot
.
visual_model
)
viz
.
initViewer
()
viz
.
loadViewerModel
()
gv
=
viz
.
viewer
.
gui
except
:
print
(
"
No viewer
"
)
# The pinocchio model is what we are really interested by.
model
=
robot
.
model
model
.
q0
=
robot
.
q0
data
=
model
.
createData
()
# Initial config, also used for warm start
x0
=
np
.
concatenate
([
model
.
q0
,
np
.
zeros
(
model
.
nv
)])
# Some key elements of the model
towIds
=
{
idf
:
model
.
getFrameId
(
f
'
{
model
.
frames
[
idf
].
name
}
_tow
'
)
for
idf
in
contactIds
}
heelIds
=
{
idf
:
model
.
getFrameId
(
f
'
{
model
.
frames
[
idf
].
name
}
_heel
'
)
for
idf
in
contactIds
}
baseId
=
model
.
getFrameId
(
'
root_joint
'
)
robotweight
=
-
sum
([
Y
.
mass
for
Y
in
model
.
inertias
])
*
model
.
gravity
.
linear
[
2
]
com0
=
pin
.
centerOfMass
(
model
,
data
,
model
.
q0
)
pin
.
framesForwardKinematics
(
model
,
data
,
x0
[:
model
.
nq
])
###################################################################################################
### TUNING ########################################################################################
###################################################################################################
# In the code, cost terms with 0 weight are commented for reducing execution cost
# An example of working weight value is then given as comment at the end of the line.
# When setting them to >0, take care to uncomment the corresponding line.
# All these lines are marked with the tag ##0##.
from
mpcparams
import
*
assert
(
len
(
STATE_WEIGHT
)
==
model
.
nv
*
2
)
# Contact are specified with the order chosen in <contactIds>
# contactPattern = [] \
# + [ [ 1,1 ] ] * 40 \
# + [ [ 1,0 ] ] * 50 \
# + [ [ 1,1 ] ] * 11 \
# + [ [ 0,1 ] ] * 50 \
# + [ [ 1,1 ] ] * 11 \
# + [ [ 1,1 ] ] * 40 \
# + [ [ 1,1 ] ]
contactPattern
=
[]
\
+
[
[
1
,
1
]
]
*
T_START
\
+
[
[
1
,
1
]
]
*
T_DOUBLE
\
+
[
[
1
,
0
]
]
*
T_SINGLE
\
+
[
[
1
,
1
]
]
*
T_DOUBLE
\
+
[
[
0
,
1
]
]
*
T_SINGLE
\
+
[
[
1
,
1
]
]
*
T_DOUBLE
\
+
[
[
1
,
1
]
]
*
T_END
\
+
[
[
1
,
1
]
]
T
=
len
(
contactPattern
)
-
1
def
patternToId
(
pattern
):
'''
Return the tuple of active contact from a pattern like [0,1], [1,0] or [1,1].
'''
return
tuple
(
contactIds
[
i
]
for
i
,
c
in
enumerate
(
pattern
)
if
c
==
1
)
# ### REF FORCES ######################################################################
# The force costs are defined using a reference (smooth) force.
# # Search the contact phase of minimal duration (typically double support)
# contactState=[]
# dur=mindur=len(contactPattern)
# for t,s in enumerate(contactPattern):
# dur+=1
# if s!=contactState:
# contactState=s
# mindur=min(mindur,dur)
# dur=0
# # Select the smoothing transition to be smaller than half of the minimal duration.
# transDuration=(mindur-1)//2
# # Compute contact importance, ie how much of the weight should be supported by each
# # foot at each time.
# contactImportance = weightShareSmoothProfile(contactPattern,transDuration,switch=switch_linear)
# # Contact reference forces are set to contactimportance*weight
# weightReaction = np.array([0,0,robotweight,0,0,0])
# referenceForces = [
# [ weightReaction*contactImportance[t,cid] for cid,__c in enumerate(pattern) ]
# for t,pattern in enumerate(contactPattern) ]
# # Take care, we suppose here that foot normal is vertical.
referenceForces
=
computeReferenceForces
(
contactPattern
,
robotweight
)
# #################################################################################################
# #################################################################################################
# #################################################################################################
models
=
[]
# #################################################################################################
for
t
,
pattern
in
enumerate
(
contactPattern
[:
-
1
]):
#print(f'time t={t}')
# Basics
state
=
croc
.
StateMultibody
(
model
)
actuation
=
croc
.
ActuationModelFloatingBase
(
state
)
# Contacts
contacts
=
croc
.
ContactModelMultiple
(
state
,
actuation
.
nu
)
for
k
,
cid
in
enumerate
(
contactIds
):
if
not
pattern
[
k
]:
continue
contact
=
croc
.
ContactModel6D
(
state
,
cid
,
pin
.
SE3
.
Identity
(),
actuation
.
nu
,
baumgartGains
)
contacts
.
addContact
(
model
.
frames
[
cid
].
name
+
"
_contact
"
,
contact
)
# Costs
costs
=
croc
.
CostModelSum
(
state
,
actuation
.
nu
)
xRegResidual
=
croc
.
ResidualModelState
(
state
,
x0
,
actuation
.
nu
)
xRegCost
=
croc
.
CostModelResidual
(
state
,
croc
.
ActivationModelWeightedQuad
(
STATE_WEIGHT
**
2
),
xRegResidual
)
costs
.
addCost
(
"
stateReg
"
,
xRegCost
,
refStateWeight
)
uResidual
=
croc
.
ResidualModelControl
(
state
,
actuation
.
nu
)
uRegCost
=
croc
.
CostModelResidual
(
state
,
croc
.
ActivationModelWeightedQuad
(
np
.
array
(
CONTROL_WEIGHT
**
2
)),
uResidual
)
costs
.
addCost
(
"
ctrlReg
"
,
uRegCost
,
refTorqueWeight
)
comVelResidual
=
sobec
.
ResidualModelCoMVelocity
(
state
,
VCOM_TARGET
,
actuation
.
nu
)
comVelCost
=
croc
.
CostModelResidual
(
state
,
comVelResidual
)
costs
.
addCost
(
"
comVelCost
"
,
comVelCost
,
vcomWeight
)
# Contact costs
for
k
,
cid
in
enumerate
(
contactIds
):
if
not
pattern
[
k
]:
continue
copResidual
=
sobec
.
ResidualModelCenterOfPressure
(
state
,
cid
,
actuation
.
nu
)
copAct
=
croc
.
ActivationModelWeightedQuad
(
np
.
array
([
1
/
FOOT_SIZE
**
2
]
*
2
))
copCost
=
croc
.
CostModelResidual
(
state
,
copAct
,
copResidual
)
costs
.
addCost
(
f
'
{
model
.
frames
[
cid
].
name
}
_cop
'
,
copCost
,
copWeight
)
# Cone with enormous friction (Assuming the robot will barely ever slide).
# FOOT_SIZE is the allowed area size, while cone expects the corner coordinates => x2
cone
=
croc
.
WrenchCone
(
np
.
eye
(
3
),
1000
,
np
.
array
([
FOOT_SIZE
*
2
]
*
2
),
4
,
True
,
1
,
10000
)
coneCost
=
croc
.
ResidualModelContactWrenchCone
(
state
,
cid
,
cone
,
actuation
.
nu
)
ub
=
cone
.
ub
.
copy
()
ub
[:
4
]
=
np
.
inf
#ub[5:] = np.inf ### DEBUG
ub
[
-
8
:]
=
np
.
inf
coneAct
=
croc
.
ActivationModelQuadraticBarrier
(
croc
.
ActivationBounds
(
cone
.
lb
,
ub
))
coneCost
=
croc
.
CostModelResidual
(
state
,
coneAct
,
coneCost
)
costs
.
addCost
(
f
'
{
model
.
frames
[
cid
].
name
}
_cone
'
,
coneCost
,
conePenaltyWeight
)
# Penalize the distance to the central axis of the cone ...
# ... using normalization weights depending on the axis.
# The weights are squared to match the tuning of the CASADI formulation.
coneAxisResidual
=
croc
.
ResidualModelContactForce
(
state
,
cid
,
pin
.
Force
.
Zero
(),
6
,
actuation
.
nu
);
w
=
np
.
array
(
forceImportance
**
2
);
w
[
2
]
=
0
coneAxisAct
=
croc
.
ActivationModelWeightedQuad
(
w
)
coneAxisCost
=
croc
.
CostModelResidual
(
state
,
coneAxisAct
,
coneAxisResidual
);
costs
.
addCost
(
f
'
{
model
.
frames
[
cid
].
name
}
_coneaxis
'
,
coneAxisCost
,
coneAxisWeight
)
# Follow reference (smooth) contact forces
forceRefResidual
=
croc
.
ResidualModelContactForce
(
state
,
cid
,
pin
.
Force
(
referenceForces
[
t
][
k
]),
6
,
actuation
.
nu
)
forceRefCost
=
croc
.
CostModelResidual
(
state
,
forceRefResidual
)
costs
.
addCost
(
f
'
{
model
.
frames
[
cid
].
name
}
_forceref
'
,
forceRefCost
,
refForceWeight
/
robotweight
**
2
)
# IMPACT
for
k
,
cid
in
enumerate
(
contactIds
):
if
t
>
0
and
not
contactPattern
[
t
-
1
][
k
]
and
pattern
[
k
]:
# REMEMBER TO divide the weight by DT, as impact should be independant of the node duration
# (at least, that s how weights are tuned in casadi).
print
(
f
'
Impact
{
cid
}
at time
{
t
}
'
)
if
'
left
'
in
model
.
frames
[
cid
].
name
:
itarget
=
np
.
array
([
0
,.
1
,
0
])
else
:
itarget
=
np
.
array
([
0
,
-
.
1
,
0
])
impactResidual
=
croc
.
ResidualModelFrameTranslation
(
state
,
cid
,
itarget
,
actuation
.
nu
)
impactAct
=
croc
.
ActivationModelWeightedQuad
(
np
.
array
([
0
,
100
,
1
]))
impactCost
=
croc
.
CostModelResidual
(
state
,
impactAct
,
impactResidual
)
costs
.
addCost
(
f
'
{
model
.
frames
[
cid
].
name
}
_atitudeimpact
'
,
impactCost
,
impactAltitudeWeight
/
DT
)
impactVelResidual
=
croc
.
ResidualModelFrameVelocity
(
state
,
cid
,
pin
.
Motion
.
Zero
(),
pin
.
ReferenceFrame
.
LOCAL
,
actuation
.
nu
)
impactVelCost
=
croc
.
CostModelResidual
(
state
,
impactVelResidual
)
costs
.
addCost
(
f
'
{
model
.
frames
[
cid
].
name
}
_velimpact
'
,
impactVelCost
,
impactVelocityWeight
/
DT
)
impactRotResidual
=
croc
.
ResidualModelFrameRotation
(
state
,
cid
,
np
.
eye
(
3
),
actuation
.
nu
)
impactRotAct
=
croc
.
ActivationModelWeightedQuad
(
np
.
array
([
1
,
1
,
0
]))
impactRotCost
=
croc
.
CostModelResidual
(
state
,
impactRotAct
,
impactRotResidual
)
costs
.
addCost
(
f
'
{
model
.
frames
[
cid
].
name
}
_rotimpact
'
,
impactRotCost
,
impactRotationWeight
/
DT
)
impactRefJointsResidual
=
croc
.
ResidualModelState
(
state
,
x0
,
actuation
.
nu
)
jselec
=
np
.
zeros
(
model
.
nv
*
2
)
jselec
[[
model
.
joints
[
model
.
getJointId
(
name
)].
idx_v
for
name
in
MAIN_JOINTS
]]
=
1
impactRefJointsAct
=
croc
.
ActivationModelWeightedQuad
(
jselec
)
impactRefJointCost
=
croc
.
CostModelResidual
(
state
,
impactRefJointsAct
,
impactRefJointsResidual
)
costs
.
addCost
(
'
impactRefJoint
'
,
impactRefJointCost
,
refMainJointsAtImpactWeight
/
DT
)
# Flying foot
for
k
,
cid
in
enumerate
(
contactIds
):
if
pattern
[
k
]:
continue
verticalFootVelResidual
=
croc
.
ResidualModelFrameVelocity
(
state
,
cid
,
pin
.
Motion
.
Zero
(),
pin
.
ReferenceFrame
.
LOCAL_WORLD_ALIGNED
,
actuation
.
nu
)
verticalFootVelAct
=
croc
.
ActivationModelWeightedQuad
(
np
.
array
([
0
,
0
,
1
,
0
,
0
,
0
]))
verticalFootVelCost
=
croc
.
CostModelResidual
(
state
,
verticalFootVelAct
,
verticalFootVelResidual
)
costs
.
addCost
(
f
'
{
model
.
frames
[
cid
].
name
}
_vfoot_vel
'
,
verticalFootVelCost
,
verticalFootVelWeight
)
# Slope is /2 since it is squared in casadi (je me comprends)
flyHighResidual
=
sobec
.
ResidualModelFlyHigh
(
state
,
cid
,
flyHighSlope
/
2
,
actuation
.
nu
)
flyHighCost
=
croc
.
CostModelResidual
(
state
,
flyHighResidual
)
costs
.
addCost
(
f
'
{
model
.
frames
[
cid
].
name
}
_flyhigh
'
,
flyHighCost
,
flyWeight
)
groundColRes
=
croc
.
ResidualModelFrameTranslation
(
state
,
cid
,
np
.
zeros
(
3
),
actuation
.
nu
)
#groundColBounds = croc.ActivationBounds(np.array([-np.inf,-np.inf,0.01]),np.array([np.inf,np.inf,np.inf]))
# np.inf introduces an error on lb[2] ... why? TODO ... patch by replacing np.inf with 1000
groundColBounds
=
croc
.
ActivationBounds
(
np
.
array
([
-
1000
,
-
1000
,
0.0
]),
np
.
array
([
1000
,
1000
,
1000
]))
groundColAct
=
croc
.
ActivationModelQuadraticBarrier
(
groundColBounds
)
groundColCost
=
croc
.
CostModelResidual
(
state
,
groundColAct
,
groundColRes
)
costs
.
addCost
(
f
'
{
model
.
frames
[
cid
].
name
}
_groundcol
'
,
groundColCost
,
groundColWeight
)
# Action
damodel
=
croc
.
DifferentialActionModelContactFwdDynamics
(
state
,
actuation
,
contacts
,
costs
,
kktDamping
,
True
)
amodel
=
croc
.
IntegratedActionModelEuler
(
damodel
,
DT
)
models
.
append
(
amodel
)
#stophere
# #################################################################################################
pattern
=
contactPattern
[
-
1
]
state
=
croc
.
StateMultibody
(
model
)
actuation
=
croc
.
ActuationModelFloatingBase
(
state
)
# Contacts
contacts
=
croc
.
ContactModelMultiple
(
state
,
actuation
.
nu
)
for
k
,
cid
in
enumerate
(
contactIds
):
if
not
pattern
[
k
]:
continue
contact
=
croc
.
ContactModel6D
(
state
,
cid
,
pin
.
SE3
.
Identity
(),
actuation
.
nu
,
baumgartGains
)
contacts
.
addContact
(
model
.
frames
[
cid
].
name
+
"
_contact
"
,
contact
)
# Costs
costs
=
croc
.
CostModelSum
(
state
,
actuation
.
nu
)
xRegResidual
=
croc
.
ResidualModelState
(
state
,
x0
,
actuation
.
nu
)
xRegCost
=
croc
.
CostModelResidual
(
state
,
croc
.
ActivationModelWeightedQuad
(
np
.
array
([
0
]
*
model
.
nv
+
[
1
]
*
model
.
nv
)),
xRegResidual
)
costs
.
addCost
(
"
stateReg
"
,
xRegCost
,
terminalNoVelocityWeight
)
damodel
=
croc
.
DifferentialActionModelContactFwdDynamics
(
state
,
actuation
,
contacts
,
costs
,
kktDamping
,
True
)
termmodel
=
croc
.
IntegratedActionModelEuler
(
damodel
,
DT
)
# #################################################################################################
#GUESS_FILE = '/tmp/sol.npy'
#guess = np.load(GUESS_FILE,allow_pickle=True)[()]
#print(f'Load "{GUESS_FILE}"!')
# #################################################################################################
problem
=
croc
.
ShootingProblem
(
x0
,
models
,
termmodel
)
ddp
=
croc
.
SolverFDDP
(
problem
)
x0s
=
[
x0
.
copy
()]
*
(
len
(
models
)
+
1
)
u0s
=
[
m
.
quasiStatic
(
d
,
x
)
for
m
,
d
,
x
in
zip
(
problem
.
runningModels
,
problem
.
runningDatas
,
x0s
)
]
ddp
.
setCallbacks
([
croc
.
CallbackVerbose
()])
#x0s = [x for x in guess['xs']]
#u0s = [u for u in guess['us']]
# l = pin.StdVec_Double()
# l.append(2)
# ddp.alphas = l
#ddp.th_acceptStep = 0.1
ddp
.
th_stop
=
1e-3
ddp
.
solve
(
x0s
,
u0s
,
200
)
# ### MPC #########################################################################################
Tmpc
=
100
mpcProblem
=
croc
.
ShootingProblem
(
x0
,
models
[:
Tmpc
],
termmodel
)
mpcSolver
=
croc
.
SolverFDDP
(
mpcProblem
)
#mpcSolver.setCallbacks([croc.CallbackVerbose()])
mpcSolver
.
th_stop
=
1e-3
mpcSolver
.
solve
(
ddp
.
xs
[:
Tmpc
+
1
],
ddp
.
us
[:
Tmpc
],
10
,
isFeasible
=
True
)
x
=
mpcSolver
.
xs
[
1
].
copy
()
def
contact2car
(
contacts
):
if
len
(
contacts
)
==
2
:
return
'
=
'
if
len
(
contacts
)
==
0
:
return
'
'
if
'
right
'
in
next
(
iter
(
contacts
)).
key
():
return
'
_
'
if
'
left
'
in
next
(
iter
(
contacts
)).
key
():
return
'
⎻
'
error
def
dispocp
(
pb
):
return
''
.
join
([
contact2car
(
r
.
differential
.
contacts
.
contacts
)
for
r
in
pb
.
runningModels
])
hx
=
[
x
]
hiter
=
[
mpcSolver
.
iter
]
for
t
in
range
(
1
,
10000
):
#tlast = t+Tmpc
tlast
=
T_START
+
((
t
+
Tmpc
-
T_START
)
%
(
2
*
T_SINGLE
+
2
*
T_DOUBLE
))
#print(f't={t} ... last is {tlast}')
mpcProblem
.
circularAppend
(
problem
.
runningModels
[
tlast
],
problem
.
runningDatas
[
tlast
])
mpcProblem
.
x0
=
x
.
copy
()
print
(
dispocp
(
mpcProblem
))
#assert(mpcProblem.runningModels[0] == problem.runningModels[t])
xg
=
list
(
mpcSolver
.
xs
)[
1
:]
+
[
mpcSolver
.
xs
[
-
1
]]
ug
=
list
(
mpcSolver
.
us
)[
1
:]
+
[
mpcSolver
.
us
[
-
1
]]
mpcSolver
.
solve
(
xg
,
ug
)
x
=
mpcSolver
.
xs
[
1
].
copy
()
hx
.
append
(
x
)
hiter
.
append
(
mpcSolver
.
iter
)
if
not
t
%
10
:
viz
.
display
(
x
[:
model
.
nq
])
time
.
sleep
(
DT
)
def
savempc
():
SOLU_FILE
=
"
/tmp/mpc.npy
"
np
.
save
(
open
(
SOLU_FILE
,
"
wb
"
),
{
"
xs
"
:
np
.
array
(
hx
),
})
print
(
f
'
Save
"
{
SOLU_FILE
}
"
!
'
)
# ### PLOT ######################################################################
# ### PLOT ######################################################################
# ### PLOT ######################################################################
xs_sol
=
np
.
array
(
ddp
.
xs
)
us_sol
=
np
.
array
(
ddp
.
us
)
acs_sol
=
np
.
array
([
d
.
differential
.
xout
for
d
in
problem
.
runningDatas
])
fs_sol
=
[
[
(
cd
.
data
().
jMf
.
inverse
()
*
cd
.
data
().
f
).
vector
for
cd
in
d
.
differential
.
multibody
.
contacts
.
contacts
]
for
d
in
problem
.
runningDatas
]
fs_sol0
=
[
np
.
concatenate
([
(
d
.
differential
.
multibody
.
contacts
.
contacts
[
f
'
{
model
.
frames
[
cid
].
name
}
_contact
'
].
jMf
.
inverse
()
*
d
.
differential
.
multibody
.
contacts
.
contacts
[
f
'
{
model
.
frames
[
cid
].
name
}
_contact
'
].
f
).
vector
if
f
'
{
model
.
frames
[
cid
].
name
}
_contact
'
in
d
.
differential
.
multibody
.
contacts
.
contacts
else
np
.
zeros
(
6
)
for
cid
in
contactIds
])
for
m
,
d
in
zip
(
problem
.
runningModels
,
problem
.
runningDatas
)
]
from
walk_plotter
import
WalkPlotter
plotter
=
WalkPlotter
(
model
,
contactIds
)
plotter
.
setData
(
contactPattern
,
xs_sol
,
us_sol
,
fs_sol0
)
plotter
.
plotBasis
(
X_TARGET
)
plotter
.
plotTimeCop
()
plotter
.
plotCopAndFeet
(
FOOT_SIZE
,.
6
)
plotter
.
plotForces
(
referenceForces
)
plotter
.
plotCom
(
com0
)
plotter
.
plotFeet
()
plotter
.
plotFootCollision
(
footMinimalDistance
)
# ### SAVE #####################################################################
def
save
():
SOLU_FILE
=
"
/tmp/ddp.npy
"
np
.
save
(
open
(
SOLU_FILE
,
"
wb
"
),
{
"
xs
"
:
xs_sol
,
"
us
"
:
us_sol
,
"
acs
"
:
acs_sol
,
"
fs
"
:
np
.
array
(
fs_sol0
),
})
print
(
f
'
Save
"
{
SOLU_FILE
}
"
!
'
)
'''
-B < tau/f < B
tau < Bf
Bf - tau >=0
tau >= -Bf
Bf + tau >= 0
'''
### DEBUG
# dmodel = problem.runningModels[0].differential
# ddata = problem.runningDatas[0].differential
# contactmodell = dmodel.contacts.contacts['left_sole_link_contact'].contact
# contactmodelr = dmodel.contacts.contacts['right_sole_link_contact'].contact
# contactdatal = ddata.multibody.contacts.contacts['left_sole_link_contact']
# contactdatar = ddata.multibody.contacts.contacts['right_sole_link_contact']
# copcostmodell = dmodel.costs.costs['left_sole_link_cop'].cost
# copcostmodelr = dmodel.costs.costs['right_sole_link_cop'].cost
# copcostdatal = ddata.costs.costs['left_sole_link_cop']
# copcostdatar = ddata.costs.costs['right_sole_link_cop']
t
=
60
;
fid
=
48
t
=
119
;
fid
=
34
t
=
90
;
cid
=
48
# impact
xs
=
guess
[
'
xs
'
]
us
=
guess
[
'
us
'
]
fs0
=
guess
[
'
fs
'
]
acs
=
guess
[
'
acs
'
]
dadata
=
problem
.
runningDatas
[
t
].
differential
damodel
=
problem
.
runningModels
[
t
].
differential
damodel
.
calc
(
dadata
,
xs
[
t
],
us
[
t
])
damodel
.
calcDiff
(
dadata
,
xs
[
t
],
us
[
t
])
cosname
=
'
left_sole_link_cone
'
cosname
=
'
right_sole_link_cone
'
#cosname='altitudeImpact'
cosname
=
'
right_sole_link_vfoot_vel
'
# t = 60
cosname
=
'
right_sole_link_flyhigh
'
cosname
=
f
'
{
model
.
frames
[
fid
].
name
}
_flyhigh
'
cosname
=
f
'
{
model
.
frames
[
fid
].
name
}
_groundcol
'
cosname
=
f
'
{
model
.
frames
[
cid
].
name
}
_velimpact
'
cosname
=
'
impactRefJoint
'
cosdata
=
dadata
.
costs
.
costs
[
cosname
]
cosmodel
=
damodel
.
costs
.
costs
[
cosname
].
cost
np
.
set_printoptions
(
precision
=
6
,
linewidth
=
300
,
suppress
=
True
,
threshold
=
10000
)
This diff is collapsed.
Click to expand it.
mpc/mpcparams.py
0 → 100644
+
3
−
0
View file @
6ab7b5b2
from
params
import
*
terminalNoVelocityWeight
=
20
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment