derive_FK_Jacobian.m

syms q1 q2 q3 real
init_quad_whole_body_ctrl;
% sym modern robotics fk 

thetalist =[q1; q2; q3];

fk_space = simplify(FKinSpace_sym(quad_param.M3, quad_param.Slist_link2, thetalist));
Js_space = JacobianSpace_sym(quad_param.Slist_link2, thetalist);

matlabFunction(fk_space,...
    'file','autoGen_fk_space.m',...
    'vars',{q1,q2,q3},...
    'outputs',{'fk_space'});
matlabFunction(Js_space,...
    'file','autoGen_J_space.m',...
    'vars',{q1,q2,q3},...
    'outputs',{'Js'});

fk_body = simplify(FKinBody_sym(quad_param.M3, quad_param.Blist_link2, thetalist));
Js_body = JacobianBody_sym(quad_param.Blist_link2, thetalist);

matlabFunction(fk_body,...
    'file','autoGen_fk_body.m',...
    'vars',{q1,q2,q3},...
    'outputs',{'fk_body'});
matlabFunction(Js_body,...
    'file','autoGen_J_body.m',...
    'vars',{q1,q2,q3},...
    'outputs',{'Jb'});