diff --git a/task_2_5/task_2_5.py b/task_2_5/task_2_5.py
index 85bbab1..ba14c45 100644
--- a/task_2_5/task_2_5.py
+++ b/task_2_5/task_2_5.py
@@ -14,11 +14,9 @@ observe = True
 
 def run_task_2_5(Ctrl, msg, direction='left'):
     section('任务2.5：预备进入任务3', "启动")
+    turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
     # go_straight(Ctrl, msg, distance=-0.1, speed=0.5, observe=observe)
 
-    # TEST
-    turn_degree_v2(Ctrl, msg, 90, absolute=observe)
-
     section('任务2.5-1：第一次旋转', "移动")
 
     turn_success, res = arc_turn_around_hori_line(
diff --git a/task_3/Gait_Params_down.toml b/task_3/Gait_Params_down.toml
index bf837a3..0c9a445 100644
--- a/task_3/Gait_Params_down.toml
+++ b/task_3/Gait_Params_down.toml
@@ -4,7 +4,7 @@
 # 1
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -31,7 +31,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -57,7 +57,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -83,7 +83,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -110,7 +110,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -137,7 +137,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -163,7 +163,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -189,7 +189,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -215,7 +215,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -242,7 +242,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -268,7 +268,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -294,7 +294,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -320,7 +320,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -347,7 +347,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -373,7 +373,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -399,7 +399,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -425,7 +425,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -452,7 +452,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -478,7 +478,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -504,7 +504,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -531,7 +531,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -558,7 +558,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -584,7 +584,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -610,7 +610,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -636,7 +636,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -663,7 +663,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -689,7 +689,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -715,7 +715,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -741,7 +741,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -768,7 +768,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -794,7 +794,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -820,7 +820,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -846,7 +846,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -873,7 +873,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -899,7 +899,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -925,7 +925,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -952,7 +952,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -979,7 +979,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1005,7 +1005,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1031,7 +1031,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1057,7 +1057,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1084,7 +1084,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1110,7 +1110,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1136,7 +1136,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1162,7 +1162,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1189,7 +1189,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1215,7 +1215,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1241,7 +1241,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1267,7 +1267,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1294,7 +1294,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1320,7 +1320,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1346,7 +1346,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1373,7 +1373,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1400,7 +1400,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1426,7 +1426,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1452,7 +1452,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1478,7 +1478,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1505,7 +1505,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1531,7 +1531,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1557,7 +1557,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1583,7 +1583,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1610,7 +1610,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1636,7 +1636,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1662,7 +1662,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1688,7 +1688,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1715,7 +1715,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1741,7 +1741,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1767,7 +1767,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1794,7 +1794,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1821,7 +1821,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1847,7 +1847,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1873,7 +1873,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1899,7 +1899,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1926,7 +1926,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1952,7 +1952,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1978,7 +1978,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2004,7 +2004,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2031,7 +2031,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2057,7 +2057,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2083,7 +2083,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2109,7 +2109,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2136,7 +2136,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2162,7 +2162,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2188,7 +2188,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2214,7 +2214,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2241,7 +2241,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2267,7 +2267,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2293,7 +2293,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2319,7 +2319,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2346,7 +2346,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2372,7 +2372,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2398,7 +2398,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2424,7 +2424,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2451,7 +2451,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2477,7 +2477,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2503,7 +2503,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2529,7 +2529,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2556,7 +2556,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2582,7 +2582,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2608,7 +2608,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2635,7 +2635,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2662,7 +2662,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2688,7 +2688,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2714,7 +2714,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2740,7 +2740,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2767,7 +2767,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2793,7 +2793,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2819,7 +2819,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2845,7 +2845,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2872,7 +2872,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2898,7 +2898,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2924,7 +2924,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2950,7 +2950,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2977,7 +2977,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3003,7 +3003,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3029,7 +3029,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3056,7 +3056,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3083,7 +3083,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3109,7 +3109,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3135,7 +3135,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3161,7 +3161,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3188,7 +3188,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3214,7 +3214,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3240,7 +3240,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3266,7 +3266,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3293,7 +3293,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3319,7 +3319,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3345,7 +3345,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3371,7 +3371,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3398,7 +3398,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3424,7 +3424,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3450,7 +3450,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3477,7 +3477,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3504,7 +3504,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3530,7 +3530,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3556,7 +3556,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3582,7 +3582,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3609,7 +3609,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3635,7 +3635,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3661,7 +3661,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3687,7 +3687,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3714,7 +3714,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3740,7 +3740,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3766,7 +3766,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3792,7 +3792,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3819,7 +3819,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3845,7 +3845,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3871,7 +3871,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3898,7 +3898,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3925,7 +3925,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3951,7 +3951,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3977,7 +3977,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4003,7 +4003,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4030,7 +4030,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4056,7 +4056,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4082,7 +4082,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4108,7 +4108,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4135,7 +4135,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4161,7 +4161,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4187,7 +4187,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4213,7 +4213,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4240,7 +4240,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4266,7 +4266,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4292,7 +4292,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4319,7 +4319,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4346,7 +4346,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4372,7 +4372,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4398,7 +4398,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4424,7 +4424,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4451,7 +4451,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4477,7 +4477,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4503,7 +4503,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4529,7 +4529,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4556,7 +4556,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4582,7 +4582,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4608,7 +4608,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4634,7 +4634,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4661,7 +4661,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4687,7 +4687,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4713,7 +4713,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4740,7 +4740,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4767,7 +4767,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4793,7 +4793,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4819,7 +4819,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4845,7 +4845,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4872,7 +4872,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4898,7 +4898,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4924,7 +4924,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4950,7 +4950,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4977,7 +4977,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5003,7 +5003,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5029,7 +5029,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5055,7 +5055,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5082,7 +5082,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5108,7 +5108,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5134,7 +5134,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5161,7 +5161,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5188,7 +5188,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5214,7 +5214,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5240,7 +5240,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5266,7 +5266,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5293,7 +5293,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5319,7 +5319,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5345,7 +5345,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5371,7 +5371,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5398,7 +5398,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5424,7 +5424,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5450,7 +5450,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5476,7 +5476,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5503,7 +5503,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5529,7 +5529,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5555,7 +5555,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5581,7 +5581,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5608,7 +5608,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5634,7 +5634,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5660,7 +5660,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5686,7 +5686,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5713,7 +5713,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5739,7 +5739,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5765,7 +5765,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5791,7 +5791,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5818,7 +5818,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5844,7 +5844,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5870,7 +5870,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5896,7 +5896,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5923,7 +5923,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5949,7 +5949,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5975,7 +5975,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6002,7 +6002,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6029,7 +6029,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6055,7 +6055,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6081,7 +6081,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6107,7 +6107,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6134,7 +6134,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6160,7 +6160,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6186,7 +6186,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6212,7 +6212,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6239,7 +6239,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6265,7 +6265,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6291,7 +6291,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6317,7 +6317,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6344,7 +6344,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6370,7 +6370,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6396,7 +6396,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6423,7 +6423,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6450,7 +6450,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6476,7 +6476,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6502,7 +6502,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6528,7 +6528,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6555,7 +6555,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6581,7 +6581,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6607,7 +6607,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6633,7 +6633,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6660,7 +6660,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6686,7 +6686,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6712,7 +6712,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6738,7 +6738,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6765,7 +6765,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6791,7 +6791,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6817,7 +6817,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6844,7 +6844,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6871,7 +6871,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6897,7 +6897,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6923,7 +6923,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6949,7 +6949,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6976,7 +6976,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7002,7 +7002,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7028,7 +7028,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7054,7 +7054,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7081,7 +7081,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7107,7 +7107,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7133,7 +7133,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7159,7 +7159,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7186,7 +7186,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7212,7 +7212,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7238,7 +7238,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7265,7 +7265,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7292,7 +7292,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7318,7 +7318,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7344,7 +7344,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7370,7 +7370,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7397,7 +7397,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7423,7 +7423,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7449,7 +7449,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7475,7 +7475,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7502,7 +7502,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7528,7 +7528,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7554,7 +7554,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7580,7 +7580,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7607,7 +7607,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7633,7 +7633,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7659,7 +7659,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7686,7 +7686,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7713,7 +7713,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7739,7 +7739,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7765,7 +7765,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7791,7 +7791,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7818,7 +7818,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7844,7 +7844,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7870,7 +7870,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7896,7 +7896,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7923,7 +7923,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7949,7 +7949,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7975,7 +7975,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8001,7 +8001,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8028,7 +8028,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8054,7 +8054,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8080,7 +8080,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8107,7 +8107,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8134,7 +8134,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8160,7 +8160,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8186,7 +8186,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8212,7 +8212,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8239,7 +8239,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8265,7 +8265,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8291,7 +8291,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8317,7 +8317,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8344,7 +8344,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8370,7 +8370,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8396,7 +8396,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8422,7 +8422,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8449,7 +8449,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8475,7 +8475,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8501,7 +8501,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8528,7 +8528,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8555,7 +8555,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8581,7 +8581,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8607,7 +8607,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8633,7 +8633,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.08, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8660,7 +8660,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8686,7 +8686,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8712,7 +8712,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.05, 0.0, 0.0, 0.0, 0.001]
+body_pos_des = [0.0, 0.08, 0.0, 0.0, 0.0, 0.001]
 body_vel_des = [0.08, 0.0, 0.0]
 duration = 300
 gait_id = 110
diff --git a/task_3/Gait_Params_down_full.toml b/task_3/Gait_Params_down_full.toml
index b599fc7..3a546e1 100644
--- a/task_3/Gait_Params_down_full.toml
+++ b/task_3/Gait_Params_down_full.toml
@@ -5,7 +5,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -20,7 +20,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -35,7 +35,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -50,7 +50,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -65,7 +65,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -80,7 +80,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -95,7 +95,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -110,7 +110,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -125,7 +125,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -140,7 +140,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -155,7 +155,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -170,7 +170,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -185,7 +185,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -200,7 +200,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -215,7 +215,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -230,7 +230,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -245,7 +245,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -260,7 +260,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -275,7 +275,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -290,7 +290,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -305,7 +305,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -320,7 +320,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -335,7 +335,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -350,7 +350,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -365,7 +365,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -380,7 +380,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -395,7 +395,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -410,7 +410,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -425,7 +425,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -440,7 +440,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -455,7 +455,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -470,7 +470,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -485,7 +485,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -500,7 +500,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -515,7 +515,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -530,7 +530,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -545,7 +545,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -560,7 +560,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -575,7 +575,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -590,7 +590,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -605,7 +605,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -620,7 +620,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -635,7 +635,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -650,7 +650,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -665,7 +665,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -680,7 +680,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -695,7 +695,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -710,7 +710,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -725,7 +725,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -740,7 +740,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -755,7 +755,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -770,7 +770,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -785,7 +785,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -800,7 +800,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -815,7 +815,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -830,7 +830,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -845,7 +845,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -860,7 +860,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -875,7 +875,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -890,7 +890,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -905,7 +905,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -920,7 +920,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -935,7 +935,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -950,7 +950,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -965,7 +965,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -980,7 +980,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -995,7 +995,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1010,7 +1010,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1025,7 +1025,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1040,7 +1040,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1055,7 +1055,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1070,7 +1070,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1085,7 +1085,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1100,7 +1100,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1115,7 +1115,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1130,7 +1130,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1145,7 +1145,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1160,7 +1160,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1175,7 +1175,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1190,7 +1190,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1205,7 +1205,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1220,7 +1220,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1235,7 +1235,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1250,7 +1250,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1265,7 +1265,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1280,7 +1280,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1295,7 +1295,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1310,7 +1310,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1325,7 +1325,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1340,7 +1340,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1355,7 +1355,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1370,7 +1370,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1385,7 +1385,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1400,7 +1400,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1415,7 +1415,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1430,7 +1430,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1445,7 +1445,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1460,7 +1460,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1475,7 +1475,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1490,7 +1490,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1505,7 +1505,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1520,7 +1520,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1535,7 +1535,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1550,7 +1550,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1565,7 +1565,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1580,7 +1580,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1595,7 +1595,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1610,7 +1610,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1625,7 +1625,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1640,7 +1640,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1655,7 +1655,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1670,7 +1670,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1685,7 +1685,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1700,7 +1700,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1715,7 +1715,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1730,7 +1730,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1745,7 +1745,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1760,7 +1760,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1775,7 +1775,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1790,7 +1790,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1805,7 +1805,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1820,7 +1820,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1835,7 +1835,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1850,7 +1850,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1865,7 +1865,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1880,7 +1880,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1895,7 +1895,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1910,7 +1910,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1925,7 +1925,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1940,7 +1940,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1955,7 +1955,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1970,7 +1970,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -1985,7 +1985,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2000,7 +2000,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2015,7 +2015,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2030,7 +2030,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2045,7 +2045,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2060,7 +2060,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2075,7 +2075,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2090,7 +2090,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2105,7 +2105,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2120,7 +2120,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2135,7 +2135,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2150,7 +2150,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2165,7 +2165,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2180,7 +2180,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2195,7 +2195,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2210,7 +2210,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2225,7 +2225,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2240,7 +2240,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2255,7 +2255,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2270,7 +2270,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2285,7 +2285,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2300,7 +2300,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2315,7 +2315,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2330,7 +2330,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2345,7 +2345,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2360,7 +2360,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2375,7 +2375,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2390,7 +2390,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2405,7 +2405,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2420,7 +2420,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2435,7 +2435,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2450,7 +2450,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2465,7 +2465,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2480,7 +2480,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2495,7 +2495,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2510,7 +2510,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2525,7 +2525,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2540,7 +2540,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2555,7 +2555,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2570,7 +2570,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2585,7 +2585,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2600,7 +2600,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2615,7 +2615,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2630,7 +2630,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2645,7 +2645,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2660,7 +2660,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2675,7 +2675,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2690,7 +2690,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2705,7 +2705,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2720,7 +2720,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2735,7 +2735,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2750,7 +2750,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2765,7 +2765,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2780,7 +2780,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2795,7 +2795,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2810,7 +2810,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2825,7 +2825,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2840,7 +2840,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2855,7 +2855,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2870,7 +2870,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2885,7 +2885,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2900,7 +2900,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2915,7 +2915,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2930,7 +2930,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2945,7 +2945,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2960,7 +2960,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2975,7 +2975,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -2990,7 +2990,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3005,7 +3005,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3020,7 +3020,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3035,7 +3035,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3050,7 +3050,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3065,7 +3065,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3080,7 +3080,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3095,7 +3095,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3110,7 +3110,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3125,7 +3125,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3140,7 +3140,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3155,7 +3155,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3170,7 +3170,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3185,7 +3185,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3200,7 +3200,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3215,7 +3215,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3230,7 +3230,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3245,7 +3245,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3260,7 +3260,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3275,7 +3275,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3290,7 +3290,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3305,7 +3305,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3320,7 +3320,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3335,7 +3335,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3350,7 +3350,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3365,7 +3365,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3380,7 +3380,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3395,7 +3395,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3410,7 +3410,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3425,7 +3425,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3440,7 +3440,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3455,7 +3455,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3470,7 +3470,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3485,7 +3485,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3500,7 +3500,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3515,7 +3515,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3530,7 +3530,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3545,7 +3545,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3560,7 +3560,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3575,7 +3575,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3590,7 +3590,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3605,7 +3605,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3620,7 +3620,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3635,7 +3635,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3650,7 +3650,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3665,7 +3665,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3680,7 +3680,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3695,7 +3695,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3710,7 +3710,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3725,7 +3725,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3740,7 +3740,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3755,7 +3755,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3770,7 +3770,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3785,7 +3785,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3800,7 +3800,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3815,7 +3815,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3830,7 +3830,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3845,7 +3845,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3860,7 +3860,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3875,7 +3875,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3890,7 +3890,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3905,7 +3905,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3920,7 +3920,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3935,7 +3935,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3950,7 +3950,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3965,7 +3965,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3980,7 +3980,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -3995,7 +3995,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4010,7 +4010,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4025,7 +4025,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4040,7 +4040,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4055,7 +4055,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4070,7 +4070,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4085,7 +4085,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4100,7 +4100,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4115,7 +4115,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4130,7 +4130,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4145,7 +4145,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4160,7 +4160,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4175,7 +4175,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4190,7 +4190,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4205,7 +4205,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4220,7 +4220,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4235,7 +4235,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4250,7 +4250,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4265,7 +4265,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4280,7 +4280,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4295,7 +4295,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4310,7 +4310,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4325,7 +4325,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4340,7 +4340,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4355,7 +4355,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4370,7 +4370,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4385,7 +4385,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4400,7 +4400,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4415,7 +4415,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4430,7 +4430,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4445,7 +4445,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4460,7 +4460,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4475,7 +4475,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4490,7 +4490,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4505,7 +4505,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4520,7 +4520,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4535,7 +4535,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4550,7 +4550,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4565,7 +4565,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4580,7 +4580,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4595,7 +4595,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4610,7 +4610,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4625,7 +4625,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4640,7 +4640,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4655,7 +4655,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4670,7 +4670,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4685,7 +4685,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4700,7 +4700,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4715,7 +4715,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4730,7 +4730,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4745,7 +4745,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4760,7 +4760,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4775,7 +4775,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4790,7 +4790,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4805,7 +4805,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4820,7 +4820,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4835,7 +4835,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4850,7 +4850,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4865,7 +4865,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4880,7 +4880,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4895,7 +4895,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4910,7 +4910,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4925,7 +4925,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4940,7 +4940,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4955,7 +4955,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
@@ -4970,7 +4970,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.08, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.05, 0.0,]
+rpy_des = [ 0.0, 0.08, 0.0,]
 pos_des = [ 0.0, 0.0, 0.001,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.5,]
diff --git a/task_3/Gait_Params_up.toml b/task_3/Gait_Params_up.toml
index 21cc6a1..a4da226 100644
--- a/task_3/Gait_Params_up.toml
+++ b/task_3/Gait_Params_up.toml
@@ -4,7 +4,7 @@
 # 1
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -31,7 +31,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -57,7 +57,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -83,7 +83,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -110,7 +110,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -137,7 +137,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -163,7 +163,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -189,7 +189,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -215,7 +215,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -242,7 +242,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -268,7 +268,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -294,7 +294,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -320,7 +320,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -347,7 +347,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -373,7 +373,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -399,7 +399,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -425,7 +425,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -452,7 +452,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -478,7 +478,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -504,7 +504,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -531,7 +531,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -558,7 +558,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -584,7 +584,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -610,7 +610,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -636,7 +636,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -663,7 +663,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -689,7 +689,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -715,7 +715,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -741,7 +741,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -768,7 +768,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -794,7 +794,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -820,7 +820,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -846,7 +846,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -873,7 +873,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -899,7 +899,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -925,7 +925,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -952,7 +952,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -979,7 +979,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1005,7 +1005,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1031,7 +1031,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1057,7 +1057,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1084,7 +1084,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1110,7 +1110,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1136,7 +1136,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1162,7 +1162,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1189,7 +1189,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1215,7 +1215,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1241,7 +1241,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1267,7 +1267,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1294,7 +1294,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1320,7 +1320,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1346,7 +1346,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1373,7 +1373,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1400,7 +1400,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1426,7 +1426,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1452,7 +1452,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1478,7 +1478,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1505,7 +1505,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1531,7 +1531,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1557,7 +1557,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1583,7 +1583,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1610,7 +1610,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1636,7 +1636,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1662,7 +1662,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1688,7 +1688,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1715,7 +1715,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1741,7 +1741,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1767,7 +1767,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1794,7 +1794,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1821,7 +1821,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1847,7 +1847,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1873,7 +1873,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1899,7 +1899,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -1926,7 +1926,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1952,7 +1952,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -1978,7 +1978,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2004,7 +2004,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2031,7 +2031,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2057,7 +2057,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2083,7 +2083,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2109,7 +2109,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2136,7 +2136,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2162,7 +2162,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2188,7 +2188,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2214,7 +2214,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2241,7 +2241,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2267,7 +2267,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2293,7 +2293,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2319,7 +2319,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2346,7 +2346,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2372,7 +2372,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2398,7 +2398,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2424,7 +2424,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2451,7 +2451,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2477,7 +2477,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2503,7 +2503,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2529,7 +2529,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2556,7 +2556,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2582,7 +2582,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2608,7 +2608,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2635,7 +2635,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2662,7 +2662,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2688,7 +2688,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2714,7 +2714,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2740,7 +2740,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2767,7 +2767,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2793,7 +2793,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2819,7 +2819,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2845,7 +2845,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2872,7 +2872,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2898,7 +2898,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2924,7 +2924,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -2950,7 +2950,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -2977,7 +2977,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3003,7 +3003,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3029,7 +3029,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3056,7 +3056,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3083,7 +3083,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3109,7 +3109,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3135,7 +3135,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3161,7 +3161,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3188,7 +3188,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3214,7 +3214,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3240,7 +3240,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3266,7 +3266,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3293,7 +3293,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3319,7 +3319,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3345,7 +3345,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3371,7 +3371,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3398,7 +3398,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3424,7 +3424,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3450,7 +3450,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3477,7 +3477,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3504,7 +3504,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3530,7 +3530,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3556,7 +3556,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3582,7 +3582,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3609,7 +3609,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3635,7 +3635,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3661,7 +3661,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3687,7 +3687,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3714,7 +3714,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3740,7 +3740,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3766,7 +3766,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3792,7 +3792,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3819,7 +3819,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3845,7 +3845,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3871,7 +3871,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3898,7 +3898,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -3925,7 +3925,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3951,7 +3951,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -3977,7 +3977,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4003,7 +4003,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4030,7 +4030,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4056,7 +4056,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4082,7 +4082,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4108,7 +4108,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4135,7 +4135,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4161,7 +4161,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4187,7 +4187,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4213,7 +4213,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4240,7 +4240,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4266,7 +4266,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4292,7 +4292,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4319,7 +4319,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4346,7 +4346,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4372,7 +4372,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4398,7 +4398,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4424,7 +4424,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4451,7 +4451,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4477,7 +4477,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4503,7 +4503,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4529,7 +4529,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4556,7 +4556,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4582,7 +4582,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4608,7 +4608,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4634,7 +4634,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4661,7 +4661,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4687,7 +4687,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4713,7 +4713,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4740,7 +4740,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4767,7 +4767,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4793,7 +4793,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4819,7 +4819,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4845,7 +4845,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4872,7 +4872,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4898,7 +4898,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4924,7 +4924,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -4950,7 +4950,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -4977,7 +4977,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5003,7 +5003,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5029,7 +5029,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5055,7 +5055,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5082,7 +5082,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5108,7 +5108,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5134,7 +5134,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5161,7 +5161,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5188,7 +5188,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5214,7 +5214,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5240,7 +5240,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5266,7 +5266,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5293,7 +5293,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5319,7 +5319,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5345,7 +5345,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5371,7 +5371,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5398,7 +5398,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5424,7 +5424,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5450,7 +5450,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5476,7 +5476,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5503,7 +5503,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5529,7 +5529,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5555,7 +5555,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5581,7 +5581,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5608,7 +5608,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5634,7 +5634,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5660,7 +5660,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5686,7 +5686,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5713,7 +5713,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5739,7 +5739,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5765,7 +5765,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5791,7 +5791,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5818,7 +5818,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5844,7 +5844,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5870,7 +5870,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5896,7 +5896,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -5923,7 +5923,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5949,7 +5949,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -5975,7 +5975,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6002,7 +6002,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6029,7 +6029,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6055,7 +6055,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6081,7 +6081,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6107,7 +6107,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6134,7 +6134,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6160,7 +6160,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6186,7 +6186,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6212,7 +6212,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6239,7 +6239,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6265,7 +6265,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6291,7 +6291,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6317,7 +6317,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6344,7 +6344,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6370,7 +6370,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6396,7 +6396,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6423,7 +6423,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6450,7 +6450,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6476,7 +6476,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6502,7 +6502,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6528,7 +6528,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6555,7 +6555,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6581,7 +6581,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6607,7 +6607,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6633,7 +6633,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6660,7 +6660,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6686,7 +6686,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6712,7 +6712,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6738,7 +6738,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6765,7 +6765,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6791,7 +6791,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6817,7 +6817,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6844,7 +6844,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6871,7 +6871,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6897,7 +6897,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6923,7 +6923,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -6949,7 +6949,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -6976,7 +6976,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7002,7 +7002,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7028,7 +7028,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7054,7 +7054,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7081,7 +7081,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7107,7 +7107,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7133,7 +7133,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7159,7 +7159,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7186,7 +7186,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7212,7 +7212,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7238,7 +7238,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7265,7 +7265,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7292,7 +7292,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7318,7 +7318,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7344,7 +7344,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7370,7 +7370,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7397,7 +7397,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7423,7 +7423,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7449,7 +7449,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7475,7 +7475,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7502,7 +7502,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7528,7 +7528,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7554,7 +7554,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7580,7 +7580,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7607,7 +7607,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7633,7 +7633,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7659,7 +7659,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7686,7 +7686,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7713,7 +7713,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7739,7 +7739,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7765,7 +7765,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7791,7 +7791,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7818,7 +7818,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7844,7 +7844,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7870,7 +7870,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7896,7 +7896,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -7923,7 +7923,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7949,7 +7949,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -7975,7 +7975,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8001,7 +8001,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8028,7 +8028,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8054,7 +8054,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8080,7 +8080,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8107,7 +8107,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8134,7 +8134,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8160,7 +8160,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8186,7 +8186,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8212,7 +8212,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8239,7 +8239,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8265,7 +8265,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8291,7 +8291,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8317,7 +8317,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8344,7 +8344,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8370,7 +8370,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8396,7 +8396,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8422,7 +8422,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8449,7 +8449,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8475,7 +8475,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8501,7 +8501,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8528,7 +8528,7 @@ weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 # 1
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8555,7 +8555,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8581,7 +8581,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8607,7 +8607,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8633,7 +8633,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005] #roll、pitch、yaw、x、y、z
 body_vel_des = [0.10, 0.0, 0.0] #x,y,yaw
 type = "usergait"
 #x，y，z方向的落地位置偏置
@@ -8660,7 +8660,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8686,7 +8686,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
@@ -8712,7 +8712,7 @@ use_mpc_traj = 1
 weight = [20.0, 25.0, 30.0, 10.0, 10.0, 50.0]
 
 [[step]]
-body_pos_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.005]
+body_pos_des = [0.0, -0.5, 0.0, 0.0, 0.0, 0.005]
 body_vel_des = [0.10, 0.0, 0.0]
 duration = 300
 gait_id = 110
diff --git a/task_3/Gait_Params_up_full.toml b/task_3/Gait_Params_up_full.toml
index 4f331cb..7e3f34b 100644
--- a/task_3/Gait_Params_up_full.toml
+++ b/task_3/Gait_Params_up_full.toml
@@ -5,7 +5,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -20,7 +20,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -35,7 +35,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -50,7 +50,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -65,7 +65,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -80,7 +80,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -95,7 +95,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -110,7 +110,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -125,7 +125,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -140,7 +140,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -155,7 +155,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -170,7 +170,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -185,7 +185,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -200,7 +200,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -215,7 +215,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -230,7 +230,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -245,7 +245,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -260,7 +260,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -275,7 +275,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -290,7 +290,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -305,7 +305,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -320,7 +320,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -335,7 +335,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -350,7 +350,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -365,7 +365,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -380,7 +380,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -395,7 +395,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -410,7 +410,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -425,7 +425,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -440,7 +440,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -455,7 +455,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -470,7 +470,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -485,7 +485,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -500,7 +500,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -515,7 +515,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -530,7 +530,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -545,7 +545,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -560,7 +560,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -575,7 +575,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -590,7 +590,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -605,7 +605,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -620,7 +620,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -635,7 +635,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -650,7 +650,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -665,7 +665,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -680,7 +680,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -695,7 +695,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -710,7 +710,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -725,7 +725,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -740,7 +740,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -755,7 +755,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -770,7 +770,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -785,7 +785,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -800,7 +800,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -815,7 +815,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -830,7 +830,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -845,7 +845,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -860,7 +860,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -875,7 +875,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -890,7 +890,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -905,7 +905,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -920,7 +920,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -935,7 +935,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -950,7 +950,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -965,7 +965,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -980,7 +980,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -995,7 +995,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1010,7 +1010,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1025,7 +1025,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1040,7 +1040,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1055,7 +1055,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1070,7 +1070,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1085,7 +1085,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1100,7 +1100,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1115,7 +1115,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1130,7 +1130,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1145,7 +1145,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1160,7 +1160,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1175,7 +1175,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1190,7 +1190,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1205,7 +1205,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1220,7 +1220,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1235,7 +1235,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1250,7 +1250,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1265,7 +1265,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1280,7 +1280,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1295,7 +1295,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1310,7 +1310,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1325,7 +1325,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1340,7 +1340,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1355,7 +1355,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1370,7 +1370,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1385,7 +1385,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1400,7 +1400,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1415,7 +1415,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1430,7 +1430,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1445,7 +1445,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1460,7 +1460,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1475,7 +1475,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1490,7 +1490,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1505,7 +1505,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1520,7 +1520,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1535,7 +1535,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1550,7 +1550,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1565,7 +1565,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1580,7 +1580,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1595,7 +1595,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1610,7 +1610,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1625,7 +1625,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1640,7 +1640,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1655,7 +1655,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1670,7 +1670,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1685,7 +1685,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1700,7 +1700,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1715,7 +1715,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1730,7 +1730,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1745,7 +1745,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1760,7 +1760,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1775,7 +1775,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1790,7 +1790,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1805,7 +1805,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1820,7 +1820,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1835,7 +1835,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1850,7 +1850,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1865,7 +1865,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1880,7 +1880,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1895,7 +1895,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1910,7 +1910,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1925,7 +1925,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1940,7 +1940,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1955,7 +1955,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1970,7 +1970,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -1985,7 +1985,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2000,7 +2000,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2015,7 +2015,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2030,7 +2030,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2045,7 +2045,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2060,7 +2060,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2075,7 +2075,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2090,7 +2090,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2105,7 +2105,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2120,7 +2120,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2135,7 +2135,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2150,7 +2150,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2165,7 +2165,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2180,7 +2180,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2195,7 +2195,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2210,7 +2210,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2225,7 +2225,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2240,7 +2240,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2255,7 +2255,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2270,7 +2270,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2285,7 +2285,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2300,7 +2300,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2315,7 +2315,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2330,7 +2330,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2345,7 +2345,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2360,7 +2360,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2375,7 +2375,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2390,7 +2390,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2405,7 +2405,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2420,7 +2420,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2435,7 +2435,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2450,7 +2450,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2465,7 +2465,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2480,7 +2480,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2495,7 +2495,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2510,7 +2510,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2525,7 +2525,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2540,7 +2540,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2555,7 +2555,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2570,7 +2570,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2585,7 +2585,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2600,7 +2600,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2615,7 +2615,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2630,7 +2630,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2645,7 +2645,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2660,7 +2660,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2675,7 +2675,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2690,7 +2690,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2705,7 +2705,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2720,7 +2720,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2735,7 +2735,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2750,7 +2750,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2765,7 +2765,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2780,7 +2780,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2795,7 +2795,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2810,7 +2810,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2825,7 +2825,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2840,7 +2840,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2855,7 +2855,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2870,7 +2870,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2885,7 +2885,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2900,7 +2900,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2915,7 +2915,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2930,7 +2930,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2945,7 +2945,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2960,7 +2960,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2975,7 +2975,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -2990,7 +2990,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3005,7 +3005,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3020,7 +3020,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3035,7 +3035,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3050,7 +3050,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3065,7 +3065,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3080,7 +3080,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3095,7 +3095,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3110,7 +3110,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3125,7 +3125,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3140,7 +3140,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3155,7 +3155,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3170,7 +3170,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3185,7 +3185,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3200,7 +3200,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3215,7 +3215,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3230,7 +3230,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3245,7 +3245,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3260,7 +3260,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3275,7 +3275,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3290,7 +3290,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3305,7 +3305,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3320,7 +3320,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3335,7 +3335,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3350,7 +3350,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3365,7 +3365,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3380,7 +3380,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3395,7 +3395,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3410,7 +3410,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3425,7 +3425,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3440,7 +3440,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3455,7 +3455,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3470,7 +3470,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3485,7 +3485,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3500,7 +3500,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3515,7 +3515,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3530,7 +3530,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3545,7 +3545,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3560,7 +3560,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3575,7 +3575,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3590,7 +3590,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3605,7 +3605,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3620,7 +3620,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3635,7 +3635,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3650,7 +3650,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3665,7 +3665,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3680,7 +3680,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3695,7 +3695,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3710,7 +3710,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3725,7 +3725,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3740,7 +3740,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3755,7 +3755,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3770,7 +3770,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3785,7 +3785,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3800,7 +3800,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3815,7 +3815,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3830,7 +3830,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3845,7 +3845,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3860,7 +3860,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3875,7 +3875,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3890,7 +3890,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3905,7 +3905,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3920,7 +3920,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3935,7 +3935,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3950,7 +3950,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3965,7 +3965,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3980,7 +3980,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -3995,7 +3995,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4010,7 +4010,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4025,7 +4025,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4040,7 +4040,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4055,7 +4055,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4070,7 +4070,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4085,7 +4085,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4100,7 +4100,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4115,7 +4115,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4130,7 +4130,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4145,7 +4145,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4160,7 +4160,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4175,7 +4175,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4190,7 +4190,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4205,7 +4205,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4220,7 +4220,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4235,7 +4235,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4250,7 +4250,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4265,7 +4265,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4280,7 +4280,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4295,7 +4295,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4310,7 +4310,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4325,7 +4325,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4340,7 +4340,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4355,7 +4355,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4370,7 +4370,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4385,7 +4385,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4400,7 +4400,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4415,7 +4415,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4430,7 +4430,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4445,7 +4445,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4460,7 +4460,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4475,7 +4475,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4490,7 +4490,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4505,7 +4505,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4520,7 +4520,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4535,7 +4535,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4550,7 +4550,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4565,7 +4565,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4580,7 +4580,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4595,7 +4595,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4610,7 +4610,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4625,7 +4625,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4640,7 +4640,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4655,7 +4655,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4670,7 +4670,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4685,7 +4685,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4700,7 +4700,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4715,7 +4715,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4730,7 +4730,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4745,7 +4745,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4760,7 +4760,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4775,7 +4775,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4790,7 +4790,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4805,7 +4805,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4820,7 +4820,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4835,7 +4835,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4850,7 +4850,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4865,7 +4865,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4880,7 +4880,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4895,7 +4895,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4910,7 +4910,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4925,7 +4925,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4940,7 +4940,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4955,7 +4955,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
@@ -4970,7 +4970,7 @@ gait_id = 110
 contact = 17
 life_count = 0
 vel_des = [ 0.1, 0.0, 0.0,]
-rpy_des = [ 0.0, 0.0, 0.0,]
+rpy_des = [ 0.0, -0.5, 0.0,]
 pos_des = [ 0.0, 0.0, 0.005,]
 acc_des = [ 20.0, 25.0, 30.0, 10.0, 10.0, 50.0,]
 ctrl_point = [ 0.02, 0.0, 0.2,]
diff --git a/task_3/task_3.py b/task_3/task_3.py
index 8b40265..6f91fe7 100644
--- a/task_3/task_3.py
+++ b/task_3/task_3.py
@@ -42,7 +42,6 @@ robot_cmd = {
 
 def pass_up_down(ctrl, msg):
     usergait_msg = file_send_lcmt()
-    # lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
     
     try:
         steps = toml.load("./task_3/Gait_Params_up.toml")
@@ -96,27 +95,13 @@ def pass_up_down(ctrl, msg):
         msg.duration = 1000
         msg.life_count += 1
         
-        # 参数设置
-        stable_count = 0  # 用于计数z轴稳定的次数
-        stable_threshold = 8  # 连续15次检测z轴不再增加则认为已经停止
-        z_speed_threshold = 0.01  # z轴速度阈值，小于这个值认为已经停止爬升
         climb_speed_threshold = 0.05  # 检测到开始爬坡的速度阈值
-        max_iterations = 280  # 最大循环次数，作为安全保障
-        min_iterations = 170  # 最小循环次数，作为安全保障
-        y_target = 7.5  # 目标y坐标，达到此值时停止
-
-        # 姿态判断参数
-        pitch_threshold = 0.05  # 俯仰角阈值（弧度）
-        angular_rate_threshold = 0.03  # 角速度阈值（弧度/秒）
+        max_iterations = 400  # 最大循环次数，作为安全保障
+        y_target = 7.8  # 目标y坐标，达到此值时停止
 
         # 阶段控制
         climbing_detected = False  # 是否检测到正在爬坡
         
-        # 高度变化记录
-        height_window = []
-        pitch_window = []
-        window_size = 8
-        
         # 记录起始姿态和高度
         start_height = ctrl.odo_msg.xyz[2]
         start_y = ctrl.odo_msg.xyz[1]
@@ -131,15 +116,6 @@ def pass_up_down(ctrl, msg):
             current_height = ctrl.odo_msg.xyz[2]  # 当前高度
             current_y = ctrl.odo_msg.xyz[1]  # 当前y坐标
             current_pitch = ctrl.odo_msg.rpy[1]  # 当前俯仰角
-            pitch_rate = ctrl.odo_msg.omegaBody[1]  # 俯仰角速度
-            vbody_z = ctrl.odo_msg.vBody[2]  # 机体坐标系Z速度
-            
-            # 更新滑动窗口数据
-            height_window.append(current_height)
-            pitch_window.append(current_pitch)
-            if len(height_window) > window_size:
-                height_window.pop(0)
-                pitch_window.pop(0)
             
             # 每10次迭代打印一次当前信息
             if i % 10 == 0:
@@ -167,7 +143,7 @@ def pass_up_down(ctrl, msg):
     section('任务3-2：x = 2', "开始")
     turn_degree_v2(ctrl, msg, 90, absolute=True)
     center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False, detect_height=0.3)
-    go_straight(ctrl, msg, distance=0.3, speed=0.5, observe=True)
+    go_straight(ctrl, msg, distance=0.6, speed=0.5, observe=True)
     time.sleep(1)
 
     section('任务3-3：down', "完成")
@@ -224,29 +200,16 @@ def pass_up_down(ctrl, msg):
         msg.life_count += 1
         
         # 参数设置
-        stable_count = 0  # 用于计数z轴稳定的次数
-        stable_threshold = 10  # 连续10次检测z轴速度接近零则认为已经到达平地
-        z_speed_threshold = 0.005  # z轴速度阈值，小于这个值认为已经停止下降
         descent_speed_threshold = -0.05  # 检测到开始下坡的速度阈值（负值表示下降）
-        max_iterations = 110  # 最大循环次数，作为安全保障
-        min_iterations = 80  # 最小循环次数，确保有足够的时间开始动作
+        max_iterations = 200  # 最大循环次数，作为安全保障
         start_height = ctrl.odo_msg.xyz[2]  # 记录起始高度
         start_y = ctrl.odo_msg.xyz[1]  # 记录起始y坐标
-        y_target = 10.3  # 目标y坐标，达到此值时停止
-        
-        # 姿态判断参数
-        pitch_threshold = 0.05  # 俯仰角阈值（弧度）
-        angular_rate_threshold = 0.03  # 角速度阈值（弧度/秒）
+        y_target = 10.2  # 目标y坐标，达到此值时停止
         
         # 阶段控制
         descending_detected = False  # 是否检测到正在下坡
         flat_ground_detected = False  # 是否检测到已到达平地
         
-        # 高度变化记录
-        height_window = []
-        pitch_window = []
-        window_size = 8
-        
         info(f"开始监测下坡过程，初始高度: {start_height}, 初始Y坐标: {start_y:.3f}", "监测")
         
         for i in range(max_iterations):
@@ -258,16 +221,7 @@ def pass_up_down(ctrl, msg):
             current_height = ctrl.odo_msg.xyz[2]  # 当前高度
             current_y = ctrl.odo_msg.xyz[1]  # 当前y坐标
             current_pitch = ctrl.odo_msg.rpy[1]  # 当前俯仰角
-            pitch_rate = ctrl.odo_msg.omegaBody[1]  # 俯仰角速度
-            vbody_z = ctrl.odo_msg.vBody[2]  # 机体坐标系Z速度
-            
-            # 更新滑动窗口数据
-            height_window.append(current_height)
-            pitch_window.append(current_pitch)
-            if len(height_window) > window_size:
-                height_window.pop(0)
-                pitch_window.pop(0)
-            
+
             # 每10次迭代打印一次当前信息
             if observe and i % 10 == 0:
                 info(f"Step:{i} 当前Y坐标={current_y:.3f}, 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}", "监测")
@@ -459,6 +413,7 @@ def run_task_3(ctrl, msg, time_sleep=5000):
     info('开始执行任务3...', "启动")
 
     turn_degree_v2(ctrl, msg, 90, absolute=True)
+    go_straight(ctrl, msg, distance=0.1, speed=0.5, observe=True)
 
     section('任务3-1：up', "开始")
     pass_up_down(ctrl, msg)
diff --git a/utils/detect_dual_track_lines.py b/utils/detect_dual_track_lines.py
index 5accca5..bb26548 100644
--- a/utils/detect_dual_track_lines.py
+++ b/utils/detect_dual_track_lines.py
@@ -11,6 +11,7 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True,
                             min_line_length=0.05,
                             max_line_gap=40,
                             detect_height=0.4):
+    save_log = False
     """
     检测左右两条平行的黄色轨道线，优化后能够更准确处理各种路况