├── README.md
├── fpp_planner
    └── fpp_planner.m
├── hardware_params.m
├── main.m
├── math
    ├── rot_x.m
    ├── rot_y.m
    ├── rot_z.m
    ├── rot_zyx.m
    └── skew_mat.m
├── mpc_controller
    ├── add_state_boundaries.asv
    ├── add_state_boundaries.m
    ├── form_mpc_prob.m
    └── unpacks_sol.m
├── srb_dynamics
    └── get_srb_dynamics.m
├── state_estimation
    └── state_estimation.m
├── video
    ├── 2d_walk_1.gif
    ├── 2d_walk_1.mp4
    ├── 3d_running_6.mp4
    ├── 3d_walk_1.gif
    ├── 3d_walk_1.mp4
    ├── 3d_walk_2.gif
    ├── 3d_walk_2.mp4
    ├── 3d_walk_3.gif
    ├── 3d_walk_3.mp4
    ├── 3d_walk_4.gif
    ├── 3d_walk_4.mp4
    ├── 3d_walk_5.mp4
    ├── 3d_walk_6.mp4
    └── walking_combined.mp4
└── visualization
    ├── leg_ik.m
    ├── plot_cube.m
    └── rbt_anime.m


/README.md:
--------------------------------------------------------------------------------
 1 | # bipedal_pointfoot_mpc_ctr
 2 | 
 3 | <br><img src="video/2d_walk_1.gif" width="500"> <br>
 4 | 
 5 | <br><img src="video/3d_walk_1.gif" width="500"> <br>
 6 | 
 7 | <br><img src="video/3d_walk_2.gif" width="500"> <br>
 8 | 
 9 | <br><img src="video/3d_walk_3.gif" width="500"> <br>
10 | 
11 | <br><img src="video/3d_walk_4.gif" width="500"> <br>


--------------------------------------------------------------------------------
/fpp_planner/fpp_planner.m:
--------------------------------------------------------------------------------
 1 | function [ref_traj_v] = fpp_planner(world_p, body_p, ctr_p, path)
 2 | 
 3 | addpath(path.casadi);
 4 | import casadi.*;
 5 | 
 6 | %% Generate reference trajectory
 7 | ref_traj_v.x_ref_val = zeros(12,ctr_p.N+1);
 8 | ref_traj_v.f_ref_val = zeros(6,ctr_p.N);
 9 | ref_traj_v.fp_ref_val = zeros(6,ctr_p.N);
10 | 
11 | for i = 1:6
12 |     ref_traj_v.x_ref_val(i,:) = linspace(ctr_p.x_init_tar_val(i),ctr_p.x_final_tar_val(i),ctr_p.N+1); %rpy xyz
13 |     ref_traj_v.x_ref_val(i+6,:) = linspace(ctr_p.dx_init_tar_val(i),ctr_p.dx_final_tar_val(i),ctr_p.N+1); %velocity
14 | end
15 | 
16 | % spine on the z axis
17 | s_a = [ref_traj_v.x_ref_val(4,1),ref_traj_v.x_ref_val(4,ctr_p.N/2),ref_traj_v.x_ref_val(4,ctr_p.N)]; % x axis
18 | s_b = [ctr_p.x_init_tar_val(6),ctr_p.x_final_tar_val(6),ctr_p.x_init_tar_val(6)+0]; % z axis
19 | ref_traj_v.x_ref_val(6,:) = interp1(s_a,s_b,ref_traj_v.x_ref_val(4,:),'spline');
20 | 
21 | 
22 | % calcuate foot placement points, openloop one
23 | t_stance = ctr_p.contact_state_ratio(1)*ctr_p.dt_val(1);
24 | ww = body_p.width;
25 | ll = 0;
26 | 
27 | for k=1:ctr_p.N
28 | 
29 |     fp_symmetry = t_stance/2*ctr_p.dx_final_tar_val(4:6);
30 | 
31 |     fp_centrifugal = cross((1/2*sqrt(ctr_p.init_z/world_p.g)*ctr_p.dx_final_tar_val(4:6)), ctr_p.dx_final_tar_val(1:3));
32 | 
33 |     rot_mat_z_arr = rotz(ref_traj_v.x_ref_val(3,k));
34 |     hip_com_g_vec_r = ref_traj_v.x_ref_val(4:6,k) + rot_mat_z_arr* [0; 1*ww/2; 0] - [0;0;ctr_p.init_z];
35 |     hip_com_g_vec_l = ref_traj_v.x_ref_val(4:6,k) + rot_mat_z_arr* [0; -1*ww/2; 0] - [0;0;ctr_p.init_z];
36 |     
37 |     ref_traj_v.fp_ref_val(:,k) = [hip_com_g_vec_r;hip_com_g_vec_l]+...
38 |         repmat(fp_symmetry,2,1)+...
39 |         repmat(fp_centrifugal,2,1);
40 |    
41 |     contact_v = ctr_p.contact_state_val(:,k);
42 |     
43 |     % calcuate ref pos for swing foot
44 |     period_now = mod(k,ctr_p.N/ctr_p.gait_num);
45 |     period_now_rad = pi*(period_now/(ctr_p.N/ctr_p.gait_num));
46 |     
47 |     for leg_k = 1:2
48 |         if(contact_v(leg_k) == 0)
49 |             ref_traj_v.fp_ref_val(leg_k*3,k) = 0.2 * sin(period_now_rad); % sine foot swing height
50 |         end
51 |     end
52 |     
53 | end
54 | 
55 | % combine all ref traj
56 | ref_traj_v.p = [reshape(ref_traj_v.x_ref_val,body_p.state_dim*(ctr_p.N+1),1);...
57 |                 reshape(ref_traj_v.f_ref_val,body_p.f_dim*ctr_p.N,1);...
58 |                 reshape(ref_traj_v.fp_ref_val,body_p.fp_dim*ctr_p.N,1);...
59 |                 reshape(ctr_p.contact_state_val,2*ctr_p.N,1)]; % * 2 legs
60 |       
61 | ref_traj_v.x0 = [reshape(ref_traj_v.x_ref_val,body_p.state_dim*(ctr_p.N+1),1);...
62 |                  reshape(ref_traj_v.f_ref_val,body_p.f_dim*ctr_p.N,1);...
63 |                  reshape(ref_traj_v.fp_ref_val,body_p.fp_dim*ctr_p.N,1)]; % initial states
64 |       
65 | end
66 | 
67 | 


--------------------------------------------------------------------------------
/hardware_params.m:
--------------------------------------------------------------------------------
  1 | function [world, body, ctr, path] = hardware_params()
  2 | %% Casadi path
  3 | % Change to your casadi path
  4 | path.casadi = 'D:\matlab_lib\casadi-3.6.0-windows64-matlab2018b';
  5 | 
  6 | %% Simulation params
  7 | world.fk = 0.5; % friction
  8 | world.g = 9.81; % gravity constant
  9 | 
 10 | world.friction_cone = [1/world.fk, 0 -1;...
 11 |                       -1/world.fk, 0 -1;...
 12 |                       0, 1/world.fk, -1;...
 13 |                       0, -1/world.fk, -1];
 14 | 
 15 | %% Controller params
 16 | ctr.phase_num = 4;
 17 | ctr.N = 80*3; % mpc period window
 18 | ctr.T = 3*3; % mpc period time
 19 | ctr.dt_val = (ctr.T/ctr.N) .* ones(ctr.N,1); % dt vector
 20 | 
 21 | ctr.max_jump_z = 0.55; % max jumping height, constraints
 22 | ctr.min_dump_z = 0.15; % min standing height
 23 | ctr.max_lift_vel_z = 6.5; % max jumping velocity
 24 | ctr.init_z = 0.3;
 25 | 
 26 | ctr.x_init_tar_val = [0; 0; 0; 0; 0; ctr.init_z]; % init state
 27 | ctr.dx_init_tar_val = [0; 0; 0; 0; 0; 0]; % init d state
 28 | ctr.x_final_tar_val = [0; 0; 0.2*9*0; 0.4*9*1.5; 0.4*9*0; 0.3]; % final target state r p y; x y z
 29 | ctr.dx_final_tar_val = [0; 0; 0.2*0; 0.4*1.5; 0.4*0; 0];
 30 | 
 31 | %ctr.contact_state_ratio = ctr.N.*[0.35 0.15 0.475 0.025]; % pull, jump, flight, impact
 32 | ctr.contact_state_ratio = ctr.N.*[1/48 1/12 1/12 1/12 1/12 1/12 1/12 1/12 1/12 1/12 1/12 1/12]; % pull, jump, flight, impact
 33 | 
 34 | % ctr.contact_state_val = [ones(2, ctr.contact_state_ratio(1)),...
 35 | %                              0 * ones(2, ctr.contact_state_ratio(2)),...
 36 | %                              ones(2, ctr.contact_state_ratio(3)),...
 37 | %                              0 * ones(2, ctr.contact_state_ratio(4)),...
 38 | %                              ones(2, ctr.contact_state_ratio(1)),...
 39 | %                              0 * ones(2, ctr.contact_state_ratio(2)),...
 40 | %                              ones(2, ctr.contact_state_ratio(3)),...
 41 | %                              0 * ones(2, ctr.contact_state_ratio(4)),...
 42 | %                              ones(2, ctr.contact_state_ratio(1)),...
 43 | %                              0 * ones(2, ctr.contact_state_ratio(2)),...
 44 | %                              ones(2, ctr.contact_state_ratio(3)),...
 45 | %                              0 * ones(2, ctr.contact_state_ratio(4))]; % no foot contact during last 2 phases
 46 | 
 47 | ctr.gait_num = 12;
 48 | ctr.contact_state_val = [repmat([1;0], 1, ctr.contact_state_ratio(1)),...
 49 |                          repmat([0;0], 1, ctr.contact_state_ratio(1)),...
 50 |                          repmat([0;1], 1, ctr.contact_state_ratio(1)),...
 51 |                          repmat([0;0], 1, ctr.contact_state_ratio(1))];
 52 | ctr.contact_state_val = repmat(ctr.contact_state_val, 1, ctr.gait_num);
 53 | % no foot contact during last 2 phases
 54 |                  
 55 | % cost gains
 56 | ctr.weight.QX = [10 10 0, 10 10 10, 10 10 10, 10 10 10 ]'; % state error
 57 | ctr.weight.QN = [10 10 0, 50 50 50, 10 10 10, 10 10 10 ]'; % state error, final
 58 | ctr.weight.Qc = 100*[50 50 50]'; % foot placement error on 3 axis
 59 | ctr.weight.Qf = [0.1 0.1 0.1]'; % input error on 3 axis
 60 | 
 61 | % casadi optimal settings
 62 | ctr.opt_setting.expand =true;
 63 | ctr.opt_setting.ipopt.max_iter=1500;
 64 | ctr.opt_setting.ipopt.print_level=0;
 65 | ctr.opt_setting.ipopt.acceptable_tol=1e-4;
 66 | ctr.opt_setting.ipopt.acceptable_obj_change_tol=1e-6;
 67 | ctr.opt_setting.ipopt.tol=1e-4;
 68 | ctr.opt_setting.ipopt.nlp_scaling_method='gradient-based';
 69 | ctr.opt_setting.ipopt.constr_viol_tol=1e-3;
 70 | ctr.opt_setting.ipopt.fixed_variable_treatment='relax_bounds';
 71 | 
 72 | %% Robot params
 73 | 
 74 | body.state_dim = 12; % number of dim of state, rpy xyz dot_rpy dot_xyz
 75 | body.f_dim = 6; % number of dim of leg force, 3*2, 2 leg
 76 | body.fp_dim = 6; % number of dim of leg pos, 3*2, 2 leg
 77 | 
 78 | 
 79 | body.m = 5;
 80 | body.i_vec = [0.06 0.1 0.05]*2;
 81 | body.i_mat = [body.i_vec(1) 0 0;... % roll
 82 |               0 body.i_vec(2) 0;... % pitch
 83 |               0 0 body.i_vec(3)]; % yaw
 84 |        
 85 | %body.length = 0.34;
 86 | body.width = 0.3; % distance between 2 legs
 87 | 
 88 | % leg length
 89 | body.l1 = 0.2;
 90 | body.l2 = 0.17;
 91 | 
 92 | % foot motion range, in m
 93 | body.foot_x_range = 0.15;
 94 | body.foot_y_range = 0.15;
 95 | body.foot_z_range = 0.3;
 96 | 
 97 | % output force range
 98 | body.max_zforce = 100;
 99 | 
100 | % calaute 2 hip positions
101 | body.hip_vec = [0; body.width/2; 0];
102 | body.hip_dir_mat = [0 0; 1 -1; 0 0]; % 3,2 hip vec
103 | body.hip_pos = body.hip_dir_mat .* repmat(body.hip_vec,1,2); % 3,2 hip pos vec
104 | body.foot_pos = repmat([0; 0; -0.6*ctr.init_z],1,2); % init foot pos
105 | 
106 | body.phip_swing_ref = body.hip_pos + body.foot_pos;
107 | % ref foot pos at swing phase
108 | 
109 | body.phip_swing_ref_vec = reshape(body.phip_swing_ref,[],1);
110 | 
111 | % the range foot can move within
112 | body.foot_convex_hull = [1 0 0 -body.foot_x_range;
113 |                         -1 0 0 -body.foot_x_range;
114 |                         0 1 0 -body.foot_y_range;
115 |                         0 -1 0 -body.foot_y_range;
116 |                         0 0 1 -ctr.min_dump_z;
117 |                         0 0 -1 -body.foot_z_range];
118 |                 
119 | 
120 | end


--------------------------------------------------------------------------------
/main.m:
--------------------------------------------------------------------------------
  1 | %% Add casadi path
  2 | 
  3 | clc; clear;
  4 | close all; warning off;
  5 | 
  6 | %% Get hardware params & set package path
  7 | [world_params, body_params, ctr_params, path] = hardware_params();
  8 | 
  9 | addpath(path.casadi);
 10 | import casadi.*;
 11 | 
 12 | addpath('math\');
 13 | addpath('srb_dynamics\');
 14 | addpath('mpc_controller\');
 15 | addpath('fpp_planner\');
 16 | addpath('visualization\');
 17 | 
 18 | %% Get dynamics
 19 | [dyn_f] = get_srb_dynamics(world_params, body_params, path);
 20 | 
 21 | %% Form the mpc problem
 22 | [mpc_v, mpc_c, mpc_p] = form_mpc_prob(world_params, body_params, ctr_params, dyn_f, path);
 23 | %%
 24 | [boundray_v] = add_state_boundaries(mpc_v, mpc_c, world_params, body_params, ctr_params, path);
 25 | %%
 26 | [ref_traj_v] = fpp_planner(world_params, body_params, ctr_params, path);
 27 | 
 28 | %% Slove the NLP prob
 29 | sol = mpc_p.solver('x0',ref_traj_v.x0,...
 30 |                    'lbx',boundray_v.lbx,...
 31 |                    'ubx',boundray_v.ubx,...
 32 |                    'lbg',boundray_v.lbg,...
 33 |                    'ubg',boundray_v.ubg,...
 34 |                    'p',ref_traj_v.p);
 35 |                
 36 |   %% Unpack data
 37 |   [x_sol, f_sol, fp_l_sol, fp_g_sol] = unpacks_sol(sol, body_params, ctr_params, path);
 38 |   
 39 |   %%
 40 |   rbt_anime(x_sol,f_sol,fp_g_sol,ref_traj_v,ctr_params.T,ctr_params.N,body_params);
 41 |   
 42 | %   %% Plots
 43 | %  clf;
 44 | %  subplot(1,2,1);
 45 | % plot(x_sol(4,:),"LineWidth",1.5);
 46 | % hold on;
 47 | % plot(x_sol(5,:),"LineWidth",1.5);
 48 | % hold on;
 49 | % plot(x_sol(6,:),"LineWidth",1.5);
 50 | % hold on;
 51 | % title("Body's Position")
 52 | % xlabel("Step")
 53 | % ylabel("m")
 54 | % legend("Position X","Position Y","Position Z");
 55 | % grid on  
 56 | % 
 57 | %  subplot(1,2,2);
 58 | %  plot(x_sol(1,:),"LineWidth",1.5);
 59 | % hold on;
 60 | % plot(x_sol(2,:),"LineWidth",1.5);
 61 | % hold on;
 62 | % plot(x_sol(3,:),"LineWidth",1.5);
 63 | % hold on;
 64 | % title("Body's Euler Angle")
 65 | % xlabel("Step")
 66 | % ylabel("rad")
 67 | % legend("Roll","Pitch","Yaw");
 68 | % grid on  
 69 | % 
 70 | % %% velocity plot
 71 | % clf;
 72 | % plot(x_sol(9,:),"LineWidth",1.5);
 73 | % hold on;
 74 | % plot(x_sol(10,:),"LineWidth",1.5);
 75 | % hold on;
 76 | % title("Body's Velocity")
 77 | % xlabel("Step")
 78 | % ylabel("m/s or rad/s")
 79 | % legend("Yaw Velocity rad/s","X Velocity m/s");
 80 | % grid on  
 81 | % 
 82 | % %% fpp
 83 | % clf;
 84 | %  subplot(1,2,1);
 85 | % plot(fp_g_sol(1,:),"LineWidth",1.5);
 86 | % hold on;
 87 | % plot(fp_g_sol(2,:),"LineWidth",1.5);
 88 | % hold on;
 89 | % plot(fp_g_sol(3,:),"LineWidth",1.5);
 90 | % hold on;
 91 | % title("Foot Placement Point global of FL leg")
 92 | % xlabel("Step")
 93 | % ylabel("m")
 94 | % legend("Position X","Position Y","Position Z");
 95 | % grid on
 96 | % 
 97 | %  subplot(1,2,2);
 98 | %  plot(f_sol(1,:),"LineWidth",1.5);
 99 | % hold on;
100 | % plot(f_sol(2,:),"LineWidth",1.5);
101 | % hold on;
102 | % plot(f_sol(3,:),"LineWidth",1.5);
103 | % hold on;
104 | % title("Ground Reaction Force of FL leg")
105 | % xlabel("Step")
106 | % ylabel("N")
107 | % legend("Force X","Force Y","Force Z");
108 | % grid on  
109 | 


--------------------------------------------------------------------------------
/math/rot_x.m:
--------------------------------------------------------------------------------
1 | function rot_mat_x = rot_x(rad)
2 | 
3 | c = cos(rad);
4 | s = sin(rad);
5 | 
6 | rot_mat_x = [1 0 0;...
7 |              0 c -1*s;...
8 |              0 s  c];
9 | end


--------------------------------------------------------------------------------
/math/rot_y.m:
--------------------------------------------------------------------------------
1 | function rot_mat_y = rot_y(rad)
2 | 
3 | c = cos(rad);
4 | s = sin(rad);
5 | 
6 | rot_mat_y = [c 0 s;...
7 |              0 1 0;...
8 |              -1*s 0 c];
9 | end


--------------------------------------------------------------------------------
/math/rot_z.m:
--------------------------------------------------------------------------------
1 | function rot_mat_z = rot_z(rad)
2 | 
3 | c = cos(rad);
4 | s = sin(rad);
5 | 
6 | rot_mat_z = [c -1*s 0;...
7 |              s c 0;...
8 |              0 0 1];
9 | end


--------------------------------------------------------------------------------
/math/rot_zyx.m:
--------------------------------------------------------------------------------
 1 | function rot_mat = rot_zyx(vec_)
 2 | 
 3 | psi_ = vec_(3);
 4 | theta_ = vec_(2);
 5 | phi_ = vec_(1);
 6 | 
 7 | %z-psi-偏航-yaw
 8 | %y-theta-俯仰-pitch
 9 | %x-phi-横滚-roll
10 | 
11 | rot_mat_z = rot_z(psi_);
12 | rot_mat_y = rot_y(theta_);
13 | rot_mat_x = rot_x(phi_);
14 | 
15 | rot_mat = rot_mat_z * rot_mat_y * rot_mat_x;
16 | 
17 | end
18 | 


--------------------------------------------------------------------------------
/math/skew_mat.m:
--------------------------------------------------------------------------------
 1 | function s_mat = skew_mat(vec_)
 2 | 
 3 | % generate a skew-symmetric matrix for 3d vec's corss product
 4 | % [a]x b = a x b
 5 | 
 6 | a1 = vec_(1);
 7 | a2 = vec_(2);
 8 | a3 = vec_(3);
 9 | 
10 | s_mat = [0 -1*a3 a2;...
11 |          a3 0 -1*a1;...
12 |          -1*a2 a1 0];
13 | 
14 | end


--------------------------------------------------------------------------------
/mpc_controller/add_state_boundaries.asv:
--------------------------------------------------------------------------------
 1 | %% lower & upper boundary of eq & ieq constrains
 2 | 
 3 | function [outputArg1,outputArg2] = add_state_boundaries(mpc_v, mpc_c, world_p, body_p, ctr_p, path)
 4 | 
 5 | addpath(path.casadi);
 6 | import casadi.*;
 7 | 
 8 | % eq constraint equation Ac = 0 
 9 | args.lbg(1:mpc_c.eq_con_dim) = 0;
10 | args.ubg(1:mpc_c.eq_con_dim) = 0;
11 | 
12 | % ineq constraint equation -inf<Ac<0
13 | args.lbg(mpc_c.eq_con_dim+1: mpc_c.eq_con_dim+mpc_c.ineq_con_dim) = -inf;
14 | args.ubg(mpc_c.eq_con_dim+1: mpc_c.eq_con_dim+mpc_c.ineq_con_dim) = 0;
15 | 
16 | % state lower & upper boundary
17 | % state
18 | ub_state = [3*pi*ones(3,1);10*ones(2,1);ctr_p.max_jump_z;...
19 |             5*pi*ones(3,1);50*ones(2,1);ctr_p.max_lift_vel_z];
20 | lb_state = [-3*pi*ones(3,1);-10*ones(2,1);ctr_p.min_dump_z;...
21 |             -5*pi*ones(3,1);-50*ones(3,1)];
22 | ub_state_arr = repmat(ub_state,N+1,1);
23 | lb_state_arr = repmat(lb_state,N+1,1);
24 | 
25 | % leg force
26 | ub_f_leg = [body_p.m*world_p.g*world_p.fk*50; body_p.m*world_p.g*world_p.fk*50; body_p.max_zforce]; %xyz maximum leg force
27 | lb_f_leg = [-1*body.m*world.g*world.fk*50; -1*body.m*world.g*world.fk*50; 0]; %minimum leg force
28 | ub_f = repmat(ub_f_leg,4,1); % for 4 legs
29 | lb_f = repmat(lb_f_leg,4,1);
30 | ub_f_arr = repmat(ub_f,N,1);
31 | lb_f_arr = repmat(lb_f,N,1);
32 | 
33 | % foot pos
34 | ub_fp = repmat([0.4;0.4;inf],4,1);
35 | lb_fp = repmat([-0.4;-0.4;-inf],4,1);
36 | ub_fp_arr = repmat(ub_fp,N,1);
37 | lb_fp_arr = repmat(lb_fp,N,1);
38 | 
39 | % combine lower & upper bounds together
40 | args.ubx = [ub_state_arr; ub_f_arr; ub_fp_arr];
41 | args.lbx = [lb_state_arr; lb_f_arr; lb_fp_arr];
42 | 
43 | end
44 | 
45 | 


--------------------------------------------------------------------------------
/mpc_controller/add_state_boundaries.m:
--------------------------------------------------------------------------------
 1 | %% lower & upper boundary of eq & ieq constrains
 2 | 
 3 | function [boundray_v] = add_state_boundaries(mpc_v, mpc_c, world_p, body_p, ctr_p, path)
 4 | 
 5 | addpath(path.casadi);
 6 | import casadi.*;
 7 | 
 8 | % eq constraint equation Ac = 0 
 9 | boundray_v.lbg(1:mpc_c.eq_con_dim) = 0;
10 | boundray_v.ubg(1:mpc_c.eq_con_dim) = 0;
11 | 
12 | % ineq constraint equation -inf<Ac<0
13 | boundray_v.lbg(mpc_c.eq_con_dim+1: mpc_c.eq_con_dim+mpc_c.ineq_con_dim) = -inf;
14 | boundray_v.ubg(mpc_c.eq_con_dim+1: mpc_c.eq_con_dim+mpc_c.ineq_con_dim) = 0;
15 | 
16 | % state lower & upper boundary
17 | % state
18 | ub_state = [3*pi*ones(3,1);10*ones(2,1);ctr_p.max_jump_z;...
19 |             5*pi*ones(3,1);50*ones(2,1);ctr_p.max_lift_vel_z];
20 | lb_state = [-3*pi*ones(3,1);-10*ones(2,1);ctr_p.min_dump_z;...
21 |             -5*pi*ones(3,1);-50*ones(3,1)];
22 | ub_state_arr = repmat(ub_state,ctr_p.N+1,1);
23 | lb_state_arr = repmat(lb_state,ctr_p.N+1,1);
24 | 
25 | % leg force
26 | ub_f_leg = [body_p.m*world_p.g*world_p.fk; body_p.m*world_p.g*world_p.fk; body_p.max_zforce]; %xyz maximum leg force
27 | lb_f_leg = [-1*body_p.m*world_p.g*world_p.fk; -1*body_p.m*world_p.g*world_p.fk; 0]; %minimum leg force
28 | ub_f = repmat(ub_f_leg,2,1); % for 2 legs *
29 | lb_f = repmat(lb_f_leg,2,1);
30 | ub_f_arr = repmat(ub_f,ctr_p.N,1);
31 | lb_f_arr = repmat(lb_f,ctr_p.N,1);
32 | 
33 | % foot pos
34 | ub_fp = repmat([0.4;0.4;inf],2,1); % for 2 legs
35 | lb_fp = repmat([-0.4;-0.4;-inf],2,1);
36 | ub_fp_arr = repmat(ub_fp,ctr_p.N,1);
37 | lb_fp_arr = repmat(lb_fp,ctr_p.N,1);
38 | 
39 | % combine lower & upper bounds together, can be changed in each mpc cycle
40 | boundray_v.ubx = [ub_state_arr; ub_f_arr; ub_fp_arr];
41 | boundray_v.lbx = [lb_state_arr; lb_f_arr; lb_fp_arr];
42 | 
43 | end
44 | 
45 | 


--------------------------------------------------------------------------------
/mpc_controller/form_mpc_prob.m:
--------------------------------------------------------------------------------
  1 | %% Get all constraints in a mpc cycle
  2 | 
  3 | % mpc variables, mpc contraints, mpc problem & solver
  4 | function [mpc_v, mpc_c, mpc_p] = form_mpc_prob(world_p, body_p, ctr_p, dyn_f, path)
  5 | 
  6 | addpath(path.casadi);
  7 | import casadi.*;
  8 | 
  9 | %% Casadi variables array in one prediction widow
 10 | mpc_v.x_arr = SX.sym('x_arr', body_p.state_dim, ctr_p.N+1); % state
 11 | mpc_v.f_arr = SX.sym('f_arr', body_p.f_dim, ctr_p.N); % foot force / input
 12 | mpc_v.fp_arr = SX.sym('fp_arr', body_p.fp_dim, ctr_p.N); % foot position
 13 | 
 14 | %% Reference traj
 15 | mpc_v.x_ref_arr = SX.sym('x_ref_arr', body_p.state_dim, ctr_p.N+1); % state
 16 | mpc_v.f_ref_arr = SX.sym('f_ref_arr', body_p.f_dim, ctr_p.N); % foot force
 17 | mpc_v.fp_ref_arr = SX.sym('fp_ref_arr', body_p.fp_dim, ctr_p.N); % foot position
 18 | % contact mat arr, which the leg touches ground, set by fpp_planner
 19 | % only foot on ground can output a ground reaction force, for 2 legs
 20 | mpc_v.contact_mat_arr = SX.sym('contact_mat_arr', 2, ctr_p.N);
 21 | 
 22 | mpc_v.cost_fcn = 0; % the cost function
 23 | 
 24 | %% Constraints and cost function
 25 | N = ctr_p.N;
 26 | state_dim = body_p.state_dim; % number of dim of state, rpy xyz dot_rpy dot_xyz
 27 | f_dim = body_p.f_dim; % number of dim of leg force, 3*4
 28 | fp_dim = body_p.fp_dim; % number of dim of leg pos, 3*4
 29 | 
 30 | % equal constraints
 31 | mpc_c.eq_con_dynamic = SX.zeros(state_dim*(N+1),1); % dynamic constraint, 12*(N+1),1
 32 | mpc_c.eq_con_foot_contact_ground = SX.zeros(2*N,1); % constraint on leg's motion range, 2*N,1 *
 33 | mpc_c.eq_con_foot_non_slip = SX.zeros(fp_dim*N,1); % constraint on foot's non slip stane phase, 6*N,1
 34 | mpc_c.eq_con_init_state = mpc_v.x_ref_arr(:,1)-mpc_v.x_arr(:,1); % set init condition constraint, 12,1
 35 | 
 36 | mpc_c.eq_con_dim = state_dim*(N+1) + 2*N + fp_dim*N + 12; % dim for eq constraints
 37 | 
 38 | % inequal constraints 
 39 | mpc_c.ineq_con_foot_range = SX.zeros(2*6*N,1); % ieq constraint on foot's motion range 2 legs*3 axis*2 dir *
 40 | mpc_c.ineq_con_foot_friction = SX.zeros(2*4*N,1); % ieq constraint on foot friction 2 legs*2axis*2dir *
 41 | mpc_c.ineq_con_zforce_dir = SX.zeros(1*N,1); % z axis force always pointing up
 42 | mpc_c.ineq_con_zforce_range = SX.zeros(2*N,1); % z axis force range, swing phase ->0, stance phase -> < max z force, 2 legs *
 43 | 
 44 | mpc_c.ineq_con_dim = 2*6*N + 2*4*N + N + 2*N; % dim for ieq constraints
 45 | 
 46 | fprintf('%d eq constraints and %d ineq constraints', mpc_c.eq_con_dim, mpc_c.ineq_con_dim);
 47 | 
 48 | % define cost function and constraints
 49 | for k = 1:N
 50 |     x_t = mpc_v.x_arr(:,k); % current state
 51 |     x_next = mpc_v.x_arr(:,k+1); %next state
 52 |     
 53 |     f_t = mpc_v.f_arr(:,k); % current force
 54 |     fp_t = mpc_v.fp_arr(:,k); % current foot placement point
 55 |     fp_g_t = repmat(x_t(4:6),2,1)+fp_t; %foot pos in global coord * 2 legs
 56 |     
 57 |     x_ref_t = mpc_v.x_ref_arr(:,k); % current reference state
 58 |     f_ref_t = mpc_v.f_ref_arr(:,k); % current reference force
 59 |     fp_ref_t = mpc_v.fp_ref_arr(:,k); % current reference foot placement point
 60 |     
 61 |     contact_mat_t = mpc_v.contact_mat_arr(:,k); % current foot contact point
 62 |     dt_t = ctr_p.dt_val(k);
 63 |     
 64 |     % constraints
 65 |     % dynamic equation constraint
 66 |     mpc_c.eq_con_dynamic(state_dim*(k-1)+1:state_dim*k) = x_next - (x_t + dyn_f(x_t,f_t,fp_t)*dt_t);
 67 |     % zforce direction always point up
 68 |     mpc_c.ineq_con_zforce_dir(k) = -1*dot(f_t,repmat([0;0;1],2,1)); % * 2 legs, dot out 1 value
 69 |     % zforce range < max z force
 70 |     mpc_c.ineq_con_zforce_range(2*(k-1)+1:2*k) = f_t([3,6]) - contact_mat_t.*repmat(body_p.max_zforce,2,1); % * 2 legs
 71 |     
 72 |     for leg_k = 1:2
 73 | 
 74 |         xyz_i = 3*(leg_k-1)+1:3*leg_k; % index for xyz dir
 75 |         
 76 |         % constrant leg on ground, z=0
 77 |         mpc_c.eq_con_foot_contact_ground(2*(k-1)+leg_k) = contact_mat_t(leg_k)*fp_g_t(3*(leg_k-1)+3); % * 2 legs
 78 |         
 79 |         % keep foot within motion range
 80 |         rot_zyx_t = rot_zyx(x_t(1:3));
 81 |         hip_pos_global_t = rot_zyx_t*body_p.phip_swing_ref + x_t(4:6);
 82 |         leg_vec_t = (fp_g_t(xyz_i) - hip_pos_global_t(:,leg_k)); % leg vector, from hip to foot
 83 | 
 84 |         mpc_c.ineq_con_foot_range(2*6*(k-1)+6*(leg_k-1)+1: 2*6*(k-1)+6*leg_k) =...
 85 |             body_p.foot_convex_hull*[leg_vec_t;1]; % leg's motion range limit * 2 legs
 86 |         
 87 |         % ground reaction force within friction cone 
 88 |         % * 2 legs *  2 +- dir * 2 axis, xy // 4 = 2 axis * 2 dir
 89 |         mpc_c.ineq_con_foot_friction(2*4*(k-1)+2*2*(leg_k-1)+1: 2*4*(k-1)+2*2*leg_k) = world_p.friction_cone*f_t(xyz_i);
 90 |         
 91 |         % non-slip, if leg touches the ground, ffp_now = ffp_next
 92 |         if(k<N)
 93 |             fp_g_next = repmat(x_next(4:6),2,1)+mpc_v.fp_arr(:,k+1); % next foot pos in global coord * 2 legs
 94 | 
 95 |             mpc_c.eq_con_foot_non_slip(2*3*(k-1)+3*(leg_k-1)+1: 2*3*(k-1)+3*leg_k) =...
 96 |                 contact_mat_t(leg_k)*(fp_g_next(xyz_i)-fp_g_t(xyz_i)); % * 2 legs * 3 axis, x,y,z
 97 |         end
 98 |         
 99 |     end
100 |     
101 |     % Add up errors to cost fcn
102 |     x_err = x_t - x_ref_t; % state error
103 |     f_err = f_t - f_ref_t; % leg force error
104 |     % foot placement pos error. may change to fp_ref_t with fpp planner *
105 |     fp_err = fp_g_t - fp_ref_t;
106 |     
107 |     % running cost * 2 legs
108 |     mpc_v.cost_fcn = mpc_v.cost_fcn + (x_err'*diag(ctr_p.weight.QX)*x_err...
109 |                            + f_err'*diag(repmat(ctr_p.weight.Qf,2,1))*f_err...
110 |                            + fp_err'*diag(repmat(ctr_p.weight.Qc,2,1))*fp_err) * dt_t;
111 |                        
112 | end
113 | 
114 | % terminate cost
115 | x_err_f = mpc_v.x_arr(:,N+1)-mpc_v.x_ref_arr(:,N+1);
116 | mpc_v.cost_fcn = mpc_v.cost_fcn + x_err_f'*diag(ctr_p.weight.QN)*x_err_f;
117 | 
118 | % combine all constraints
119 | mpc_c.constraint_arr = [mpc_c.eq_con_init_state; mpc_c.eq_con_dynamic; mpc_c.eq_con_foot_contact_ground; mpc_c.eq_con_foot_non_slip;...
120 |                         mpc_c.ineq_con_foot_range; mpc_c.ineq_con_foot_friction; mpc_c.ineq_con_zforce_dir; mpc_c.ineq_con_zforce_range];
121 | 
122 | %% Init the optimal problem, solver
123 | % reform, state, leg force, foot pos into one n*1 vector
124 | mpc_v.opt_variables = [reshape(mpc_v.x_arr, state_dim*(N+1),1);...
125 |                        reshape(mpc_v.f_arr, f_dim*N,1);...
126 |                        reshape(mpc_v.fp_arr, fp_dim*N,1)];
127 | % reform, reference state, leg force, foot pos into one n*1 vector * 2 legs
128 | mpc_v.opt_ref_param =[reshape(mpc_v.x_ref_arr, state_dim*(N+1),1);...
129 |                       reshape(mpc_v.f_ref_arr, f_dim*N,1);...
130 |                       reshape(mpc_v.fp_ref_arr, fp_dim*N,1);...
131 |                       reshape(mpc_v.contact_mat_arr, 2*N,1)];
132 | 
133 | % construct the mpc problem and solver
134 | mpc_p.nlp_prob = struct('f',mpc_v.cost_fcn, 'x',mpc_v.opt_variables, 'p',mpc_v.opt_ref_param, 'g',mpc_c.constraint_arr);
135 | mpc_p.solver = nlpsol('solver','ipopt',mpc_p.nlp_prob, ctr_p.opt_setting);
136 | 
137 | %% gen c code
138 | 
139 | 
140 | end
141 | 
142 | 


--------------------------------------------------------------------------------
/mpc_controller/unpacks_sol.m:
--------------------------------------------------------------------------------
 1 | function [x_sol, f_sol, fp_l_sol, fp_g_sol] = unpacks_sol(sol, body_p, ctr_p, path)
 2 | 
 3 | addpath(path.casadi);
 4 | import casadi.*;
 5 | 
 6 | % unpack data from casadi result 
 7 | state_dim = body_p.state_dim;
 8 | f_dim = body_p.f_dim;
 9 | fp_dim = body_p.fp_dim;
10 | N = ctr_p.N;
11 | 
12 | x_sol=sol.x(1:state_dim*(N+1));
13 | x_sol=reshape(full(x_sol),state_dim,(N+1));% COM under world coord
14 | 
15 | f_sol=sol.x(state_dim*(N+1)+1:state_dim*(N+1)+f_dim*N);
16 | f_sol=reshape(full(f_sol),f_dim,N);% foot force
17 | 
18 | fp_l_sol=sol.x(state_dim*(N+1)+f_dim*N+1:state_dim*(N+1)+f_dim*N+fp_dim*N);
19 | % Foot pos in rbt coord
20 | fp_l_sol=reshape(full(fp_l_sol),f_dim,N);
21 | 
22 | % Foot pos in world coord. * 2 legs
23 | fp_g_sol=fp_l_sol+repmat(x_sol(4:6,1:end-1),2,1);
24 | 
25 | end
26 | 
27 | 


--------------------------------------------------------------------------------
/srb_dynamics/get_srb_dynamics.m:
--------------------------------------------------------------------------------
 1 | %% get SRB dynamic model of point foot bipedal robot
 2 | % Shuang Peng 05/2023 
 3 | 
 4 | function [dyn_f] = get_srb_dynamics(world_p, body_p, path)
 5 | 
 6 | addpath(path.casadi);
 7 | import casadi.*;
 8 | 
 9 | % build the dynamic equation
10 | % use casadi variables for optimization
11 | x_k = SX.sym('x_k', body_p.state_dim, 1); % state 12
12 | f_k = SX.sym('f_k', body_p.f_dim, 1); % foot force 6
13 | fp_k = SX.sym('fp_k', body_p.fp_dim, 1); % foot position 6
14 | 
15 | % z-psi-yaw
16 | % y-theta-pitch
17 | % x-phi-roll
18 | 
19 | rot_mat_zyx = rot_zyx(x_k(1:3));
20 | 
21 | s_yaw = sin(x_k(3));
22 | c_yaw = cos(x_k(3));
23 | t_yaw = tan(x_k(3));
24 | 
25 | s_pitch = sin(x_k(2));
26 | c_pitch = cos(x_k(2));
27 | t_pitch = tan(x_k(2));
28 | 
29 | inv_rot_linear = [c_yaw s_yaw 0; 
30 |                   -1*s_yaw c_yaw 0; 
31 |                   0 0 1];
32 |               
33 | inv_rot_nonlinear = [c_yaw/c_pitch s_yaw/c_pitch 0;
34 |                      -1*s_yaw c_yaw 0;
35 |                      c_yaw*t_pitch s_yaw*t_pitch 1];
36 |                 
37 | % convert the intertia tensor from local cod to world cod
38 | i_mat_w = rot_mat_zyx*body_p.i_mat*rot_mat_zyx'; %i_mat in world
39 | i_mat_w_inv = eye(3)/i_mat_w;
40 | 
41 | % A, B, G mat
42 | A = [zeros(3) zeros(3) inv_rot_linear zeros(3);...
43 |      zeros(3) zeros(3) zeros(3) eye(3);...
44 |      zeros(3) zeros(3) zeros(3) zeros(3);...
45 |      zeros(3) zeros(3) zeros(3) zeros(3)];
46 | % A = [z3 z3 inv_rot z3;
47 | %      z3 z3 z3      I3
48 | %      z3 z3 z3      z3
49 | %      z3 z3 z3      z3];
50 | 
51 | B = [zeros(3) zeros(3);...
52 |      zeros(3) zeros(3);...
53 |      i_mat_w_inv*skew_mat(fp_k(1:3)), i_mat_w_inv*skew_mat(fp_k(4:6));...
54 |      eye(3)/body_p.m, eye(3)/body_p.m];
55 | % B = [z3 z3 z3 z3
56 | %      z3 z3 z3 z3
57 | %      (I_m)^-1*[f_pos]x
58 | %      I3/m I3/m I3/m I3/m]; 12*6
59 | 
60 | G = zeros(12,1);
61 | G(12) = -1*world_p.g;
62 | 
63 | d_x = A*x_k + B*f_k + G;
64 | 
65 | % map the dynamic function
66 | dyn_f = Function('dyn_f',{x_k;f_k;fp_k},{d_x},{'state','leg_force','foot_pos'},{'d_state'});
67 | 
68 | end


--------------------------------------------------------------------------------
/state_estimation/state_estimation.m:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/state_estimation/state_estimation.m


--------------------------------------------------------------------------------
/video/2d_walk_1.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/2d_walk_1.gif


--------------------------------------------------------------------------------
/video/2d_walk_1.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/2d_walk_1.mp4


--------------------------------------------------------------------------------
/video/3d_running_6.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/3d_running_6.mp4


--------------------------------------------------------------------------------
/video/3d_walk_1.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/3d_walk_1.gif


--------------------------------------------------------------------------------
/video/3d_walk_1.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/3d_walk_1.mp4


--------------------------------------------------------------------------------
/video/3d_walk_2.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/3d_walk_2.gif


--------------------------------------------------------------------------------
/video/3d_walk_2.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/3d_walk_2.mp4


--------------------------------------------------------------------------------
/video/3d_walk_3.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/3d_walk_3.gif


--------------------------------------------------------------------------------
/video/3d_walk_3.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/3d_walk_3.mp4


--------------------------------------------------------------------------------
/video/3d_walk_4.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/3d_walk_4.gif


--------------------------------------------------------------------------------
/video/3d_walk_4.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/3d_walk_4.mp4


--------------------------------------------------------------------------------
/video/3d_walk_5.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/3d_walk_5.mp4


--------------------------------------------------------------------------------
/video/3d_walk_6.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/3d_walk_6.mp4


--------------------------------------------------------------------------------
/video/walking_combined.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/psrobotics/bipedal_pointfoot_mpc_ctr/eb5029ca5e8767065627cf1b7bb416cacd0f1d29/video/walking_combined.mp4


--------------------------------------------------------------------------------
/visualization/leg_ik.m:
--------------------------------------------------------------------------------
 1 | function [j_rad_arr, j_pos_arr] = leg_ik(leg_vec,l1,l2)
 2 | % get joint rad and pos for 3dof leg with a input leg vec
 3 | 
 4 | x = leg_vec(1);
 5 | y = leg_vec(2);
 6 | z = leg_vec(3);
 7 | 
 8 | d_yz = sqrt(y^2+z^2);
 9 | d_xyz = sqrt(x^2+y^2+z^2);
10 | j_rad_arr.a1 = atan2(z,y);
11 | 
12 | b1 = acos((l1^2+d_xyz^2-l2^2)/(2*l1*d_xyz));
13 | b2 = atan2( -1*d_yz,x );
14 | j_rad_arr.a2 = b1+b2;
15 | 
16 | b3 = ( (l1^2+l2^2-d_xyz^2)/(2*l1*l2) );
17 | j_rad_arr.a3 = pi+j_rad_arr.a2-b3;
18 | 
19 | j_pos_arr.hip = [0;0;0];
20 | d1 = l1*sin(j_rad_arr.a2);
21 | j_pos_arr.knee = [l1*cos(j_rad_arr.a2); -1*d1*cos(j_rad_arr.a1); -1*d1*sin(j_rad_arr.a1)];
22 | j_pos_arr.foot = leg_vec;
23 | 
24 | end


--------------------------------------------------------------------------------
/visualization/plot_cube.m:
--------------------------------------------------------------------------------
1 | function plot_cube(R, a, b, c,pos)
2 | x = [-1,  1,  1, -1, -1,  1,  1, -1] * a/2;
3 | y = [-1, -1,  1,  1, -1, -1,  1,  1] * b/2;
4 | z = [-1, -1, -1, -1,  1,  1,  1,  1] * c/2;
5 | P = R * [x; y; z]+pos;
6 | order = [1, 2, 3, 4, 1, 5, 6, 7, 8, 5, 6, 2, 3, 7, 8, 4];
7 | plot3(P(1, order), P(2, order), P(3, order), 'black','linewidth',2);
8 | end


--------------------------------------------------------------------------------
/visualization/rbt_anime.m:
--------------------------------------------------------------------------------
 1 | function [] = rbt_anime(x_arr,f_arr,fp_arr,ref_traj_v,T,N,body_p)
 2 | 
 3 | size_arr = size(x_arr);
 4 | len = size_arr(2);
 5 | 
 6 | figure(1);
 7 | 
 8 |  v = VideoWriter('3d_walk_6','MPEG-4');
 9 |  v.FrameRate = N/T;
10 |  open(v);
11 | 
12 | for k = 1:1:len-1
13 | 
14 |     x_t = x_arr(:,k);
15 | 
16 |     fp_w_1 = fp_arr(1:3,k);
17 |     fp_w_2 = fp_arr(4:6,k);
18 | 
19 |     ref_fp_w_1 = ref_traj_v.fp_ref_val(1:3,k);
20 |     ref_fp_w_2 = ref_traj_v.fp_ref_val(4:6,k);
21 |     
22 |     r_mat = rot_zyx(x_t(1:3));
23 |     
24 |     foot_pos = fp_arr(:,k);
25 |     feetforce_used = f_arr(:,k)*0.5;
26 | 
27 |     % get leg ik info
28 |     hip_g_r = x_t(4:6) + r_mat * [0; 0.3/2; 0];
29 |     hip_g_l = x_t(4:6) + r_mat * [0; -0.3/2; 0];
30 |     leg_vec_r = fp_w_1 - hip_g_r;
31 |     leg_vec_l = fp_w_2 - hip_g_l;
32 | 
33 |     [j_r_r, j_p_r] = leg_ik(leg_vec_r,0.29,0.27);
34 |     [j_r_l, j_p_l] = leg_ik(leg_vec_l,0.29,0.27);
35 | 
36 |     p_knee_g_r = hip_g_r + r_mat*j_p_r.knee;
37 |     p_knee_g_l = hip_g_l + r_mat*j_p_l.knee;
38 | 
39 |     p_leg_arr_r = [x_t(4:6), hip_g_r, p_knee_g_r, fp_w_1];
40 |     p_leg_arr_l = [x_t(4:6), hip_g_l, p_knee_g_l, fp_w_2];
41 |     
42 |     clf;
43 |     hold on;
44 |     axis equal;
45 |     grid on;
46 |     
47 |     view(-34,33);
48 |     %view(90,0)
49 |     axis([x_t(4)-0.9,x_t(4)+0.9,-0.6,0.6,0,1.2]);
50 |     
51 |     plot_cube(r_mat,0.15,0.25,0.4,x_t(4:6)+r_mat*[0;0;0.2]);
52 | 
53 |     % leg 
54 |     plot3(p_leg_arr_r(1,:), p_leg_arr_r(2,:), p_leg_arr_r(3,:), 'black','linewidth',2);
55 |     plot3(p_leg_arr_l(1,:), p_leg_arr_l(2,:), p_leg_arr_l(3,:), 'black','linewidth',2);
56 | 
57 |     % foot
58 |     plot3(fp_w_1(1),fp_w_1(2),fp_w_1(3),'o','linewidth',2,'color','b','markersize',6);
59 |     plot3(fp_w_2(1),fp_w_2(2),fp_w_2(3),'o','linewidth',2,'color','b','markersize',6);
60 | 
61 |     % hip
62 |     plot3(hip_g_r(1),hip_g_r(2),hip_g_r(3),'o','linewidth',2,'color','b','markersize',6);
63 |     plot3(hip_g_l(1),hip_g_l(2),hip_g_l(3),'o','linewidth',2,'color','b','markersize',6);
64 | 
65 |     % knee
66 |     plot3(p_knee_g_r(1),p_knee_g_r(2),p_knee_g_r(3),'o','linewidth',2,'color','b','markersize',6);
67 |     plot3(p_knee_g_l(1),p_knee_g_l(2),p_knee_g_l(3),'o','linewidth',2,'color','b','markersize',6);
68 | 
69 |     % ref foot
70 |     plot3(ref_fp_w_1(1),ref_fp_w_1(2),ref_fp_w_1(3),'o','linewidth',2,'color','g','markersize',4);
71 |     plot3(ref_fp_w_2(1),ref_fp_w_2(2),ref_fp_w_2(3),'o','linewidth',2,'color','g','markersize',4);
72 |     
73 |     % * 2 legs
74 |     for i=1:2
75 |         x_indx=3*(i-1)+1;
76 |         y_indx=3*(i-1)+2;
77 |         z_indx=3*(i-1)+3;
78 |         
79 |          plot3([foot_pos(x_indx),foot_pos(x_indx)+0.01*feetforce_used(x_indx)],...
80 |             [foot_pos(y_indx),foot_pos(y_indx)+0.01*feetforce_used(y_indx)],...
81 |             [foot_pos(z_indx),foot_pos(z_indx)+0.01*feetforce_used(z_indx)],'linewidth',1.1,'color','r');
82 |         
83 |     end
84 |     
85 |     pause(T/N);
86 |     hold on;
87 |     
88 |       frame = getframe(gcf);
89 |       writeVideo(v,frame);
90 |     
91 | end
92 | 
93 |  close(v);
94 | 
95 | end
96 | 
97 | 


--------------------------------------------------------------------------------