Skip to content
Snippets Groups Projects

Muscodwalkin. Closes #72

Merged Rohan Budhiraja requested to merge proyan/crocoddyl:muscodwalkin into devel
Files
2
@@ -81,8 +81,9 @@ def createMultiphaseShootingProblem(rmodel, rdata, patch_name_map, cs, phi_c, sw
#Define Cost weights
w = EmptyClass()
w.com = 1e3; w.state = 1e-1; w.control = 1e-3;
w.swing_patch = 1e3; w.forces = 1e-4;
w.com = 1e2; w.state = 1e-1; w.control = 1e-3;
w.swing_patch = 1e4; w.forces = 1e-4;
w.swingv = 1e4
#Define state cost vector for WeightedActivation
w.xweight = np.array([0]*6+[0.01]*(rmodel.nv-6)+[10.]*rmodel.nv)
@@ -141,12 +142,20 @@ def createMultiphaseShootingProblem(rmodel, rdata, patch_name_map, cs, phi_c, sw
#Control Regularization
cost_regu = CostModelControl(rmodel, nu = actuationff.nu)
cost_model.addCost("regu", cost_regu, w.control)
dmodel = DifferentialActionModelFloatingInContact(rmodel, actuationff,
contact_model, cost_model)
imodel = IntegratedActionModelEuler(dmodel)
problem_models.append(imodel)
#for the last model of the phase, add velocity cost on swing limbs.
for patch in swing_patch:
cost_vswing = CostModelFrameVelocity(rmodel,
frame=rmodel.getFrameId(patch_name_map[patch]),
ref=m2a(pinocchio.Motion.Zero().vector),
nu=actuationff.nu)
problem_models[-1].differential.costs.addCost("swingv_"+patch, cost_swing, w.swingv)
#Create Terminal Model.
contact_model = ContactModelMultiple(rmodel)
# Add contact constraints for the active contact patches.
Loading