Odd and unstable walking gait for Kbot Simulation using Centroidal MPC

[moavia90]

Screencast.from.16-09-2025.22_48_36.online-video-cutter.com.1.mp4

@manumerous @nicholaspalomo I was facing the issues while working with kbot and not able to generate a stable walking gait for the robot. The robot moves oddly in simulation. I enabled the verbose in task.info file. I verified the ordering of joints of the kbot robot in task.info and reference.info using testPinocchioModel.cpp. Following are the last lines from the log where it was displayed that QP solved!

[humanoid_centroidal_mpc_sqp_node-1] 
[humanoid_centroidal_mpc_sqp_node-1] === HPIPM ===
[humanoid_centroidal_mpc_sqp_node-1] HPIPM returned with flag 0. -> QP solved!
[humanoid_centroidal_mpc_sqp_node-1] ipm iter = 0
[humanoid_centroidal_mpc_sqp_node-1] ipm residuals max: res_g = 1.046991e+04, res_b = 5.684342e-14, res_d = 0.000000e+00, res_m = 0.000000e+00
[humanoid_centroidal_mpc_sqp_node-1] 
[humanoid_centroidal_mpc_sqp_node-1] alpha_aff	mu_aff		sigma		alpha_prim	alpha_dual	mu		res_stat	res_eq		res_ineq	res_comp	lq fact		itref pred	itref corr	lin res stat	lin res eq	lin res ineq	lin res comp
[humanoid_centroidal_mpc_sqp_node-1] 0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	1.046991e+04	5.684342e-14	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	
[humanoid_centroidal_mpc_sqp_node-1] 
[humanoid_centroidal_mpc_sqp_node-1] === Linesearch ===
[humanoid_centroidal_mpc_sqp_node-1] Baseline:
[humanoid_centroidal_mpc_sqp_node-1] Rollout Merit:              978765328.618809700Rollout Cost:               978765328.618809700
[humanoid_centroidal_mpc_sqp_node-1] Dual feasibilities SSE:     0.000000000 Dynamics violation SSE:     240634.582234734
[humanoid_centroidal_mpc_sqp_node-1] Equality constraints SSE:   453.571605996Inequality constraints SSE: 0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Equality Lagrangian:        0.000000000 Inequality Lagrangian:      0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Step size: 1.000000000, Step Type: Constraint (Rejected)
[humanoid_centroidal_mpc_sqp_node-1] |dx| = 117571.589859952	|du| = 110920838.124285311
[humanoid_centroidal_mpc_sqp_node-1] Rollout Merit:              10606260118865384.000000000Rollout Cost:               10606260118865384.000000000
[humanoid_centroidal_mpc_sqp_node-1] Dual feasibilities SSE:     0.000000000 Dynamics violation SSE:     22677898.807004996
[humanoid_centroidal_mpc_sqp_node-1] Equality constraints SSE:   29905831505.879516602Inequality constraints SSE: 0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Equality Lagrangian:        0.000000000 Inequality Lagrangian:      0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Step size: 0.500000000, Step Type: Constraint (Rejected)
[humanoid_centroidal_mpc_sqp_node-1] |dx| = 58785.794929976	|du| = 55460419.062142655
[humanoid_centroidal_mpc_sqp_node-1] Rollout Merit:              3120171457719481.000000000Rollout Cost:               3120171457719481.000000000
[humanoid_centroidal_mpc_sqp_node-1] Dual feasibilities SSE:     0.000000000 Dynamics violation SSE:     6630232.605099032
[humanoid_centroidal_mpc_sqp_node-1] Equality constraints SSE:   4318608244.273506165Inequality constraints SSE: 0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Equality Lagrangian:        0.000000000 Inequality Lagrangian:      0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Step size: 0.250000000, Step Type: Constraint (Rejected)
[humanoid_centroidal_mpc_sqp_node-1] |dx| = 29392.897464988	|du| = 27730209.531071328
[humanoid_centroidal_mpc_sqp_node-1] Rollout Merit:              688866161522337.125000000Rollout Cost:               688866161522337.125000000
[humanoid_centroidal_mpc_sqp_node-1] Dual feasibilities SSE:     0.000000000 Dynamics violation SSE:     12175772.060101271
[humanoid_centroidal_mpc_sqp_node-1] Equality constraints SSE:   991849674.707218885Inequality constraints SSE: 0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Equality Lagrangian:        0.000000000 Inequality Lagrangian:      0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Step size: 0.125000000, Step Type: Constraint (Rejected)
[humanoid_centroidal_mpc_sqp_node-1] |dx| = 14696.448732494	|du| = 13865104.765535664
[humanoid_centroidal_mpc_sqp_node-1] Rollout Merit:              176729049256458.500000000Rollout Cost:               176729049256458.500000000
[humanoid_centroidal_mpc_sqp_node-1] Dual feasibilities SSE:     0.000000000 Dynamics violation SSE:     644368.503335964
[humanoid_centroidal_mpc_sqp_node-1] Equality constraints SSE:   153131787.345017463Inequality constraints SSE: 0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Equality Lagrangian:        0.000000000 Inequality Lagrangian:      0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Step size: 0.062500000, Step Type: Constraint (Rejected)
[humanoid_centroidal_mpc_sqp_node-1] |dx| = 7348.224366247	|du| = 6932552.382767832
[humanoid_centroidal_mpc_sqp_node-1] Rollout Merit:              49850230575631.132812500Rollout Cost:               49850230575631.132812500
[humanoid_centroidal_mpc_sqp_node-1] Dual feasibilities SSE:     0.000000000 Dynamics violation SSE:     308252.251606979
[humanoid_centroidal_mpc_sqp_node-1] Equality constraints SSE:   32985768.608591698Inequality constraints SSE: 0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Equality Lagrangian:        0.000000000 Inequality Lagrangian:      0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Step size: 0.031250000, Step Type: Constraint (Rejected)
[humanoid_centroidal_mpc_sqp_node-1] |dx| = 3674.112183123	|du| = 3466276.191383916
[humanoid_centroidal_mpc_sqp_node-1] Rollout Merit:              5613425853062.357421875Rollout Cost:               5613425853062.357421875
[humanoid_centroidal_mpc_sqp_node-1] Dual feasibilities SSE:     0.000000000 Dynamics violation SSE:     260169.500849474
[humanoid_centroidal_mpc_sqp_node-1] Equality constraints SSE:   19444809.798529536Inequality constraints SSE: 0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Equality Lagrangian:        0.000000000 Inequality Lagrangian:      0.000000000 
[humanoid_centroidal_mpc_sqp_node-1] Step size: 0.015625000, Step Type: Constraint (Rejected)

Here are the links to the task.info , reference.info and kbot urdf files from my forked repo branch.

https://github.com/moavia90/wb_humanoid_mpc/blob/moavia-kbot/robot_models/kbot-v2/kbot_v2_centroidal_mpc/config/mpc/task.info

https://github.com/moavia90/wb_humanoid_mpc/blob/moavia-kbot/robot_models/kbot-v2/kbot_v2_centroidal_mpc/config/command/reference.info

https://github.com/moavia90/wb_humanoid_mpc/blob/moavia-kbot/robot_models/kbot-v2/kbot_v2_description/urdf/robot.urdf

Below is the complete log file for the centroidal MPC for the kbot robot
centroidal_mpc.log

Any help or guidance would be greatly appreciated.

[willthibault]

Hi @moavia90,

I'm not entirely certain but I think we may be experiencing similar issues. See my issue here for more details: #35

It looks like the orientation of your feet are rotated from the expected contact with the ground, your model just takes a bit longer to converge than mine. If you visualize the corners of the contact (just check the box in RViz), I would suspect that they do not match the bottom of the feet. Looking at your robot URDF this would seem likely given the contact parent joint you are using has a rotation. I believe a rotation needs to be added to the contact offset.

I'm working through a fix now but if you beat me to it then it would be great to see!