├── startup.m ├── Common ├── Sym.m ├── wsquare.m ├── RotZ.m ├── spatial_v2 │ ├── slprj │ │ └── sim │ │ │ └── varcache │ │ │ └── example6 │ │ │ ├── varInfo.mat │ │ │ ├── checksumOfCache.mat │ │ │ └── tmwinternal │ │ │ └── simulink_cache.xml │ ├── 3D │ │ ├── rx.m │ │ ├── ry.m │ │ ├── rz.m │ │ ├── skew.m │ │ ├── rqd.m │ │ ├── rq.m │ │ └── rv.m │ ├── spatial │ │ ├── crf.m │ │ ├── Fpt.m │ │ ├── rotx.m │ │ ├── roty.m │ │ ├── xlt.m │ │ ├── rotz.m │ │ ├── Xpt.m │ │ ├── Vpt.m │ │ ├── crm.m │ │ ├── plux.m │ │ ├── plnr.m │ │ ├── pluho.m │ │ ├── XtoV.m │ │ └── mcI.m │ ├── sparsity │ │ ├── mpyLi.m │ │ ├── mpyL.m │ │ ├── mpyLit.m │ │ ├── mpyLt.m │ │ ├── expandLambda.m │ │ ├── mpyH.m │ │ ├── LTDL.m │ │ └── LTL.m │ ├── models │ │ ├── cartpole.m~ │ │ ├── cartpole.m │ │ ├── planar.m │ │ ├── singlebody.m │ │ ├── scissor.m │ │ ├── rollingdisc.m │ │ ├── floatbase.m │ │ ├── autoTree.m │ │ ├── doubleElbow.m │ │ ├── vee.m │ │ └── gfxExample.m │ ├── dynamics │ │ ├── FDcrb.m │ │ ├── get_gravity.m │ │ ├── ID.m │ │ ├── EnerMo.m │ │ ├── jcalc.m │ │ ├── FDab.m │ │ ├── apply_external_forces.m │ │ ├── HandC.m │ │ ├── HD.m │ │ ├── IDfb.m │ │ ├── fbanim.m │ │ ├── FDfb.m │ │ ├── FDgq.m │ │ └── fbkin.m │ └── startup.m ├── RotX.m ├── RotY.m ├── Trans.m ├── pointToCloud.m ├── MatrixPartial.m ├── getMatVecProdPar.m ├── getRectangle.m └── Gait.m ├── HSDDP ├── default_resetmap.m ├── default_resetmap_partial.m ├── impact_aware_step.m ├── AL_params_struct.m ├── ReB_params_struct.m ├── ConstraintAbstract.m ├── TCostPartialData.m ├── TConstraintData.m ├── TrajReference.m ├── PathConstraintData.m ├── RCostPartialData.m ├── ReducedBarrier.m ├── update_tcost_with_tconstraint.m ├── update_rcost_with_pconstraint.m ├── ConstraintContainer.m ├── Trajectory.m ├── Phase.m └── HSDDP.m ├── Utils └── Graphics │ ├── drawGround.m │ ├── drawBody2D.m │ ├── drawLink2D.m │ └── QuadGraphics.m ├── Examples ├── BouncingBall │ ├── Data │ │ └── Nathan │ │ │ ├── direct_collocation_trajectory_1_bounce.mat │ │ │ └── saltation_matrix_trajectory_1_bounce.mat │ ├── Figure │ │ ├── plot_constraint_violation.m │ │ └── compare_hsddp2saltation.m │ ├── initializeBouncingTraj.m │ ├── Constraints │ │ └── BouncingConstraint.m │ ├── createBouncingBallTask.m │ ├── Cost │ │ └── BouncingCost.m │ ├── Dynamics │ │ └── BouncingBall.m │ └── BouncingBallOptimization.m └── Quadurped │ ├── Controllers │ └── HeuristicPD │ │ ├── getSpringVec.m │ │ └── heuristic_bounding_controller.m │ ├── Dynamics │ ├── BaseDyn.m │ ├── FB │ │ ├── Support │ │ │ ├── FBDynamics.m │ │ │ ├── FBDynamics_par.m │ │ │ └── FBDynamimcs_support.m │ │ ├── FootholdPlanner.m │ │ └── PlanarFloatingBase.m │ ├── WB │ │ ├── getFDPar.m │ │ ├── Support │ │ │ └── WBDynamics_support.m │ │ └── build2DminiCheetah.m │ ├── get2DMCParams.m │ └── get3DMCParams.m │ ├── Kinematics │ ├── Link1Jacobian.m │ ├── Link4Jacobian.m │ ├── Link2Jacobian.m │ ├── Link5Jacobian.m │ ├── Link3Jacobian.m │ ├── Link1Jacobian_par.m │ ├── Link4Jacobian_par.m │ ├── Link2Jacobian_par.m │ ├── Link3Jacobian_par.m │ └── Link5Jacobian_par.m │ └── Bounding │ ├── Constraint │ ├── SwitchingConstraint.m │ ├── torque_limit.m │ ├── support │ │ ├── WB_terminal_constr_support.m │ │ ├── Back_TouchDown_Constraint.m │ │ └── Front_TouchDown_Constraint.m │ ├── joint_limit.m │ └── GRF_constraint.m │ ├── BoundingModeLib.m │ ├── BoundingReference.m │ ├── Cost │ ├── FBCost.m │ └── WBCost.m │ ├── BoundingOptimization.m │ └── createBoundingTask.m └── LICENSE /startup.m: -------------------------------------------------------------------------------- 1 | addpath(genpath ('./')); -------------------------------------------------------------------------------- /Common/Sym.m: -------------------------------------------------------------------------------- 1 | function H_sym = Sym(H) 2 | H_sym = (H+H')/2; 3 | end -------------------------------------------------------------------------------- /Common/wsquare.m: -------------------------------------------------------------------------------- 1 | function s = wsquare(x, W) 2 | x = x(:); 3 | s = x'*W*x; 4 | end -------------------------------------------------------------------------------- /HSDDP/default_resetmap.m: -------------------------------------------------------------------------------- 1 | function xnext = default_resetmap(x) 2 | xnext = x; 3 | end -------------------------------------------------------------------------------- /HSDDP/default_resetmap_partial.m: -------------------------------------------------------------------------------- 1 | function Px = default_resetmap_partial(x) 2 | Px = eye(length(x)); 3 | end -------------------------------------------------------------------------------- /Utils/Graphics/drawGround.m: -------------------------------------------------------------------------------- 1 | function o = drawGround(h) 2 | p = plot([-1 4],h*ones(1,2),'k-','linewidth',1.5); 3 | o.p = p; 4 | end -------------------------------------------------------------------------------- /Common/RotZ.m: -------------------------------------------------------------------------------- 1 | function RotZ 2 | T = [cos(t) -sin(t) 0 0; 3 | sin(t) cos(t) 0 0; 4 | 0 0 1 0; 5 | 0 0 0 1]; 6 | end -------------------------------------------------------------------------------- /HSDDP/impact_aware_step.m: -------------------------------------------------------------------------------- 1 | function [G_new,H_new] = impact_aware_step(Px, G_prime,H_prime) 2 | G_new = Px'*G_prime; 3 | H_new = Px'*H_prime*Px; 4 | end -------------------------------------------------------------------------------- /Common/spatial_v2/slprj/sim/varcache/example6/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ROAM-Lab-ND/HS-DDP-MATLAB/HEAD/Common/spatial_v2/slprj/sim/varcache/example6/varInfo.mat -------------------------------------------------------------------------------- /Common/RotX.m: -------------------------------------------------------------------------------- 1 | function T = RotX(t) 2 | T = [1 0 0 0; 3 | 0 cos(t) -sin(t) 0; 4 | 0 sin(t) cost(t) 0; 5 | 0 0 0 1]; 6 | end -------------------------------------------------------------------------------- /Common/RotY.m: -------------------------------------------------------------------------------- 1 | function T = RotY(t) 2 | 3 | T=[cos(t) 0 sin(t) 0; 4 | 0 1 0 0; 5 | -sin(t) 0 cos(t) 0; 6 | 0 0 0 1]; 7 | end -------------------------------------------------------------------------------- /HSDDP/AL_params_struct.m: -------------------------------------------------------------------------------- 1 | classdef AL_params_struct 2 | properties 3 | sigma % penalty coefficient 4 | lambda % Lagrange multiplier 5 | end 6 | end -------------------------------------------------------------------------------- /Common/spatial_v2/slprj/sim/varcache/example6/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ROAM-Lab-ND/HS-DDP-MATLAB/HEAD/Common/spatial_v2/slprj/sim/varcache/example6/checksumOfCache.mat -------------------------------------------------------------------------------- /HSDDP/ReB_params_struct.m: -------------------------------------------------------------------------------- 1 | classdef ReB_params_struct 2 | properties 3 | delta % ReB relaxation param 4 | eps % ReB weighting param 5 | delta_min 6 | end 7 | end -------------------------------------------------------------------------------- /HSDDP/ConstraintAbstract.m: -------------------------------------------------------------------------------- 1 | classdef ConstraintAbstract < handle 2 | methods(Abstract) 3 | p_array = compute(Obj,x,u,y) 4 | params = get_params(Obj) 5 | end 6 | end -------------------------------------------------------------------------------- /Examples/BouncingBall/Data/Nathan/direct_collocation_trajectory_1_bounce.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ROAM-Lab-ND/HS-DDP-MATLAB/HEAD/Examples/BouncingBall/Data/Nathan/direct_collocation_trajectory_1_bounce.mat -------------------------------------------------------------------------------- /Examples/BouncingBall/Data/Nathan/saltation_matrix_trajectory_1_bounce.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ROAM-Lab-ND/HS-DDP-MATLAB/HEAD/Examples/BouncingBall/Data/Nathan/saltation_matrix_trajectory_1_bounce.mat -------------------------------------------------------------------------------- /Common/Trans.m: -------------------------------------------------------------------------------- 1 | function T=Trans(v) 2 | 3 | T = [1 0 0 v(1); 4 | 0 1 0 v(2); 5 | 0 0 1 v(3); 6 | 0 0 0 1]; 7 | end -------------------------------------------------------------------------------- /Examples/BouncingBall/Figure/plot_constraint_violation.m: -------------------------------------------------------------------------------- 1 | function plot_constraint_violation(vlts) 2 | figure 3 | plot(0:length(vlts)-1, vlts); 4 | xlabel('Outer-loop iteration'); 5 | ylabel('Maximum violation (m)'); 6 | end -------------------------------------------------------------------------------- /Common/pointToCloud.m: -------------------------------------------------------------------------------- 1 | function [p2cloud_dist,I] = pointToCloud(p, cloud) 2 | dist = zeros(1, size(cloud,2)); 3 | for i = 1:size(cloud,2) 4 | dist(i) = norm(p - cloud(:,i)); 5 | end 6 | [p2cloud_dist,I] = min(dist); 7 | end -------------------------------------------------------------------------------- /Common/MatrixPartial.m: -------------------------------------------------------------------------------- 1 | function K_x = MatrixPartial(K, x) 2 | dim_K = size(K); 3 | 4 | K_x = sym(zeros(dim_K(1),dim_K(2),length(x))); 5 | for i = 1:dim_K(1) 6 | for j = 1:dim_K(2) 7 | K_x(i,j,:) = jacobian(K(i,j),x); 8 | end 9 | end 10 | end 11 | 12 | -------------------------------------------------------------------------------- /HSDDP/TCostPartialData.m: -------------------------------------------------------------------------------- 1 | classdef TCostPartialData 2 | properties 3 | G 4 | H 5 | end 6 | methods 7 | function phi_par = TCostPartialData(xsize) 8 | phi_par.G = zeros(xsize, 1); 9 | phi_par.H = zeros(xsize, xsize); 10 | end 11 | end 12 | end -------------------------------------------------------------------------------- /Common/spatial_v2/slprj/sim/varcache/example6/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | O8utv2d5hGD/4nC8gKGGiA== 5 | 6 | -------------------------------------------------------------------------------- /Utils/Graphics/drawBody2D.m: -------------------------------------------------------------------------------- 1 | function o = drawBody2D(l, h, c) 2 | v = [-l/2, -l/2, l/2, l/2; 3 | h/2, -h/2, -h/2, h/2]'; 4 | p = patch(v(:,1), v(:,2), c); 5 | set(p, 'FaceLighting','flat'); % Set the renderer 6 | set(p, 'EdgeColor',[.1 .1 .1]); % Don't show edges 7 | set(p,'AmbientStrength',.6); 8 | o.v = v; 9 | o.p = p; 10 | end -------------------------------------------------------------------------------- /Utils/Graphics/drawLink2D.m: -------------------------------------------------------------------------------- 1 | function o = drawLink2D(w, l, c) 2 | v = [-w/2, -w/2, w/2, w/2; 3 | 0, -l, -l, 0]'; 4 | p = patch(v(:,1), v(:,2), c); 5 | set(p, 'FaceLighting','flat'); % Set the renderer 6 | set(p, 'EdgeColor',[.1 .1 .1]); % Don't show edges 7 | set(p,'AmbientStrength',.6); 8 | o.v = v; 9 | o.p = p; 10 | end -------------------------------------------------------------------------------- /Examples/BouncingBall/initializeBouncingTraj.m: -------------------------------------------------------------------------------- 1 | function initializeBouncingTraj(hybridT) 2 | folderName = 'BouncingBall/Data/Nathan/'; 3 | fileName = 'saltation_matrix_trajectory_1_bounce.mat'; 4 | u_struct = load(strcat(folderName, fileName),'u_trj'); 5 | u = u_struct.u_trj; 6 | hybridT(1).Ubar = num2cell(u(1:543)); 7 | hybirdT(2).Ubar = num2cell(u(544:end)); 8 | end -------------------------------------------------------------------------------- /Common/getMatVecProdPar.m: -------------------------------------------------------------------------------- 1 | function Abx = getMatVecProdPar(A,Ax,b,bx) 2 | % This function computes partial of A(x)b(x) w.r.t. x 3 | if isempty(A) || isempty(Ax) || isempty(b) || isempty(bx) 4 | Abx = []; 5 | return; 6 | end 7 | Abx = zeros(size(A,1), size(Ax, 3)); 8 | for i = 1:size(Ax, 3) 9 | Abx(:,i) = Ax(:,:,i)*b; 10 | end 11 | Abx = Abx + A*bx; -------------------------------------------------------------------------------- /HSDDP/TConstraintData.m: -------------------------------------------------------------------------------- 1 | classdef TConstraintData 2 | properties 3 | h = 0 4 | hx = [] 5 | hxx = [] 6 | end 7 | methods 8 | function T = TConstraintData(xs) 9 | T.h = 0; 10 | T.hx = zeros(xs, 1); 11 | T.hxx = zeros(xs, xs); 12 | end 13 | end 14 | end -------------------------------------------------------------------------------- /Common/spatial_v2/3D/rx.m: -------------------------------------------------------------------------------- 1 | function E = rx( theta ) 2 | 3 | % rx 3x3 coordinate rotation (X-axis) 4 | % rx(theta) calculates the 3x3 rotational coordinate transform matrix from 5 | % A to B coordinates, where coordinate frame B is rotated by an angle theta 6 | % (radians) relative to frame A about their common X axis. 7 | 8 | c = cos(theta); 9 | s = sin(theta); 10 | 11 | E = [ 1 0 0; 12 | 0 c s; 13 | 0 -s c ]; 14 | -------------------------------------------------------------------------------- /Common/spatial_v2/3D/ry.m: -------------------------------------------------------------------------------- 1 | function E = ry( theta ) 2 | 3 | % ry 3x3 coordinate rotation (Y-axis) 4 | % ry(theta) calculates the 3x3 rotational coordinate transform matrix from 5 | % A to B coordinates, where coordinate frame B is rotated by an angle theta 6 | % (radians) relative to frame A about their common Y axis. 7 | 8 | c = cos(theta); 9 | s = sin(theta); 10 | 11 | E = [ c 0 -s; 12 | 0 1 0; 13 | s 0 c ]; 14 | -------------------------------------------------------------------------------- /Common/spatial_v2/3D/rz.m: -------------------------------------------------------------------------------- 1 | function E = rz( theta ) 2 | 3 | % rz 3x3 coordinate rotation (Z-axis) 4 | % rz(theta) calculates the 3x3 rotational coordinate transform matrix from 5 | % A to B coordinates, where coordinate frame B is rotated by an angle theta 6 | % (radians) relative to frame A about their common Z axis. 7 | 8 | c = cos(theta); 9 | s = sin(theta); 10 | 11 | E = [ c s 0; 12 | -s c 0; 13 | 0 0 1 ]; 14 | -------------------------------------------------------------------------------- /HSDDP/TrajReference.m: -------------------------------------------------------------------------------- 1 | classdef TrajReference < handle 2 | properties 3 | xd 4 | ud 5 | yd 6 | len 7 | end 8 | methods 9 | function R = TrajReference(xs, us, ys, len_horizon) 10 | R.xd = repmat({zeros(xs,1)}, 1, len_horizon); 11 | R.ud = repmat({zeros(us,1)}, 1, len_horizon-1); 12 | R.yd = repmat({zeros(ys,1)}, 1, len_horizon-1); 13 | R.len = len_horizon; 14 | end 15 | end 16 | end -------------------------------------------------------------------------------- /Common/spatial_v2/spatial/crf.m: -------------------------------------------------------------------------------- 1 | function vcross = crf( v ) 2 | 3 | % crf spatial/planar cross-product operator (force). 4 | % crf(v) calculates the 6x6 (or 3x3) matrix such that the expression 5 | % crf(v)*f is the cross product of the motion vector v with the force 6 | % vector f. If length(v)==6 then it is taken to be a spatial vector, and 7 | % the return value is a 6x6 matrix. Otherwise, v is taken to be a planar 8 | % vector, and the return value is 3x3. 9 | 10 | vcross = -crm(v)'; 11 | -------------------------------------------------------------------------------- /Examples/Quadurped/Controllers/HeuristicPD/getSpringVec.m: -------------------------------------------------------------------------------- 1 | function v = getSpringVec(q, model, legs) 2 | v = []; 3 | for i = 1:length(legs) 4 | if legs(i) == 1 % front leg 5 | foot = model.getFootPosition(q, 3); 6 | hip = model.getPosition(q,1,model.hipLoc{1}); 7 | elseif legs(i) == 2 8 | foot = model.getFootPosition(q, 1); 9 | hip = model.getPosition(q,1,model.hipLoc{2}); 10 | end 11 | v = [v, foot - hip]; 12 | end 13 | end -------------------------------------------------------------------------------- /Common/spatial_v2/sparsity/mpyLi.m: -------------------------------------------------------------------------------- 1 | function y = mpyLi( L, lambda, x ) 2 | 3 | % mpyLi multiply vector by inverse of L factor from LTL or LTDL 4 | % mpyLi(L,lambda,x) computes inv(L)*x where L is the lower-triangular 5 | % matrix from either LTL or LTDL and lambda is the parent array describing 6 | % the sparsity pattern in L. 7 | 8 | n = size(L,1); 9 | 10 | for i = 1:n 11 | j = lambda(i); 12 | while j ~= 0 13 | x(i) = x(i) - L(i,j) * x(j); 14 | j = lambda(j); 15 | end 16 | x(i) = x(i) / L(i,i); 17 | end 18 | 19 | y = x; 20 | -------------------------------------------------------------------------------- /Common/spatial_v2/sparsity/mpyL.m: -------------------------------------------------------------------------------- 1 | function y = mpyL( L, lambda, x ) 2 | 3 | % mpyL multiply vector by L factor from LTL or LTDL 4 | % mpyL(L,lambda,x) computes L*x where L is the lower-triangular matrix from 5 | % either LTL or LTDL and lambda is the parent array describing the sparsity 6 | % pattern in L. 7 | 8 | n = size(L,1); 9 | y = x; % to give y same dimensions as x 10 | 11 | for i = 1:n 12 | y(i) = L(i,i) * x(i); 13 | j = lambda(i); 14 | while j ~= 0 15 | y(i) = y(i) + L(i,j) * x(j); 16 | j = lambda(j); 17 | end 18 | end 19 | -------------------------------------------------------------------------------- /Common/spatial_v2/sparsity/mpyLit.m: -------------------------------------------------------------------------------- 1 | function y = mpyLit( L, lambda, x ) 2 | 3 | % mpyLit multiply vector by inverse transpose of L factor from LTL or LTDL 4 | % mpyLit(L,lambda,x) computes inv(L')*x where L is the lower-triangular 5 | % matrix from either LTL or LTDL and lambda is the parent array describing 6 | % the sparsity pattern in L. 7 | 8 | n = size(L,1); 9 | 10 | for i = n:-1:1 11 | x(i) = x(i) / L(i,i); 12 | j = lambda(i); 13 | while j ~= 0 14 | x(j) = x(j) - L(i,j) * x(i); 15 | j = lambda(j); 16 | end 17 | end 18 | 19 | y = x; 20 | -------------------------------------------------------------------------------- /Common/spatial_v2/sparsity/mpyLt.m: -------------------------------------------------------------------------------- 1 | function y = mpyLt( L, lambda, x ) 2 | 3 | % mpyLt multiply vector by transpose of L factor from LTL or LTDL 4 | % mpyLt(L,lambda,x) computes L'*x where L is the lower-triangular matrix 5 | % from either LTL or LTDL and lambda is the parent array describing the 6 | % sparsity pattern in L. 7 | 8 | n = size(L,1); 9 | y = x; % to give y same dimensions as x 10 | 11 | for i = 1:n 12 | y(i) = L(i,i) * x(i); 13 | j = lambda(i); 14 | while j ~= 0 15 | y(j) = y(j) + L(i,j) * x(i); 16 | j = lambda(j); 17 | end 18 | end 19 | -------------------------------------------------------------------------------- /HSDDP/PathConstraintData.m: -------------------------------------------------------------------------------- 1 | classdef PathConstraintData 2 | properties 3 | c = 0 4 | cx = [] 5 | cu = [] 6 | cy = [] 7 | cxx = [] 8 | cuu = [] 9 | cyy = [] 10 | end 11 | methods 12 | function C = PathConstraintData(xs,us,ys) 13 | C.c = 0; 14 | C.cx = zeros(xs, 1); 15 | C.cu = zeros(us, 1); 16 | C.cy = zeros(ys, 1); 17 | C.cxx = zeros(xs, xs); 18 | C.cuu = zeros(us, us); 19 | C.cyy = zeros(ys, ys); 20 | end 21 | end 22 | end -------------------------------------------------------------------------------- /Common/spatial_v2/spatial/Fpt.m: -------------------------------------------------------------------------------- 1 | function f = Fpt( fp, p ) 2 | 3 | % Fpt forces at points --> spatial/planar forces 4 | % f=Fpt(fp,p) converts one or more linear forces fp acting at one or more 5 | % points p to their equivalent spatial or planar forces. In 3D, fp and p 6 | % are 3xn arrays and f is 6xn. In 2D, fp and p are 2xn and f is 3xn. In 7 | % both cases, f(:,i) is the equivalent of fp(:,i) acting at p(:,i). 8 | 9 | if size(fp,1)==3 % 3D forces at 3D points 10 | f = [ cross(p,fp,1); fp ]; 11 | else % 2D forces at 2D points 12 | f = [ p(1,:).*fp(2,:) - p(2,:).*fp(1,:); fp ]; 13 | end 14 | -------------------------------------------------------------------------------- /Common/spatial_v2/spatial/rotx.m: -------------------------------------------------------------------------------- 1 | function X = rotx( theta ) 2 | 3 | % rotx spatial coordinate transform (X-axis rotation). 4 | % rotx(theta) calculates the coordinate transform matrix from A to B 5 | % coordinates for spatial motion vectors, where coordinate frame B is 6 | % rotated by an angle theta (radians) relative to frame A about their 7 | % common X axis. 8 | 9 | c = cos(theta); 10 | s = sin(theta); 11 | 12 | X = [ 1 0 0 0 0 0 ; 13 | 0 c s 0 0 0 ; 14 | 0 -s c 0 0 0 ; 15 | 0 0 0 1 0 0 ; 16 | 0 0 0 0 c s ; 17 | 0 0 0 0 -s c 18 | ]; 19 | -------------------------------------------------------------------------------- /Common/spatial_v2/spatial/roty.m: -------------------------------------------------------------------------------- 1 | function X = roty( theta ) 2 | 3 | % roty spatial coordinate transform (Y-axis rotation). 4 | % roty(theta) calculates the coordinate transform matrix from A to B 5 | % coordinates for spatial motion vectors, where coordinate frame B is 6 | % rotated by an angle theta (radians) relative to frame A about their 7 | % common Y axis. 8 | 9 | c = cos(theta); 10 | s = sin(theta); 11 | 12 | X = [ c 0 -s 0 0 0 ; 13 | 0 1 0 0 0 0 ; 14 | s 0 c 0 0 0 ; 15 | 0 0 0 c 0 -s ; 16 | 0 0 0 0 1 0 ; 17 | 0 0 0 s 0 c 18 | ]; 19 | -------------------------------------------------------------------------------- /Common/spatial_v2/spatial/xlt.m: -------------------------------------------------------------------------------- 1 | function X = xlt( r ) 2 | 3 | % xlt spatial coordinate transform (translation of origin). 4 | % xlt(r) calculates the coordinate transform matrix from A to B 5 | % coordinates for spatial motion vectors, in which frame B is translated by 6 | % an amount r (3D vector) relative to frame A. r can be a row or column 7 | % vector. 8 | 9 | X = [ 1 0 0 0 0 0 ; 10 | 0 1 0 0 0 0 ; 11 | 0 0 1 0 0 0 ; 12 | 0 r(3) -r(2) 1 0 0 ; 13 | -r(3) 0 r(1) 0 1 0 ; 14 | r(2) -r(1) 0 0 0 1 15 | ]; 16 | -------------------------------------------------------------------------------- /Common/spatial_v2/spatial/rotz.m: -------------------------------------------------------------------------------- 1 | function X = rotz( theta ) 2 | 3 | % rotz spatial coordinate transform (Z-axis rotation). 4 | % rotz(theta) calculates the coordinate transform matrix from A to B 5 | % coordinates for spatial motion vectors, where coordinate frame B is 6 | % rotated by an angle theta (radians) relative to frame A about their 7 | % common Z axis. 8 | 9 | c = cos(theta); 10 | s = sin(theta); 11 | 12 | X = [ c s 0 0 0 0 ; 13 | -s c 0 0 0 0 ; 14 | 0 0 1 0 0 0 ; 15 | 0 0 0 c s 0 ; 16 | 0 0 0 -s c 0 ; 17 | 0 0 0 0 0 1 18 | ]; 19 | -------------------------------------------------------------------------------- /Common/spatial_v2/models/cartpole.m~: -------------------------------------------------------------------------------- 1 | function robot = cartpole( mass_cart, mass_pole, length_pole, grav ) 2 | 3 | robot.NB = 2; 4 | robot.parent = [0:1]; 5 | 6 | robot.jtype{1} = 'Px'; % The first joint is a prismatic in the +X0 direction 7 | robot.jtype{2} = 'Rz'; % The second joint is a revolute in the +Z1 direction 8 | 9 | robot.Xtree{1} = eye(1); % The transform from {0} to just before joint 1 is identity 10 | robot.Xtree{2} = eye(1); % The transfrom from {1} to just before joint 2 is identity 11 | 12 | robot.I{1} = mcI( mass_cart, [0 0 0], 0 ); 13 | robot.I{2} = mcI( mass_pole, [0 -length_pole 0], 0 ); 14 | 15 | robot 16 | -------------------------------------------------------------------------------- /HSDDP/RCostPartialData.m: -------------------------------------------------------------------------------- 1 | classdef RCostPartialData 2 | properties 3 | lx 4 | lu 5 | ly 6 | lxx 7 | lux 8 | luu 9 | lyy 10 | end 11 | methods 12 | function lpar = RCostPartialData(xsize, usize, ysize) 13 | lpar.lx = zeros(xsize, 1); 14 | lpar.lu = zeros(usize, 1); 15 | lpar.ly = zeros(ysize, 1); 16 | lpar.lxx = zeros(xsize, xsize); 17 | lpar.lux = zeros(usize, xsize); 18 | lpar.luu = zeros(usize, usize); 19 | lpar.lyy = zeros(ysize, ysize); 20 | end 21 | end 22 | end -------------------------------------------------------------------------------- /Common/spatial_v2/models/cartpole.m: -------------------------------------------------------------------------------- 1 | function robot = cartpole( mass_cart, mass_pole, length_pole, g ) 2 | 3 | robot.NB = 2; 4 | robot.parent = [0:1]; 5 | 6 | robot.jtype{1} = 'Px'; % The first joint is a prismatic in the +X0 direction 7 | robot.jtype{2} = 'Rz'; % The second joint is a revolute in the +Z1 direction 8 | 9 | robot.Xtree{1} = eye(1); % The transform from {0} to just before joint 1 is identity 10 | robot.Xtree{2} = eye(1); % The transfrom from {1} to just before joint 2 is identity 11 | 12 | robot.I{1} = mcI( mass_cart, [0 0 0], 0 ); 13 | robot.I{2} = mcI( mass_pole, [0 -length_pole 0], 0 ); 14 | 15 | robot.gravity = [0 -g 0]'; 16 | 17 | -------------------------------------------------------------------------------- /Common/spatial_v2/sparsity/expandLambda.m: -------------------------------------------------------------------------------- 1 | function newLambda = expandLambda( lambda, nf ) 2 | 3 | % expandLambda expand a parent array 4 | % expandLambda(lambda,nf) calculates the expanded parent array, for use in 5 | % sparse factorization algorithms, from a given parent array and an array 6 | % of joint motion freedoms. nf(i) is the degree of motion freedom allowed 7 | % by joint i. 8 | 9 | N = length(lambda); 10 | n = sum(nf); 11 | 12 | newLambda = 0:(n-1); 13 | 14 | map(1) = 0; % (matlab won't let us use map(0)) 15 | for i = 1:N 16 | map(i+1) = map(i) + nf(i); 17 | end 18 | 19 | for i = 1:N 20 | newLambda(map(i)+1) = map(lambda(i)+1); 21 | end 22 | -------------------------------------------------------------------------------- /Examples/Quadurped/Dynamics/BaseDyn.m: -------------------------------------------------------------------------------- 1 | classdef BaseDyn < handle 2 | properties (Abstract,SetAccess=private, GetAccess=public) 3 | dt 4 | qsize 5 | xsize 6 | usize 7 | ysize 8 | end 9 | 10 | methods 11 | % All member functions below implement nothing 12 | [x_next, y] = dynamics(Ph, x, u, mode, varargin) 13 | dynInfo = dynamics_par(Ph, x, u, mode,varargin) 14 | [x_next, y] = resetmap(Ph, x, mode,varargin) 15 | Px = resetmap_par(Ph, x, mode,varargin) 16 | Initialize_model(Ph, varargin) 17 | end 18 | end -------------------------------------------------------------------------------- /Common/getRectangle.m: -------------------------------------------------------------------------------- 1 | function Rectangle = getRectangle(leftTop, rightBottom, space) 2 | leftBottom = [leftTop(1), rightBottom(2)]'; 3 | rightTop = [rightBottom(1), leftTop(2)]'; 4 | 5 | [leftEdgeX,leftEdgeY] = meshgrid(leftTop(1), leftBottom(2):space:leftTop(2)); 6 | [topEdgeX,topEdgeY] = meshgrid(leftTop(1):space:rightTop(1), leftTop(2)); 7 | [rightEdgeX,rightEdgeY] = meshgrid(rightTop(1), rightBottom(2):space:rightTop(2)); 8 | [bottomEdgeX,bottomEdgeY] = meshgrid(leftBottom(1):space:rightBottom(1), leftBottom(2)); 9 | 10 | Rectangle = [leftEdgeX, leftEdgeY; 11 | topEdgeX', topEdgeY'; 12 | rightEdgeX, rightEdgeY; 13 | bottomEdgeX', bottomEdgeY']; 14 | end -------------------------------------------------------------------------------- /Examples/Quadurped/Kinematics/Link1Jacobian.m: -------------------------------------------------------------------------------- 1 | function [J,Jd] = Link1Jacobian(in1,in2) 2 | %LINK1JACOBIAN 3 | % [J,JD] = LINK1JACOBIAN(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 29-Nov-2020 17:42:37 7 | 8 | p1_1 = in2(1); 9 | p2_1 = in2(2); 10 | q3 = in1(3,:); 11 | qd3 = in1(10,:); 12 | t2 = conj(p1_1); 13 | t3 = cos(q3); 14 | t4 = conj(p2_1); 15 | t5 = sin(q3); 16 | t6 = t3.*t4; 17 | t7 = t6-t2.*t5; 18 | J = reshape([1.0,0.0,0.0,1.0,t7,-t2.*t3-t4.*t5,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2,7]); 19 | if nargout > 1 20 | Jd = reshape([0.0,0.0,0.0,0.0,-qd3.*(t2.*t3+t4.*t5),-qd3.*t7,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2,7]); 21 | end 22 | -------------------------------------------------------------------------------- /HSDDP/ReducedBarrier.m: -------------------------------------------------------------------------------- 1 | function varargout = ReducedBarrier(z, delta) 2 | % ReB Reduced barrier function penalizing z > = 0 3 | % delta relaxation parameter 4 | % Bz, Bzz Derivative of B w.r.t. z 5 | k = 2; % second order polynominal extension 6 | if z>delta 7 | B = -log(z); 8 | Bz = -z ^(-1); 9 | Bzz = z ^(-2); 10 | else 11 | B = (k-1)/k*(((z -k*delta )/((k-1)*delta ))^k - 1) - log(delta ); 12 | Bz = ((z -k*delta )/((k-1)*delta ))^(k-1)/delta ; 13 | Bzz = ((z -k*delta )/((k-1)*delta ))^(k-2); 14 | end 15 | if nargout >= 1 16 | varargout{1} = B; 17 | end 18 | if nargout >= 2 19 | varargout{2} = Bz; 20 | end 21 | if nargout == 3 22 | varargout{3} = Bzz; 23 | end 24 | end 25 | -------------------------------------------------------------------------------- /Common/spatial_v2/spatial/Xpt.m: -------------------------------------------------------------------------------- 1 | function xp = Xpt( X, p ) 2 | 3 | % Xpt apply Plucker/planar coordinate transform to 2D/3D points 4 | % xp=Xpt(X,p) applies the coordinate transform X to the points in p, 5 | % returning the new coordinates in xp. If X is a 6x6 matrix then it is 6 | % taken to be a Plucker coordinate transform, and p is expected to be a 7 | % 3xn matrix of 3D points. Otherwise, X is assumed to be a planar 8 | % coordinate transform and p a 2xn array of 2D points. 9 | 10 | if all(size(X)==[6 6]) % 3D points 11 | E = X(1:3,1:3); 12 | r = -skew(E'*X(4:6,1:3)); 13 | else % 2D points 14 | E = X(2:3,2:3); 15 | r = [ X(2,3)*X(2,1)+X(3,3)*X(3,1); X(2,3)*X(3,1)-X(3,3)*X(2,1) ]; 16 | end 17 | 18 | if size(p,2) > 1 19 | r = repmat(r,1,size(p,2)); 20 | end 21 | 22 | xp = E * (p - r); 23 | -------------------------------------------------------------------------------- /Common/spatial_v2/spatial/Vpt.m: -------------------------------------------------------------------------------- 1 | function vp = Vpt( v, p ) 2 | 3 | % Vpt spatial/planar velocities --> velocities at points 4 | % vp=Vpt(v,p) calculates the linear velocities vp at one or more points p 5 | % due to one or more spatial/planar velocities v. In 3D, v is either 6x1 6 | % or 6xn, and p and vp are 3xn. In 2D, v is either 3x1 or 3xn, and p and 7 | % vp are 2xn. If v is just a single spatial/planar vector then it applies 8 | % to every point in p; otherwise, vp(:,i) is calculated from v(:,i) and 9 | % p(:,i). 10 | 11 | if size(v,2)==1 && size(p,2)>1 12 | v = repmat(v,1,size(p,2)); 13 | end 14 | 15 | if size(v,1)==6 % 3D points and velocities 16 | vp = v(4:6,:) + cross(v(1:3,:),p,1); 17 | else % 2D points and velocities 18 | vp = v(2:3,:) + [ -v(1,:).*p(2,:); v(1,:).*p(1,:) ]; 19 | end 20 | -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/FDcrb.m: -------------------------------------------------------------------------------- 1 | function qdd = FDcrb( model, q, qd, tau, f_ext ) 2 | 3 | % FDcrb Forward Dynamics via Composite-Rigid-Body Algorithm 4 | % FDcrb(model,q,qd,tau,f_ext) calculates the forward dynamics of a 5 | % kinematic tree via the composite-rigid-body algorithm. q, qd and tau are 6 | % vectors of joint position, velocity and force variables; and the return 7 | % value is a vector of joint acceleration variables. f_ext is an optional 8 | % argument specifying the external forces acting on the bodies. It can be 9 | % omitted if there are no external forces. The format of f_ext is 10 | % explained in the source code of apply_external_forces. 11 | 12 | if nargin == 4 13 | [H,C] = HandC( model, q, qd ); 14 | else 15 | [H,C] = HandC( model, q, qd, f_ext ); 16 | end 17 | 18 | qdd = H \ (tau - C); 19 | -------------------------------------------------------------------------------- /HSDDP/update_tcost_with_tconstraint.m: -------------------------------------------------------------------------------- 1 | function [phi, phi_par] = update_tcost_with_tconstraint(phi, phi_par, tConstr, params) 2 | % tConstr: 1xn structure array of equality constraints at one time instant 3 | % params: 1xn structure array of augmented lagrangian parameters 4 | 5 | n = length(tConstr); 6 | assert(n==length(params), 'Terminal constraint and AL params have different size'); 7 | for i = 1:n 8 | sigma = params(i).sigma; 9 | lambda = params(i).lambda; 10 | h = tConstr(i).h; 11 | hx = tConstr(i).hx; 12 | hxx = tConstr(i).hxx; 13 | 14 | phi = phi + (sigma/2)^2 * (h)^2 + lambda*h; 15 | 16 | phi_par.G = phi_par.G + 2*(sigma/2)^2*h*hx + lambda*hx; 17 | phi_par.H = phi_par.H + ... 18 | 2*(sigma/2)^2*(h*hxx + hx*hx') + ... 19 | lambda*hxx; 20 | end 21 | end -------------------------------------------------------------------------------- /Examples/BouncingBall/Constraints/BouncingConstraint.m: -------------------------------------------------------------------------------- 1 | classdef BouncingConstraint < ConstraintAbstract 2 | properties 3 | mode 4 | end 5 | methods 6 | function S = BouncingConstraint(mode) 7 | S.mode = mode; 8 | end 9 | function t_array = compute(S, x) 10 | t_array = TConstraintData(length(x)); 11 | if S.mode == 1 12 | t_array.h = x(1); 13 | t_array.hx = [1 0]'; 14 | else 15 | t_array.h = x(1) - 0.3; 16 | t_array.hx = [1 0]'; 17 | end 18 | end 19 | function params = get_params(S) 20 | % Initialize Augmented Lagrangian parameters 21 | param.sigma = 0.5; 22 | param.lambda = 0; 23 | params = [param]; 24 | end 25 | end 26 | end -------------------------------------------------------------------------------- /Examples/Quadurped/Dynamics/FB/Support/FBDynamics.m: -------------------------------------------------------------------------------- 1 | function f = FBDynamics(in1,in2,in3,in4) 2 | %FBDYNAMICS 3 | % F = FBDYNAMICS(IN1,IN2,IN3,IN4) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 26-Dec-2020 23:13:41 7 | 8 | p1 = in3(1,:); 9 | p2 = in3(2,:); 10 | p3 = in3(3,:); 11 | p4 = in3(4,:); 12 | q1 = in1(1,:); 13 | q2 = in1(2,:); 14 | qd1 = in1(4,:); 15 | qd2 = in1(5,:); 16 | qd3 = in1(6,:); 17 | s1 = in4(1,:); 18 | s2 = in4(2,:); 19 | u1 = in2(1,:); 20 | u2 = in2(2,:); 21 | u3 = in2(3,:); 22 | u4 = in2(4,:); 23 | f = [qd1;qd2;qd3;s1.*u1.*1.211827435773146e-1+s2.*u3.*1.211827435773146e-1;s1.*u2.*1.211827435773146e-1+s2.*u4.*1.211827435773146e-1-9.81e2./1.0e2;-s1.*(u2.*(p1-q1).*4.307272227516377-u1.*(p2-q2).*4.307272227516377)-s2.*(u4.*(p3-q1).*4.307272227516377-u3.*(p4-q2).*4.307272227516377)]; 24 | -------------------------------------------------------------------------------- /Common/spatial_v2/3D/skew.m: -------------------------------------------------------------------------------- 1 | function out = skew( in ) 2 | 3 | % skew convert 3D vector <--> 3x3 skew-symmetric matrix 4 | % S=skew(v) and v=skew(A) calculate the 3x3 skew-symmetric matrix S 5 | % corresponding to the given 3D vector v, and the 3D vector corresponding 6 | % to the skew-symmetric component of the given arbitrary 3x3 matrix A. For 7 | % vectors a and b, skew(a)*b is the cross product of a and b. If the 8 | % argument is a 3x3 matrix then it is assumed to be A, otherwise it is 9 | % assumed to be v. skew(A) produces a column-vector result, but skew(v) 10 | % will accept a row or column vector argument. 11 | 12 | if all(size(in)==[3 3]) % do v = skew(A) 13 | out = 0.5 * [ in(3,2) - in(2,3); 14 | in(1,3) - in(3,1); 15 | in(2,1) - in(1,2) ]; 16 | else % do S = skew(v) 17 | out = [ 0, -in(3), in(2); 18 | in(3), 0, -in(1); 19 | -in(2), in(1), 0 ]; 20 | end 21 | -------------------------------------------------------------------------------- /Examples/Quadurped/Dynamics/FB/FootholdPlanner.m: -------------------------------------------------------------------------------- 1 | classdef FootholdPlanner < handle 2 | properties 3 | model 4 | params 5 | end 6 | methods 7 | function PL = FootholdPlanner(model) 8 | PL.model = model; 9 | mc3D_param = get3DMCParams(); 10 | PL.params = get2DMCParams(mc3D_param); 11 | end 12 | 13 | function foothold_prediction(PL, x, stanceTime, vd) 14 | xCoM = x(1); 15 | x_offset = vd * stanceTime/2; 16 | pfoot = zeros(4,1); 17 | pfoot(1) = xCoM + PL.params.hipLoc{1}(1) + x_offset; 18 | pfoot(2) = -0.404; 19 | pfoot(3) = xCoM + PL.params.hipLoc{2}(1) + x_offset; 20 | pfoot(4) = -0.404; 21 | PL.model.set_foothold(pfoot); 22 | end 23 | 24 | end 25 | end -------------------------------------------------------------------------------- /Common/spatial_v2/spatial/crm.m: -------------------------------------------------------------------------------- 1 | function vcross = crm( v ) 2 | 3 | % crm spatial/planar cross-product operator (motion). 4 | % crm(v) calculates the 6x6 (or 3x3) matrix such that the expression 5 | % crm(v)*m is the cross product of the motion vectors v and m. If 6 | % length(v)==6 then it is taken to be a spatial vector, and the return 7 | % value is a 6x6 matrix. Otherwise, v is taken to be a planar vector, and 8 | % the return value is 3x3. 9 | 10 | if length(v) == 6 11 | 12 | vcross = [ 0 -v(3) v(2) 0 0 0 ; 13 | v(3) 0 -v(1) 0 0 0 ; 14 | -v(2) v(1) 0 0 0 0 ; 15 | 0 -v(6) v(5) 0 -v(3) v(2) ; 16 | v(6) 0 -v(4) v(3) 0 -v(1) ; 17 | -v(5) v(4) 0 -v(2) v(1) 0 ]; 18 | 19 | else 20 | 21 | vcross = [ 0 0 0 ; 22 | v(3) 0 -v(1) ; 23 | -v(2) v(1) 0 ]; 24 | end 25 | -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/get_gravity.m: -------------------------------------------------------------------------------- 1 | function a_grav = get_gravity( model ) 2 | 3 | % get_gravity spatial/planar gravitational accn vector for given model 4 | % get_gravity(model) returns the gravitational acceleration vector to be 5 | % used in dynamics calculations for the given model. The return value is 6 | % either a spatial or a planar vector, according to the type of model. It 7 | % is computed from the field model.gravity, which is a 2D or 3D (row or 8 | % column) vector specifying the linear acceleration due to gravity. If 9 | % this field is not present then get_gravity uses the following defaults: 10 | % [0,0,-9.81] for spatial models and [0,0] for planar. 11 | 12 | if isfield( model, 'gravity' ) 13 | g = model.gravity; 14 | else 15 | g = [0;0;-9.81]; 16 | end 17 | 18 | if size(model.Xtree{1},1) == 3 % is model planar? 19 | a_grav = [0;g(1);g(2)]; 20 | else 21 | a_grav = [0;0;0;g(1);g(2);g(3)]; 22 | end 23 | -------------------------------------------------------------------------------- /Common/spatial_v2/sparsity/mpyH.m: -------------------------------------------------------------------------------- 1 | function y = mpyH( H, lambda, x ) 2 | 3 | % mpyH calculate H*x exploiting branch-induced sparsity in H 4 | % mpyH(H,lambda,x) computes H*x where x is a vector and H is a symmetric, 5 | % positive-definite matrix having the property that the nonzero elements on 6 | % row i below the main diagonal appear only in columns lambda(i), 7 | % lambda(lambda(i)), and so on. This is the pattern of branch-induced 8 | % sparsity; and H and lambda can be regarded as the joint-space inertia 9 | % matrix and parent array of a kinematic tree. lambda must satisfy 10 | % 0<=lambda(i) X 15 | 16 | o1 = [ i1, zeros(3); -i1*skew(i2), i1 ]; 17 | 18 | else % X --> E,r 19 | 20 | o1 = i1(1:3,1:3); 21 | o2 = -skew(o1'*i1(4:6,1:3)); 22 | 23 | end 24 | -------------------------------------------------------------------------------- /Examples/Quadurped/Dynamics/WB/getFDPar.m: -------------------------------------------------------------------------------- 1 | function varargout = getFDPar(K,Kx,b,bx,bu) 2 | % This function returns [FDx, FDu, gx, gu] for smooth dynamics and [FDx, 3 | % gx] for reset map which does NOT depend on u 4 | fKKT_x = zeros(size(bx)); 5 | for i = 1:size(Kx,3) 6 | Kinvxi = -K\Kx(:,:,i)/K; 7 | fKKT_x(:,i) = Kinvxi*b; 8 | end 9 | 10 | fKKT_x = fKKT_x + K\bx; 11 | FDx = fKKT_x(1:7, :); 12 | gx = []; gu = []; 13 | 14 | if length(K) > 7 % If nontrivial KKT dynamics 15 | gx = fKKT_x(8:end, :); 16 | end 17 | 18 | if nargin > 4 % bu appears in smooth dynamics 19 | 20 | fKKT_u = K\bu; 21 | 22 | FDu = fKKT_u(1:7, :); 23 | 24 | if length(K) > 7 25 | gu = fKKT_u(8:end, :); 26 | end 27 | varargout{1} = FDx; 28 | varargout{2} = FDu; 29 | varargout{3} = gx; 30 | varargout{4} = gu; 31 | else % u dose not appearh in reset map 32 | varargout{1} = FDx; 33 | varargout{2} = gx; 34 | end 35 | end -------------------------------------------------------------------------------- /Common/spatial_v2/models/planar.m: -------------------------------------------------------------------------------- 1 | function robot = planar( n ) 2 | 3 | % planar create an n-link planar robot. 4 | % planar(n) creates an all-revolute n-link planar robot with identical 5 | % links. For efficient use with Simulink, this function stores a copy of 6 | % the last robot it created, and returns the copy if the arguments match. 7 | 8 | persistent last_robot last_n; 9 | 10 | if length(last_robot) ~= 0 && last_n == n 11 | robot = last_robot; 12 | return 13 | end 14 | 15 | robot.NB = n; 16 | robot.parent = [0:n-1]; 17 | 18 | for i = 1:n 19 | robot.jtype{i} = 'r'; 20 | if i == 1 21 | robot.Xtree{i} = plnr( 0, [0 0]); 22 | else 23 | robot.Xtree{i} = plnr( 0, [1 0]); 24 | end 25 | robot.I{i} = mcI( 1, [0.5 0], 1/12 ); 26 | end 27 | 28 | robot.appearance.base = ... 29 | { 'box', [-0.2 -0.3 -0.2; 0.2 0.3 -0.07] }; 30 | 31 | for i = 1:n 32 | robot.appearance.body{i} = ... 33 | { 'box', [0 -0.07 -0.04; 1 0.07 0.04], ... 34 | 'cyl', [0 0 -0.07; 0 0 0.07], 0.1 }; 35 | end 36 | 37 | last_robot = robot; 38 | last_n = n; 39 | -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/Constraint/SwitchingConstraint.m: -------------------------------------------------------------------------------- 1 | classdef SwitchingConstraint < ConstraintAbstract 2 | properties 3 | nctact 4 | end 5 | methods 6 | function S = SwitchingConstraint(contact_next) 7 | S.nctact = contact_next; 8 | end 9 | function t_array = compute(S, x) 10 | t_array = TConstraintData(length(x)); 11 | if isequal(S.nctact, [1, 0]) 12 | [t_array.h, t_array.hx, t_array.hxx] = Front_TouchDown_Constraint(x); 13 | t_array.hx = t_array.hx(:); 14 | end 15 | if isequal(S.nctact, [0, 1]) 16 | [t_array.h, t_array.hx, t_array.hxx] = Back_TouchDown_Constraint(x); 17 | t_array.hx = t_array.hx(:); 18 | end 19 | end 20 | function params = get_params(S) 21 | % Initialize Augmented Lagrangian parameters 22 | param.sigma = 5; 23 | param.lambda = 0; 24 | params = [param]; 25 | end 26 | end 27 | end -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/Constraint/torque_limit.m: -------------------------------------------------------------------------------- 1 | classdef torque_limit < ConstraintAbstract 2 | methods 3 | function Obj = torque_limit() 4 | end 5 | end 6 | methods 7 | function p_array = compute(Obj, x, u, y) 8 | % Torque limit Cu*u + bu > = 0 9 | % return 1x8 structure array of path constraint 10 | us = length(u); 11 | p_array = repmat(PathConstraintData(14,4,4), 1, 2*us); 12 | Cu = [-eye(us); 13 | eye(us)]; 14 | bu = 33*ones(2*us, 1); 15 | for i = 1:length(p_array) 16 | p_array(i).cu = Cu(i,:)'; 17 | p_array(i).c = Cu(i,:) * u + bu(i); 18 | end 19 | end 20 | function params = get_params(Obj) 21 | % Initialize Reduced Barrier parameters 22 | param = ReB_params_struct(); 23 | param.delta = 0.1; 24 | param.eps = 0.01; 25 | param.delta_min = 0.01; 26 | params = repmat(param, 1, 8); 27 | end 28 | end 29 | end -------------------------------------------------------------------------------- /Common/spatial_v2/sparsity/LTDL.m: -------------------------------------------------------------------------------- 1 | function [L,D] = LTDL( H, lambda ) 2 | 3 | % LTDL factorize H -> L'*D*L exploiting branch-induced sparsity 4 | % [L,D]=LTDL(H,lambda) returns a unit-lower-triangular matrix L and a 5 | % diagonal matrix D that satisfy L'*D*L = H, where H is a symmetric, 6 | % positive-definite matrix having the property that the nonzero elements on 7 | % row i below the main diagonal appear only in columns lambda(i), 8 | % lambda(lambda(i)), and so on. This is the pattern of branch-induced 9 | % sparsity; and H and lambda can be regarded as the joint-space inertia 10 | % matrix and parent array of a kinematic tree. lambda must satisfy 11 | % 0<=lambda(i) X 14 | 15 | c = cos(i1); 16 | s = sin(i1); 17 | 18 | o1 = [ 1 0 0 ; 19 | s*i2(1)-c*i2(2) c s ; 20 | c*i2(1)+s*i2(2) -s c ]; 21 | 22 | else % X --> theta,r 23 | 24 | c = i1(2,2); 25 | s = i1(2,3); 26 | 27 | o1 = atan2(s,c); 28 | o2 = [ s*i1(2,1)+c*i1(3,1); s*i1(3,1)-c*i1(2,1) ]; 29 | 30 | end 31 | -------------------------------------------------------------------------------- /HSDDP/update_rcost_with_pconstraint.m: -------------------------------------------------------------------------------- 1 | function [l, lpar] = update_rcost_with_pconstraint(l, lpar, pConstr, params, dt) 2 | % l: running cost 3 | % lpar: derivative of running cost 4 | % pConstr: 1xn structure array of path constraints 5 | % params: 1xn structure array of ReB parameters 6 | n = length(pConstr); 7 | assert(n==length(params), 'Path constraints and ReB params have different size'); 8 | for i = 1:n 9 | eps = dt * (params(i).eps); 10 | delta = params(i).delta; 11 | c = pConstr(i).c; 12 | cx = pConstr(i).cx(:); 13 | cy = pConstr(i).cy(:); 14 | cu = pConstr(i).cu(:); 15 | cxx = pConstr(i).cxx; 16 | cuu = pConstr(i).cuu; 17 | cyy = pConstr(i).cyy; 18 | 19 | [B, Bc, Bcc] = ReducedBarrier(c, delta); 20 | l = l + eps*B; 21 | lpar.lx = lpar.lx + eps * Bc * cx; % cx is column vector 22 | lpar.lu = lpar.lu + eps * Bc * cu; 23 | lpar.ly = lpar.ly + eps * Bc * cy; 24 | 25 | lpar.lxx = lpar.lxx + eps * (Bcc*(cx*cx') + Bc*cxx); 26 | lpar.luu = lpar.luu + eps * (Bcc*(cu*cu') + Bc*cuu); 27 | lpar.lyy = lpar.lyy + eps * (Bcc*(cy*cy') + Bc*cyy); 28 | end 29 | end -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/Constraint/support/WB_terminal_constr_support.m: -------------------------------------------------------------------------------- 1 | function WB_terminal_constr_support(WBMC2D) 2 | q = sym('q', [7,1], 'real'); 3 | qd = sym('qd', [7,1], 'real'); 4 | x = [q; qd]; 5 | 6 | ground = -0.404; 7 | Pos_ffoot = WBMC2D.getPosition(q, 3, [0,-WBMC2D.kneeLinkLength]'); 8 | h_FTD = Pos_ffoot(2) - ground; 9 | hx_FTD = jacobian(h_FTD, x); 10 | hxx_FTD = hessian(h_FTD, x); 11 | 12 | Pos_bfoot = WBMC2D.getPosition(q, 5, [0,-WBMC2D.kneeLinkLength]'); 13 | h_BTD = Pos_bfoot(2) - ground; 14 | hx_BTD = jacobian(h_BTD, x); 15 | hxx_BTD = hessian(h_BTD, x); 16 | 17 | folder = '/home/wensinglab/HL/Code/MHPC-project/MATLAB_v2/Examples/Bounding/Constraint/'; 18 | fileName = 'Front_TouchDown_Constraint'; 19 | matlabFunction(h_FTD, hx_FTD, hxx_FTD, 'file', strcat(folder, fileName), 'vars', {x}); 20 | fileName = 'Back_TouchDown_Constraint'; 21 | matlabFunction(h_BTD, hx_BTD, hxx_BTD, 'file', strcat(folder, fileName), 'vars', {x}); 22 | fprintf('Terminal constrait functions generated successfully!\n'); 23 | end -------------------------------------------------------------------------------- /Common/spatial_v2/sparsity/LTL.m: -------------------------------------------------------------------------------- 1 | function L = LTL( H, lambda ) 2 | 3 | % LTL factorize H -> L'*L exploiting branch-induced sparsity 4 | % LTL(H,lambda) returns a lower-triangular matrix L satisfying L'*L = H, 5 | % where H is a symmetric, positive-definite matrix having the property that 6 | % the nonzero elements on row i below the main diagonal appear only in 7 | % columns lambda(i), lambda(lambda(i)), and so on. This is the pattern of 8 | % branch-induced sparsity; and H and lambda can be regarded as the 9 | % joint-space inertia matrix and parent array of a kinematic tree. lambda 10 | % must satisfy 0<=lambda(i) 4x4 homogeneous coordinate transform. 4 | % X=pluho(T) and T=pluho(X) convert between a Plucker coordinate transform 5 | % matrix X and a 4x4 homogeneous coordinate transform matrix T. If the 6 | % argument is a 6x6 matrix then it is taken to be X, otherwise T. 7 | % NOTE: the 4x4 matrices used in 3D graphics (e.g. OpenGL and Matlab handle 8 | % graphics) are displacement operators, which are the inverses of 9 | % coordinate transforms. For example, to set the 'matrix' property of a 10 | % Matlab hgtransform graphics object so as to rotate its children by an 11 | % angle theta about the X axis, use inv(pluho(rotx(theta))). 12 | 13 | % Formulae: 14 | % X = [ E 0 ] T = [ E -Er ] 15 | % [ -Erx E ] [ 0 1 ] 16 | 17 | if all(size(in)==[6 6]) % Plucker -> 4x4 homogeneous 18 | E = in(1:3,1:3); 19 | mErx = in(4:6,1:3); % - E r cross 20 | out = [ E, skew(mErx*E'); 0 0 0 1 ]; 21 | else % 4x4 homogeneous -> Plucker 22 | E = in(1:3,1:3); 23 | mEr = in(1:3,4); % - E r 24 | out = [ E, zeros(3); skew(mEr)*E, E ]; 25 | end 26 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Robotics, Optimization, and Assistive Mobility Lab @ Notre Dame 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Examples/Quadurped/Kinematics/Link4Jacobian.m: -------------------------------------------------------------------------------- 1 | function [J,Jd] = Link4Jacobian(in1,in2) 2 | %LINK4JACOBIAN 3 | % [J,JD] = LINK4JACOBIAN(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 29-Nov-2020 17:42:50 7 | 8 | p1_1 = in2(1); 9 | p2_1 = in2(2); 10 | q3 = in1(3,:); 11 | q6 = in1(6,:); 12 | qd3 = in1(10,:); 13 | qd6 = in1(13,:); 14 | t2 = sin(q3); 15 | t3 = cos(q3); 16 | t4 = cos(q6); 17 | t5 = sin(q6); 18 | t6 = conj(p1_1); 19 | t7 = t3.*t5; 20 | t8 = t2.*t4; 21 | t9 = t7+t8; 22 | t10 = conj(p2_1); 23 | t11 = t2.*t5; 24 | t13 = t3.*t4; 25 | t12 = t11-t13; 26 | t14 = t6.*(t11-t13); 27 | t15 = t3.*(1.9e1./1.0e2); 28 | t16 = t6.*t12; 29 | t17 = qd6.*(t16-t9.*t10); 30 | t18 = t2.*(1.9e1./1.0e2); 31 | J = reshape([1.0,0.0,0.0,1.0,t18-t6.*t9-t10.*t12,t14+t15-t9.*t10,0.0,0.0,0.0,0.0,-t6.*t9-t10.*t12,t14-t9.*t10,0.0,0.0],[2,7]); 32 | if nargout > 1 33 | t19 = t6.*t9; 34 | t20 = t10.*(t11-t13); 35 | t21 = t19+t20; 36 | t22 = qd6.*t21; 37 | Jd = reshape([0.0,0.0,0.0,0.0,t17+qd3.*(t15+t16-t9.*t10),t22+qd3.*(-t18+t19+t10.*t12),0.0,0.0,0.0,0.0,t17+qd3.*(t16-t9.*t10),t22+qd3.*t21,0.0,0.0],[2,7]); 38 | end 39 | -------------------------------------------------------------------------------- /Examples/Quadurped/Kinematics/Link2Jacobian.m: -------------------------------------------------------------------------------- 1 | function [J,Jd] = Link2Jacobian(in1,in2) 2 | %LINK2JACOBIAN 3 | % [J,JD] = LINK2JACOBIAN(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 29-Nov-2020 17:42:39 7 | 8 | p1_1 = in2(1); 9 | p2_1 = in2(2); 10 | q3 = in1(3,:); 11 | q4 = in1(4,:); 12 | qd3 = in1(10,:); 13 | qd4 = in1(11,:); 14 | t2 = sin(q3); 15 | t3 = cos(q3); 16 | t4 = cos(q4); 17 | t5 = sin(q4); 18 | t6 = conj(p1_1); 19 | t7 = t3.*t5; 20 | t8 = t2.*t4; 21 | t9 = t7+t8; 22 | t10 = conj(p2_1); 23 | t11 = t2.*t5; 24 | t13 = t3.*t4; 25 | t12 = t11-t13; 26 | t14 = t6.*(t11-t13); 27 | J = reshape([1.0,0.0,0.0,1.0,t2.*(-1.9e1./1.0e2)-t6.*t9-t10.*t12,t3.*(-1.9e1./1.0e2)+t14-t9.*t10,-t6.*t9-t10.*t12,t14-t9.*t10,0.0,0.0,0.0,0.0,0.0,0.0],[2,7]); 28 | if nargout > 1 29 | t15 = t6.*t12; 30 | t16 = t9.*t10; 31 | t17 = t6.*t9; 32 | t18 = t10.*(t11-t13); 33 | t19 = t17+t18; 34 | t20 = qd4.*t19; 35 | Jd = reshape([0.0,0.0,0.0,0.0,-qd3.*(t3.*(1.9e1./1.0e2)-t15+t16)+qd4.*(t15-t9.*t10),t20+qd3.*(t2.*(1.9e1./1.0e2)+t17+t18),qd3.*(t15-t16)+qd4.*(t15-t16),t20+qd3.*t19,0.0,0.0,0.0,0.0,0.0,0.0],[2,7]); 36 | end 37 | -------------------------------------------------------------------------------- /Examples/Quadurped/Dynamics/FB/Support/FBDynamics_par.m: -------------------------------------------------------------------------------- 1 | function [fx,fu] = FBDynamics_par(in1,in2,in3,in4) 2 | %FBDYNAMICS_PAR 3 | % [FX,FU] = FBDYNAMICS_PAR(IN1,IN2,IN3,IN4) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 26-Dec-2020 23:13:42 7 | 8 | p1 = in3(1,:); 9 | p2 = in3(2,:); 10 | p3 = in3(3,:); 11 | p4 = in3(4,:); 12 | q1 = in1(1,:); 13 | q2 = in1(2,:); 14 | s1 = in4(1,:); 15 | s2 = in4(2,:); 16 | u1 = in2(1,:); 17 | u2 = in2(2,:); 18 | u3 = in2(3,:); 19 | u4 = in2(4,:); 20 | fx = reshape([0.0,0.0,0.0,0.0,0.0,s1.*u2.*4.307272227516377+s2.*u4.*4.307272227516377,0.0,0.0,0.0,0.0,0.0,s1.*u1.*(-4.307272227516377)-s2.*u3.*4.307272227516377,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0],[6,6]); 21 | if nargout > 1 22 | t2 = s1.*1.211827435773146e-1; 23 | t3 = s2.*1.211827435773146e-1; 24 | fu = reshape([0.0,0.0,0.0,t2,0.0,s1.*(p2.*4.307272227516377-q2.*4.307272227516377),0.0,0.0,0.0,0.0,t2,-s1.*(p1.*4.307272227516377-q1.*4.307272227516377),0.0,0.0,0.0,t3,0.0,s2.*(p4.*4.307272227516377-q2.*4.307272227516377),0.0,0.0,0.0,0.0,t3,-s2.*(p3.*4.307272227516377-q1.*4.307272227516377)],[6,4]); 25 | end 26 | -------------------------------------------------------------------------------- /Examples/BouncingBall/createBouncingBallTask.m: -------------------------------------------------------------------------------- 1 | function [phases, hybridT] = createBouncingBallTask(problem) 2 | dt = problem.dt; 3 | len_horizons = problem.len_horizons; 4 | 5 | Ball = BouncingBall(dt); 6 | bouncingCost = BouncingCost(dt); 7 | 8 | phases(1) = Phase; 9 | phases(2) = Phase; 10 | 11 | hybridT(1) = Trajectory(2,1,1,len_horizons(1),dt); 12 | hybridT(2) = Trajectory(2,1,1,len_horizons(2),dt); 13 | 14 | phases(1).set_dynamics({@Ball.dynamics, @Ball.dynamics_par}); 15 | phases(1).set_resetmap({@Ball.resetmap, @Ball.resetmap_par}); 16 | phases(1).set_running_cost({@bouncingCost.running_cost, @bouncingCost.running_cost_par}); 17 | phases(1).set_terminal_cost({@(x) bouncingCost.terminal_cost(x,1),... 18 | @(x) bouncingCost.terminal_cost_par(x,1)}); 19 | phases(1).add_terminal_constraints(BouncingConstraint(1)); 20 | 21 | 22 | phases(2).set_dynamics({@Ball.dynamics, @Ball.dynamics_par}); 23 | phases(2).set_resetmap({@Ball.resetmap, @Ball.resetmap_par}); 24 | phases(2).set_running_cost({@bouncingCost.running_cost, @bouncingCost.running_cost_par}); 25 | phases(2).set_terminal_cost({@(x) bouncingCost.terminal_cost(x,2),... 26 | @(x) bouncingCost.terminal_cost_par(x,2)}); 27 | end -------------------------------------------------------------------------------- /Examples/BouncingBall/Cost/BouncingCost.m: -------------------------------------------------------------------------------- 1 | classdef BouncingCost < handle 2 | properties 3 | dt 4 | R 5 | Q 6 | Qf 7 | S 8 | end 9 | methods 10 | function C = BouncingCost(dt) 11 | C.dt = dt; 12 | C.R = 0.5*dt; 13 | C.Q = zeros(2); 14 | C.S = 0; 15 | C.Qf = 100 * eye(2); 16 | end 17 | function l = running_cost(C,k,x,u,y) 18 | l = C.R * u^2; 19 | end 20 | function lpar = running_cost_par(C,k,x,u,y) 21 | lpar = RCostPartialData(2,1,1); 22 | lpar.lu = 2*C.R*u; 23 | lpar.luu = 2*C.R; 24 | end 25 | function phi = terminal_cost(C,x,mode) 26 | if mode == 1 27 | phi = 0; 28 | else 29 | xd = [3, 0]'; 30 | phi = wsquare(x-xd, C.Qf); 31 | end 32 | end 33 | function phi_par = terminal_cost_par(C,x,mode) 34 | phi_par = TCostPartialData(2); 35 | xd = [3, 0]'; 36 | if mode == 2 37 | phi_par.G = 2*C.Qf*(x - xd); 38 | phi_par.H = 2*C.Qf; 39 | end 40 | end 41 | end 42 | end -------------------------------------------------------------------------------- /Examples/Quadurped/Dynamics/FB/Support/FBDynamimcs_support.m: -------------------------------------------------------------------------------- 1 | function FBDynamimcs_support(FBMC2D) 2 | q_size = 3; %(x,z,pitch (theta)) 3 | x_size = 6; 4 | u_size = 4; % ground reaction force F1 F2 5 | 6 | q = sym('q',[q_size 1],'real'); % x, z, pitch 7 | qd = sym('qd', [q_size 1], 'real'); 8 | x = [q; qd]; 9 | u = sym('u',[u_size 1],'real'); % [F1; F2] 10 | p = sym('p',[4,1],'real'); % foothold location [p1; p2] 11 | s = sym('s', [2,1], 'real'); % contact state 12 | 13 | I = FBMC2D.Inertia; 14 | m = FBMC2D.mass; 15 | 16 | % continuous dynamics 17 | f = [qd; 18 | s(1)*u(1:2,1)/m + s(2)*u(3:4)/m + [0,-9.81]'; 19 | s(1)*(I\cross2D((p(1:2,1) - x(1:2,1)),u(1:2,1))) + s(2)*(I\cross2D((p(3:4,1) - x(1:2,1)),u(3:4,1)))]; 20 | 21 | % partials of continuous dynamics 22 | fx = jacobian(f, x); 23 | fu = jacobian(f, u); 24 | 25 | matlabFunction(f, 'file', 'Dynamics/FB/Support/FBDynamics','vars',{x,u,p,s}); 26 | matlabFunction(fx, fu, 'file','Dynamics/FB/Support/FBDynamics_par','vars',{x,u,p,s}); 27 | 28 | fprintf('Trunk Dynamics support funtions generated successfully!\n'); 29 | end 30 | 31 | 32 | function result = cross2D(p1,p2) 33 | p1_3D = [p1(1),0,p1(2)]'; 34 | p2_3D = [p2(1),0,p2(2)]'; 35 | y = [0 1 0]'; 36 | result = dot(y, cross(p1_3D,p2_3D)); 37 | end -------------------------------------------------------------------------------- /Common/spatial_v2/startup.m: -------------------------------------------------------------------------------- 1 | % When Matlab starts up, it looks for a file called startup.m in the 2 | % directory (folder) in which it is being started. If such a file is 3 | % found, then it is automatically executed as part of Matlab's start-up 4 | % procedure. The line at the end of this file tells Matlab to add the 5 | % directory '/home/users/roy/booksoft/spatial_v2', and all of its 6 | % subdirectories, to Matlab's command search path. An action like this is 7 | % necessary if Matlab is to find all of the functions in spatial_v2. 8 | 9 | % WHAT YOU MUST DO: 10 | % (1) Replace the string '/home/users/roy/booksoft/spatial_v2' with the 11 | % full path name of your directory spatial_v2 (keeping the two 12 | % single-quotes that delimit the string). For example, on a Windows 13 | % PC, if you downloaded spatial_v2.zip to the folder C:\user\john and 14 | % unzipped it there, then the replacement string will be 15 | % 'C:\user\john\spatial_v2'. 16 | % (2) Copy this file to each directory in which you want Matlab to have 17 | % access to spatial_v2 when it starts. If there is already a file 18 | % startup.m in that directory, then simply add the (edited) line 19 | % below to that file. 20 | 21 | addpath( genpath( 'C:\Users\hli25\Box\My Research\Robotics\HW3_spatial_v2' ) ); 22 | -------------------------------------------------------------------------------- /Common/spatial_v2/spatial/XtoV.m: -------------------------------------------------------------------------------- 1 | function v = XtoV( X ) 2 | 3 | % XtoV obtain spatial/planar vector from small-angle transform. 4 | % XtoV(X) interprets X as the coordinate transform from A to B 5 | % coordinates, which implicitly defines the location of frame B relative to 6 | % frame A. XtoV calculates the velocity of a third frame, C(t), that 7 | % travels at constant velocity from frame A to frame B in one time unit. 8 | % Thus, C(0)=A, C(1)=B and dC/dt is a constant. The return value, v, is 9 | % the velocity of C, calculated using a small-angle approximation. It is 10 | % therefore exact only if A and B are parallel. The return value is a 11 | % spatial vector if X is a 6x6 matrix; otherwise it is a planar vector. 12 | % The return value is an invariant of X (i.e., v=X*v), and can therefore be 13 | % regarded as being expressed in both A and B coordinates. 14 | 15 | if all(size(X)==[6 6]) % Plucker xform -> spatial vector 16 | 17 | v = 0.5 * [ X(2,3) - X(3,2); 18 | X(3,1) - X(1,3); 19 | X(1,2) - X(2,1); 20 | X(5,3) - X(6,2); 21 | X(6,1) - X(4,3); 22 | X(4,2) - X(5,1) ]; 23 | 24 | else % planar xform -> planar vector 25 | 26 | v = [ X(2,3); 27 | (X(3,1) + X(2,2)*X(3,1) + X(2,3)*X(2,1))/2; 28 | (-X(2,1) - X(2,2)*X(2,1) + X(2,3)*X(3,1))/2 ]; 29 | end 30 | -------------------------------------------------------------------------------- /Examples/Quadurped/Dynamics/WB/Support/WBDynamics_support.m: -------------------------------------------------------------------------------- 1 | function WBDynamics_support(quadruped) 2 | % This function converts symbolic expression to function handles and only 3 | % executes once. 4 | qsize = 7; 5 | xsize = 14; 6 | usize = 4; 7 | ysize = 4; 8 | 9 | q = sym('q', [qsize, 1]); 10 | qd = sym('qd', [qsize, 1]); 11 | x = [q;qd]; 12 | u = sym('u', [usize, 1]); 13 | 14 | % [mc, ~] = build2DminiCheetah(); 15 | 16 | % Jacobian and Jacobian derivative 17 | p = sym('p', 2); 18 | 19 | for linkidx = 1:5 20 | contactPos = quadruped.getPosition(q,linkidx, p); 21 | J = jacobian(contactPos, q); 22 | Jd = reshape(jacobian(reshape(J, [numel(J),1]),q)*qd, 2, qsize); 23 | % J and Jd partial w.r.t. x 24 | Jx = MatrixPartial(J, x); 25 | Jdx = MatrixPartial(Jd, x); 26 | JFilename = sprintf('Kinematics/Link%dJacobian', linkidx); 27 | JPFilename = sprintf('Kinematics/Link%dJacobian_par', linkidx); 28 | matlabFunction(J, Jd,'file',JFilename,'vars',{x,p}); 29 | matlabFunction(Jx, Jdx, 'file',JPFilename,'vars',{x,p}); 30 | end 31 | 32 | % Dynamics partials 33 | [H, C] = HandC(quadruped.model,q,qd); 34 | Hx = MatrixPartial(H, x); 35 | Cx = jacobian(C, x); 36 | 37 | matlabFunction(Hx, Cx, 'file','Dynamics/WB/FreeDynamics_par','vars',{x}); 38 | 39 | fprintf('WBDynamics support functions generated successfully! \n'); 40 | end -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/ID.m: -------------------------------------------------------------------------------- 1 | function tau = ID( model, q, qd, qdd, f_ext ) 2 | 3 | % ID Inverse Dynamics via Recursive Newton-Euler Algorithm 4 | % ID(model,q,qd,qdd,f_ext) calculates the inverse dynamics of a kinematic 5 | % tree via the recursive Newton-Euler algorithm. q, qd and qdd are vectors 6 | % of joint position, velocity and acceleration variables; and the return 7 | % value is a vector of joint force variables. f_ext is an optional 8 | % argument specifying the external forces acting on the bodies. It can be 9 | % omitted if there are no external forces. The format of f_ext is 10 | % explained in the source code of apply_external_forces. 11 | 12 | a_grav = get_gravity(model); 13 | 14 | for i = 1:model.NB 15 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q(i) ); 16 | vJ = S{i}*qd(i); 17 | Xup{i} = XJ * model.Xtree{i}; 18 | if model.parent(i) == 0 19 | v{i} = vJ; 20 | a{i} = Xup{i}*(-a_grav) + S{i}*qdd(i); 21 | else 22 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 23 | a{i} = Xup{i}*a{model.parent(i)} + S{i}*qdd(i) + crm(v{i})*vJ; 24 | end 25 | f{i} = model.I{i}*a{i} + crf(v{i})*model.I{i}*v{i}; 26 | end 27 | 28 | if nargin == 5 29 | f = apply_external_forces( model.parent, Xup, f, f_ext ); 30 | end 31 | 32 | for i = model.NB:-1:1 33 | tau(i,1) = S{i}' * f{i}; 34 | if model.parent(i) ~= 0 35 | f{model.parent(i)} = f{model.parent(i)} + Xup{i}'*f{i}; 36 | end 37 | end 38 | -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/Constraint/joint_limit.m: -------------------------------------------------------------------------------- 1 | classdef joint_limit < ConstraintAbstract 2 | methods 3 | function Obj = joint_limit() 4 | end 5 | end 6 | methods 7 | function p_array = compute(Obj, x, u, y) 8 | % return 1x8 structure array of path constraint 9 | q = x(4:7); 10 | q = q(:); 11 | Cjoint = [-eye(4); 12 | eye(4)]; 13 | bjoint = [ pi/4; % front hip min 14 | -0.1; % front knee min 15 | 1.15*pi; % back hip min 16 | -0.1; % back knee min 17 | pi; % front hip max 18 | pi-0.2; % front knee max 19 | 0.1; % back hip max 20 | pi-0.2]; % back knee max 21 | p_array = repmat(PathConstraintData(14,4,4), 1, 8); 22 | for i = 1:length(p_array) 23 | p_array(i).c = Cjoint(i,:) * q + bjoint(i); 24 | p_array(i).cx = [Cjoint(i,:), zeros(1,7)]'; 25 | end 26 | end 27 | function params = get_params(Obj) 28 | % Initialize Reduced Barrier parameters 29 | param = ReB_params_struct(); 30 | param.delta = 0.1; 31 | param.eps = 0; 32 | param.delta_min = 0.01; 33 | params = repmat(param, 1, 8); 34 | end 35 | end 36 | end -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/Constraint/GRF_constraint.m: -------------------------------------------------------------------------------- 1 | classdef GRF_constraint < ConstraintAbstract 2 | properties 3 | ctact 4 | end 5 | methods 6 | function Obj = GRF_constraint(contact) 7 | % ctact: 2x1 vector for contact status of front and back foot 8 | Obj.ctact = contact; 9 | end 10 | end 11 | methods 12 | function p_array = compute(Obj, x, u, y) 13 | % return 1x3 structure array of path constraint 14 | ctact = Obj.ctact; 15 | mu_fric = 0.6; % static friction coefficient 16 | Cy = [0, ctact(1), 0, ctact(2); 17 | -ctact(1) ctact(1)*mu_fric, -ctact(2), ctact(2)*mu_fric; 18 | ctact(1) ctact(1)*mu_fric, ctact(2), ctact(2)*mu_fric]; 19 | by = zeros(3,1); 20 | p_array = repmat(PathConstraintData(14,4,4), 1, 3); 21 | for i = 1:length(p_array) 22 | p_array(i).c = Cy(i,:) * y + by(i); 23 | p_array(i).cy = Cy(i,:)'; 24 | end 25 | end 26 | function params = get_params(obj) 27 | % Initialize Reduced Barrier parameters 28 | param = ReB_params_struct(); 29 | param.delta = 0.1; 30 | param.delta_min = 0.01; 31 | param.eps = 0.01; 32 | params = repmat(param, 1, 3); 33 | end 34 | end 35 | end -------------------------------------------------------------------------------- /Common/spatial_v2/3D/rqd.m: -------------------------------------------------------------------------------- 1 | function qd = rqd( in1, in2 ) 2 | 3 | % rqd derivative of unit quaternion from angular velocity 4 | % qd=rqd(wA,q) and qd=rqd(q,wB) calculate the derivative of a unit 5 | % quaternion, q, representing the orientation of a coordinate frame B 6 | % relative to frame A, given the angular velocity w of B relative to A. If 7 | % w is expressed in A coordinates then use rqd(wA,q); and if w is expressed 8 | % in B coordinates then use rqd(q,wB). If the length of the first argument 9 | % is 4 then it is assumed to be q, otherwise it is assumed to be wA. The 10 | % return value is a column vector, but the arguments can be row or column 11 | % vectors. It is not necessary for |q| to be exactly 1. If |q|~=1 then qd 12 | % contains a magnitude-stabilizing term that will cause |q| to converge 13 | % towards 1 if q is obtained by numerical integration of qd. 14 | 15 | Kstab = 0.1; % magnitude stabilization constant: 16 | % value not critical, but K>1 too big 17 | 18 | if length(in1) == 4 % arguments are q and wB 19 | q = in1; 20 | w(1:3,1) = in2; 21 | Q = [ q(1) -q(2) -q(3) -q(4); 22 | q(2) q(1) -q(4) q(3); 23 | q(3) q(4) q(1) -q(2); 24 | q(4) -q(3) q(2) q(1) ]; 25 | else % arguments are wA and q 26 | q = in2; 27 | w(1:3,1) = in1; 28 | Q = [ q(1) -q(2) -q(3) -q(4); 29 | q(2) q(1) q(4) -q(3); 30 | q(3) -q(4) q(1) q(2); 31 | q(4) q(3) -q(2) q(1) ]; 32 | end 33 | 34 | qd = 0.5 * Q * [ Kstab*norm(w)*(1-norm(q)); w ]; 35 | -------------------------------------------------------------------------------- /Examples/Quadurped/Dynamics/get2DMCParams.m: -------------------------------------------------------------------------------- 1 | function mc2D = get2DMCParams(mc3D) 2 | % Input: mc3D physical parameters of mini Cheetah 3 | mc2D = mc3D; 4 | 5 | mc2D.hipLinkMass = mc3D.hipLinkMass * 2; 6 | mc2D.kneeLinkMass = mc3D.kneeLinkMass*2; 7 | mc2D.bodyMass = mc3D.bodyMass + 4*mc3D.abadLinkMass; % 4 abads in body 8 | 9 | % update body CoM (considering 4 abads into body) 10 | weighted_loc = mc3D.bodyMass*mc3D.bodyCoM; 11 | for i = 1:4 12 | weighted_loc = weighted_loc + mc3D.abadLinkMass*(mc3D.abadLoc{i}+mc3D.abadLinkCoM); 13 | end 14 | mc2D.bodyCoM = weighted_loc/mc2D.bodyMass; 15 | 16 | mc2D.hipRotInertia = mc3D.hipRotInertia*2; % two legs in one 17 | mc2D.kneeRotInertia = mc3D.kneeRotInertia*2; % two legs in one 18 | 19 | % update body inertia 20 | S = skew(mc2D.bodyCoM); % skew(v) defined in spatial v2 21 | mc2D.bodyRotInertia = mc3D.bodyRotInertia + mc3D.bodyMass*(S*S'); % body inertia w.r.t. new CoM 22 | for i = 1:4 23 | S = skew(mc3D.abadLoc{i} + mc3D.abadLinkCoM - mc2D.bodyCoM); 24 | mc2D.bodyRotInertia = mc2D.bodyRotInertia + mc3D.abadRotInertia + mc3D.abadLinkMass*(S*S'); % body inertia considering abads 25 | end 26 | mc2D.hipLoc{1} = [mc2D.bodyLength, 0, 0]'/2; 27 | mc2D.hipLoc{2} = [-mc2D.bodyLength, 0, 0]'/2; 28 | mc2D.robotMass = mc2D.bodyMass + 2*mc2D.kneeLinkMass + 2*mc2D.hipLinkMass; 29 | fieldstoRM = {'abadLinkMass', 'abadLinkCoM', 'abadLoc', 'abadLinkLength'}; 30 | mc2D = rmfield(mc2D, fieldstoRM); 31 | end -------------------------------------------------------------------------------- /HSDDP/ConstraintContainer.m: -------------------------------------------------------------------------------- 1 | classdef ConstraintContainer < handle 2 | properties 3 | pconstrObjs = [] 4 | tconstrObjs = [] 5 | end 6 | methods 7 | function C = ConstraintContainer() 8 | end 9 | end 10 | methods 11 | function add_pathConstraint(C, pconstrObj) 12 | C.pconstrObjs{end+1} = pconstrObj; 13 | end 14 | function p_array = compute_pConstraints(C, x, u, y) 15 | p_array = []; 16 | for i = 1:length(C.pconstrObjs) 17 | p_array = [p_array, C.pconstrObjs{i}.compute(x,u,y)]; 18 | end 19 | end 20 | function params = get_reb_params(C) 21 | params = []; 22 | for i = 1:length(C.pconstrObjs) 23 | params = [params, C.pconstrObjs{i}.get_params()]; 24 | end 25 | end 26 | end 27 | methods 28 | function add_terminalConstraint(C, tconstrObj) 29 | C.tconstrObjs{end+1} = tconstrObj; 30 | end 31 | function t_array = compute_tConstraints(C, x) 32 | t_array = []; 33 | for i = 1:length(C.tconstrObjs) 34 | t_array = [t_array, C.tconstrObjs{i}.compute(x)]; 35 | end 36 | end 37 | function params = get_al_params(C) 38 | params = []; 39 | for i = 1:length(C.tconstrObjs) 40 | params = [params, C.tconstrObjs{i}.get_params()]; 41 | end 42 | end 43 | end 44 | end -------------------------------------------------------------------------------- /Examples/Quadurped/Dynamics/WB/build2DminiCheetah.m: -------------------------------------------------------------------------------- 1 | function build2DminiCheetah(Quad) 2 | % This function reformualtes the 3D mc inertia parameter to 2D, and 3 | % creates a full planar quadruped model structure. 4 | % The 0-configuration is with legs straight down, cheetah 5 | % pointed along the +x axis of the ICS. 6 | % The ICS has +z up, +x right, and +y inner page 7 | % Planar model has 7 DoFs, x, z translation, rotation around y 8 | % and front (back) hip and knee rotations 9 | 10 | mc3D = get3DMCParams(); 11 | mc2D = get2DMCParams(mc3D); 12 | 13 | Quad.bodyMass = mc2D.bodyMass; 14 | Quad.bodyLength = mc2D.bodyLength; 15 | Quad.bodyHeight = mc2D.bodyHeight; 16 | Quad.bodyWidth = mc2D.bodyWidth; 17 | Quad.bodyCoM = mc2D.bodyCoM; 18 | Quad.bodyRotInertia = mc2D.bodyRotInertia; 19 | Quad.hipLinkLength = mc2D.hipLinkLength; 20 | Quad.hipLinkMass = mc2D.hipLinkMass; 21 | Quad.hipLinkCoM = mc2D.hipLinkCoM; 22 | Quad.hipRotInertia = mc2D.hipRotInertia; 23 | Quad.hipLoc = mc2D.hipLoc; 24 | Quad.kneeLoc = mc2D.kneeLoc; 25 | Quad.kneeLinkLength = mc2D.kneeLinkLength; 26 | Quad.kneeLinkMass = mc2D.kneeLinkMass; 27 | Quad.kneeLinkCoM = mc2D.kneeLinkCoM; 28 | Quad.kneeRotInertia = mc2D.kneeRotInertia; 29 | Quad.robotMass = mc2D.robotMass; 30 | Quad.buildModel(); 31 | 32 | end -------------------------------------------------------------------------------- /Examples/BouncingBall/Dynamics/BouncingBall.m: -------------------------------------------------------------------------------- 1 | classdef BouncingBall < handle 2 | properties 3 | dt 4 | m 5 | g 6 | e 7 | end 8 | methods 9 | function B = BouncingBall(dt) 10 | B.dt = dt; 11 | B.m = 1; 12 | B.g = 9.81; 13 | B.e = 0.75; 14 | end 15 | end 16 | methods 17 | function [f, y] = dynamics_cont(B, x, u) 18 | % x 2x1 vector. x = [z, zdot]'; 19 | % u is a scalar 20 | Ac = [0 1; 0 0]; 21 | Bc = [0; 1/B.m]; 22 | Cc = [1 0]; 23 | Dc = 0; 24 | f = Ac * x + Bc * u - [0; B.g]; 25 | y = Cc * x + Dc * u; 26 | end 27 | 28 | function [xnext, y] = dynamics(B,x,u) 29 | [f, y] = B.dynamics_cont(x,u); 30 | xnext = x + f * B.dt; 31 | end 32 | 33 | function [Ac,Bc,Cc,Dc] = dynamics_par_cont(B,x,u) 34 | Ac = [0 1; 0 0]; 35 | Bc = [0; 1/B.m]; 36 | Cc = [1 0]; 37 | Dc = 0; 38 | end 39 | function [A,B,C,D] = dynamics_par(B,x,u) 40 | [Ac,Bc,Cc,Dc] = B.dynamics_par_cont(x, u); 41 | A = eye(size(Ac)) + Ac * B.dt; 42 | B = Bc * B.dt; 43 | C = Cc; 44 | D = Dc; 45 | end 46 | function xnext = resetmap(B, x) 47 | Px = [1 0; 0 -B.e]; 48 | xnext = Px * x; 49 | end 50 | function Px = resetmap_par(B,x) 51 | Px = [1 0; 0 -B.e]; 52 | end 53 | end 54 | end -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/BoundingModeLib.m: -------------------------------------------------------------------------------- 1 | classdef BoundingModeLib < handle 2 | properties 3 | lib % structure array of events 4 | end 5 | methods 6 | function L = BoundingModeLib() 7 | L.lib = repmat(struct('mode',[], 'ctact',[], 'name', [],'duration', []), 1, 4); 8 | L.lib(1).mode = 1; 9 | L.lib(1).ctact = [0 1]; 10 | L.lib(1).name = 'BS'; 11 | L.lib(1).duration = 0.08; 12 | 13 | L.lib(2).mode = 2; 14 | L.lib(2).ctact = [0 0]; 15 | L.lib(2).name = 'F1'; 16 | L.lib(2).duration = 0.1; 17 | 18 | L.lib(3).mode = 3; 19 | L.lib(3).ctact = [1 0]; 20 | L.lib(3).name = 'FS'; 21 | L.lib(3).duration = 0.08; 22 | 23 | L.lib(4).mode = 4; 24 | L.lib(4).ctact = [0 0]; 25 | L.lib(4).name = 'F2'; 26 | L.lib(4).duration = 0.1; 27 | end 28 | 29 | function Pn = getNextMode(L, P) 30 | i = P.mode; 31 | if i+1 == 4 32 | Pn = L.lib(4); 33 | else 34 | Pn = L.lib(mod(i+1, 4)); 35 | end 36 | end 37 | 38 | function S = genModeSequence(L, P, n) 39 | % n: length of sequence 40 | % P: current phase 41 | S = repmat(struct('mode',[], 'ctact',[], 'name', [], 'duration', []), 1, n); 42 | S(1) = P; 43 | for i = 2:n 44 | S(i) = L.getNextMode(S(i-1)); 45 | end 46 | end 47 | end 48 | end -------------------------------------------------------------------------------- /Common/spatial_v2/models/singlebody.m: -------------------------------------------------------------------------------- 1 | function model = singlebody 2 | 3 | % SINGLEBODY free-floating single rigid body 4 | % singlebody creates a model data structure for a single free-floating, 5 | % unit-mass rigid body having the shape of a rectangular box with 6 | % dimensions of 3, 2 and 1 in the x, y and z directions, respectively. 7 | 8 | % For efficient use with Simulink, this function builds a new model only 9 | % the first time it is called. Thereafter, it returns this stored copy. 10 | 11 | persistent memory; 12 | 13 | if length(memory) ~= 0 14 | model = memory; 15 | return 16 | end 17 | 18 | model.NB = 1; 19 | model.parent = 0; 20 | 21 | model.jtype = {'R'}; % sacrificial joint replaced by floatbase 22 | model.Xtree = {eye(6)}; 23 | 24 | model.gravity = [0 0 0]; % zero gravity is not the default, 25 | % so it must be stated explicitly 26 | 27 | % This is the inertia of a uniform-density box with one vertex at (0,0,0) 28 | % and a diametrically opposite one at (3,2,1). 29 | 30 | model.I = { mcI( 1, [3 2 1]/2, diag([4+1 9+1 9+4]/12) ) }; 31 | 32 | % Draw the global X, Y and Z axes in red, green and blue 33 | 34 | model.appearance.base = ... 35 | { 'colour', [0.9 0 0], 'line', [0 0 0; 2 0 0], ... 36 | 'colour', [0 0.9 0], 'line', [0 0 0; 0 2 0], ... 37 | 'colour', [0 0.3 0.9], 'line', [0 0 0; 0 0 2] }; 38 | 39 | % Draw the floating body 40 | 41 | model.appearance.body{1} = { 'box', [0 0 0; 3 2 1] }; 42 | 43 | model = floatbase(model); % replace joint 1 with a chain of 6 44 | % joints emulating a floating base 45 | memory = model; 46 | -------------------------------------------------------------------------------- /Examples/Quadurped/Dynamics/get3DMCParams.m: -------------------------------------------------------------------------------- 1 | function mc3D = get3DMCParams() 2 | mc3D = struct(... 3 | 'bodyCoM', zeros(3,1),... 4 | 'hipLinkCoM', [0, 0.016, -0.02]',... 5 | 'kneeLinkCoM',[0, 0, -0.061]',... 6 | 'abadLinkCoM', [0, 0.036, 0]',... 7 | ... 8 | 'bodyMass', 3.3,... 9 | 'hipLinkMass' , 0.634,... 10 | 'kneeLinkMass' , 0.064,... 11 | 'abadLinkMass' , 0.54,... 12 | ... % link rotational inertia w.r.t. its CoM expressed in its own frame 13 | 'bodyRotInertia' , [11253, 0, 0; 0, 36203, 0; 0, 0, 42673]*1e-6,... % trunk rotational inertia 14 | 'hipRotInertia' , [1983, 245, 13; 245, 2103, 1.5; 13, 1.5, 408]*1e-6,... % hip rotational inertia 15 | 'kneeRotInertia' , [245, 0, 0; 0, 248, 0; 0, 0, 6]*1e-6,... % knee rotational inertia 16 | 'abadRotInertia' , [381, 58, 0.45; 58, 560, 0.95; 0.45, 0.95, 444]*1e-6,... % abad rotational inertia 17 | ... 18 | 'bodyLength' , 0.19 * 2,... 19 | 'bodyWidth' , 0.049 * 2,... 20 | 'bodyHeight' , 0.05 * 2,... 21 | ... 22 | 'hipLinkLength' , 0.209,... 23 | 'kneeLinkLength' , 0.195,... 24 | 'abadLinkLength', 0.062,... 25 | ... 26 | 'linkwidth', 0.03); 27 | mc3D.abadLoc{1} = [mc3D.bodyLength, mc3D.bodyWidth, 0]'/2; 28 | mc3D.abadLoc{2} = [mc3D.bodyLength, -mc3D.bodyWidth, 0]'/2; 29 | mc3D.abadLoc{3} = [-mc3D.bodyLength, mc3D.bodyWidth, 0]'/2; 30 | mc3D.abadLoc{4} = [-mc3D.bodyLength, -mc3D.bodyWidth, 0]'/2; 31 | 32 | mc3D.hipLoc = {[0, mc3D.abadLinkLength, 0]'}; 33 | mc3D.kneeLoc = [0, 0, -mc3D.hipLinkLength]'; 34 | end -------------------------------------------------------------------------------- /Examples/Quadurped/Kinematics/Link5Jacobian.m: -------------------------------------------------------------------------------- 1 | function [J,Jd] = Link5Jacobian(in1,in2) 2 | %LINK5JACOBIAN 3 | % [J,JD] = LINK5JACOBIAN(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 29-Nov-2020 17:42:53 7 | 8 | p1_1 = in2(1); 9 | p2_1 = in2(2); 10 | q3 = in1(3,:); 11 | q6 = in1(6,:); 12 | q7 = in1(7,:); 13 | qd3 = in1(10,:); 14 | qd6 = in1(13,:); 15 | qd7 = in1(14,:); 16 | t2 = sin(q3); 17 | t3 = cos(q3); 18 | t4 = sin(q6); 19 | t5 = cos(q6); 20 | t6 = cos(q7); 21 | t7 = t2.*t4; 22 | t15 = t3.*t5; 23 | t8 = t7-t15; 24 | t9 = sin(q7); 25 | t10 = t3.*t4; 26 | t11 = t2.*t5; 27 | t12 = t10+t11; 28 | t13 = t2.*t4.*(2.09e2./1.0e3); 29 | t14 = conj(p1_1); 30 | t16 = t8.*t9; 31 | t22 = t6.*t12; 32 | t27 = t16-t22; 33 | t17 = t14.*t27; 34 | t18 = conj(p2_1); 35 | t19 = t6.*t8; 36 | t20 = t9.*t12; 37 | t21 = t19+t20; 38 | t23 = t3.*t4.*(2.09e2./1.0e3); 39 | t24 = t2.*t5.*(2.09e2./1.0e3); 40 | t25 = t14.*(t19+t20); 41 | t26 = t18.*(t16-t22); 42 | t28 = t23+t24+t25+t26; 43 | t29 = t3.*(1.9e1./1.0e2); 44 | t30 = t23+t24+t25+t26+t29; 45 | t31 = t14.*t21; 46 | t32 = t18.*t27; 47 | t33 = qd7.*(t31+t32); 48 | t34 = qd6.*t28; 49 | t36 = t18.*t21; 50 | t35 = t17-t36; 51 | t37 = t2.*(1.9e1./1.0e2); 52 | J = reshape([1.0,0.0,0.0,1.0,t13+t17+t37-t3.*t5.*(2.09e2./1.0e3)-t18.*t21,t30,0.0,0.0,0.0,0.0,t13+t17-t3.*t5.*(2.09e2./1.0e3)-t18.*t21,t28,t35,t25+t26],[2,7]); 53 | if nargout > 1 54 | t39 = t3.*t5.*(2.09e2./1.0e3); 55 | t38 = t13+t17-t36-t39; 56 | Jd = reshape([0.0,0.0,0.0,0.0,t33+t34+qd3.*t30,-qd7.*t35-qd6.*t38-qd3.*(t13+t17-t36+t37-t3.*t5.*(2.09e2./1.0e3)),0.0,0.0,0.0,0.0,t33+t34+qd3.*t28,-qd3.*t38-qd7.*t35-qd6.*t38,t33+qd3.*(t31+t32)+qd6.*(t31+t32),-qd3.*t35-qd6.*t35-qd7.*t35],[2,7]); 57 | end 58 | -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/EnerMo.m: -------------------------------------------------------------------------------- 1 | function ret = EnerMo( model, q, qd ) 2 | 3 | % EnerMo calculate energy, momentum and related quantities 4 | % EnerMo(robot,q,qd) returns a structure containing the fields KE, PE, 5 | % htot, Itot, mass, cm and vcm. These fields contain the kinetic and 6 | % potential energies of the whole system, the total spatial momentum, the 7 | % total spatial inertia, total mass, position of centre of mass, and the 8 | % linear velocity of centre of mass, respectively. Vector quantities are 9 | % expressed in base coordinates. PE is defined to be zero when cm is 10 | % zero. 11 | 12 | for i = 1:model.NB 13 | [ XJ, S ] = jcalc( model.jtype{i}, q(i) ); 14 | vJ = S*qd(i); 15 | Xup{i} = XJ * model.Xtree{i}; 16 | if model.parent(i) == 0 17 | v{i} = vJ; 18 | else 19 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 20 | end 21 | Ic{i} = model.I{i}; 22 | hc{i} = Ic{i} * v{i}; 23 | KE(i) = 0.5 * v{i}' * hc{i}; 24 | end 25 | 26 | ret.Itot = zeros(size(Ic{1})); 27 | ret.htot = zeros(size(hc{1})); 28 | 29 | for i = model.NB:-1:1 30 | if model.parent(i) ~= 0 31 | Ic{model.parent(i)} = Ic{model.parent(i)} + Xup{i}'*Ic{i}*Xup{i}; 32 | hc{model.parent(i)} = hc{model.parent(i)} + Xup{i}'*hc{i}; 33 | else 34 | ret.Itot = ret.Itot + Xup{i}'*Ic{i}*Xup{i}; 35 | ret.htot = ret.htot + Xup{i}'*hc{i}; 36 | end 37 | end 38 | 39 | a_grav = get_gravity(model); 40 | 41 | if length(a_grav) == 6 42 | g = a_grav(4:6); % 3D linear gravitational accn 43 | h = ret.htot(4:6); % 3D linear momentum 44 | else 45 | g = a_grav(2:3); % 2D gravity 46 | h = ret.htot(2:3); % 2D linear momentum 47 | end 48 | 49 | [mass, cm] = mcI(ret.Itot); 50 | 51 | ret.KE = sum(KE); 52 | ret.PE = - mass * dot(cm,g); 53 | ret.mass = mass; 54 | ret.cm = cm; 55 | ret.vcm = h / mass; 56 | -------------------------------------------------------------------------------- /Examples/BouncingBall/BouncingBallOptimization.m: -------------------------------------------------------------------------------- 1 | clear all 2 | clc 3 | problem.dt = 0.001; 4 | problem.len_horizons = [544, 457]; 5 | 6 | % Set options of HSDDP solver 7 | options.alpha = 0.1; % linear search update param 8 | options.gamma = 0.01; % scale the expected cost reduction 9 | options.beta_penalty = 5; % penalty update param 10 | options.beta_relax = 0.1; % relaxation update param 11 | options.beta_reg = 2; % regularization update param 12 | options.beta_ReB = 7; 13 | options.max_DDP_iter = 5; % maximum DDP iterations 14 | options.max_AL_iter = 10; % maximum AL iterations 15 | options.DDP_thresh = 0.001; % Inner loop opt convergence threshold 16 | options.AL_thresh = 1e-3; % Outer loop opt convergence threshold 17 | options.tconstraint_active = 1; % Augmented Lagrangian active 18 | options.pconstraint_active = 0; % Reduced barrier active 19 | options.feedback_active = 1; 20 | options.Debug = 1; % Debug active 21 | options.one_more_sweep = 1; % Do one more sweep at convergence 22 | 23 | x0 = [4, 0]'; 24 | [phases, hybridT] = createBouncingBallTask(problem); 25 | hybridT(1).Xbar{1} = x0; 26 | hybirdT(1).X{1} = x0; 27 | 28 | % Initialize the trajectory using the Nathon's data 29 | % Initial guss would be set to zero if not initialized 30 | % initializeBouncingTraj(hybridT); 31 | 32 | hsddp = HSDDP(phases, hybridT); 33 | hsddp.set_initial_condition(x0); 34 | [xopt, uopt, Kopt, Info] = hsddp.solve(options); 35 | hsddp_data.xopt = xopt; 36 | hsddp_data.uopt = uopt; 37 | hsddp_data.Kopt = Kopt; 38 | hsddp_data.dt = problem.dt; 39 | compare_hsddp2saltation(hsddp_data); 40 | plot_constraint_violation(Info.max_violations); 41 | 42 | -------------------------------------------------------------------------------- /Examples/Quadurped/Kinematics/Link3Jacobian.m: -------------------------------------------------------------------------------- 1 | function [J,Jd] = Link3Jacobian(in1,in2) 2 | %LINK3JACOBIAN 3 | % [J,JD] = LINK3JACOBIAN(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 29-Nov-2020 17:42:44 7 | 8 | p1_1 = in2(1); 9 | p2_1 = in2(2); 10 | q3 = in1(3,:); 11 | q4 = in1(4,:); 12 | q5 = in1(5,:); 13 | qd3 = in1(10,:); 14 | qd4 = in1(11,:); 15 | qd5 = in1(12,:); 16 | t2 = sin(q3); 17 | t3 = cos(q3); 18 | t4 = sin(q4); 19 | t5 = cos(q4); 20 | t6 = cos(q5); 21 | t7 = t2.*t4; 22 | t15 = t3.*t5; 23 | t8 = t7-t15; 24 | t9 = sin(q5); 25 | t10 = t3.*t4; 26 | t11 = t2.*t5; 27 | t12 = t10+t11; 28 | t13 = t2.*t4.*(2.09e2./1.0e3); 29 | t14 = conj(p1_1); 30 | t16 = t8.*t9; 31 | t22 = t6.*t12; 32 | t27 = t16-t22; 33 | t17 = t14.*t27; 34 | t18 = conj(p2_1); 35 | t19 = t6.*t8; 36 | t20 = t9.*t12; 37 | t21 = t19+t20; 38 | t23 = t3.*t4.*(2.09e2./1.0e3); 39 | t24 = t2.*t5.*(2.09e2./1.0e3); 40 | t25 = t14.*(t19+t20); 41 | t26 = t18.*(t16-t22); 42 | t28 = t23+t24+t25+t26; 43 | t29 = t14.*t21; 44 | t30 = t18.*t27; 45 | t31 = qd5.*(t29+t30); 46 | t32 = qd4.*t28; 47 | t34 = t18.*t21; 48 | t33 = t17-t34; 49 | J = reshape([1.0,0.0,0.0,1.0,t2.*(-1.9e1./1.0e2)+t13+t17-t3.*t5.*(2.09e2./1.0e3)-t18.*t21,t3.*(-1.9e1./1.0e2)+t23+t24+t25+t26,t13+t17-t3.*t5.*(2.09e2./1.0e3)-t18.*t21,t28,t33,t25+t26,0.0,0.0,0.0,0.0],[2,7]); 50 | if nargout > 1 51 | t35 = t18.*(t19+t20); 52 | t36 = t3.*t5.*(2.09e2./1.0e3); 53 | t37 = t13+t17-t35-t36; 54 | t38 = t17-t35; 55 | Jd = reshape([0.0,0.0,0.0,0.0,t31+t32+qd3.*(t3.*(-1.9e1./1.0e2)+t23+t24+t29+t30),-qd4.*(t13+t17-t34-t3.*t5.*(2.09e2./1.0e3))-qd5.*t33+qd3.*(t2.*(1.9e1./1.0e2)-t13-t17+t35+t36),t31+t32+qd3.*t28,-qd3.*t37-qd4.*t37-qd5.*t38,t31+qd3.*(t29+t30)+qd4.*(t29+t30),-qd3.*t38-qd4.*t38-qd5.*t38,0.0,0.0,0.0,0.0],[2,7]); 56 | end 57 | -------------------------------------------------------------------------------- /Common/spatial_v2/spatial/mcI.m: -------------------------------------------------------------------------------- 1 | function [o1,o2,o3] = mcI( i1, i2, i3 ) 2 | 3 | % mcI rigid-body inertia <--> mass, CoM and rotational inertia. 4 | % rbi=mcI(m,c,I) and [m,c,I]=mcI(rbi) convert between the spatial or planar 5 | % inertia matrix of a rigid body (rbi) and its mass, centre of mass and 6 | % rotational inertia about the centre of mass (m, c and I). In the spatial 7 | % case, c is 3x1, I is 3x3 and rbi is 6x6. In the planar case, c is 2x1, I 8 | % is a scalar and rbi is 3x3. In both cases, m is a scalar. When c is an 9 | % argument, it can be either a row or a column vector. If only one 10 | % argument is supplied then it is assumed to be rbi; and if it is a 6x6 11 | % matrix then it is assumed to be spatial. Otherwise, three arguments must 12 | % be supplied, and if length(c)==3 then mcI calculates a spatial inertia 13 | % matrix. NOTE: (1) mcI(rbi) requires rbi to have nonzero mass; (2) if |c| 14 | % is much larger than the radius of gyration, or the dimensions of the 15 | % inertia ellipsoid, then extracting I from rbi is numerically 16 | % ill-conditioned. 17 | 18 | if nargin == 1 19 | [o1,o2,o3] = rbi_to_mcI( i1 ); 20 | else 21 | o1 = mcI_to_rbi( i1, i2, i3 ); 22 | end 23 | 24 | 25 | function rbi = mcI_to_rbi( m, c, I ) 26 | 27 | if length(c) == 3 % spatial 28 | 29 | C = skew(c); 30 | rbi = [ I + m*C*C', m*C; m*C', m*eye(3) ]; 31 | 32 | else % planar 33 | 34 | rbi = [ I+m*dot(c,c), -m*c(2), m*c(1); 35 | -m*c(2), m, 0; 36 | m*c(1), 0, m ]; 37 | end 38 | 39 | 40 | function [m,c,I] = rbi_to_mcI( rbi ) 41 | 42 | if all(size(rbi)==[6 6]) % spatial 43 | 44 | m = rbi(6,6); 45 | mC = rbi(1:3,4:6); 46 | c = skew(mC)/m; 47 | I = rbi(1:3,1:3) - mC*mC'/m; 48 | 49 | else % planar 50 | 51 | m = rbi(3,3); 52 | c = [rbi(3,1);-rbi(2,1)]/m; 53 | I = rbi(1,1) - m*dot(c,c); 54 | 55 | end 56 | -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/jcalc.m: -------------------------------------------------------------------------------- 1 | function [Xj,S] = jcalc( jtyp, q ) 2 | 3 | % jcalc joint transform and motion subspace matrices. 4 | % [Xj,S]=jcalc(type,q) returns the joint transform and motion subspace 5 | % matrices for a joint of the given type. jtyp can be either a string or a 6 | % structure containing a string-valued field called 'code'. Either way, 7 | % the string contains the joint type code. For joints that take 8 | % parameters (e.g. helical), jtyp must be a structure, and it must contain 9 | % a field called 'pars', which in turn is a structure containing one or 10 | % more parameters. (For a helical joint, pars contains a parameter called 11 | % 'pitch'.) q is the joint's position variable. 12 | 13 | if ischar( jtyp ) 14 | code = jtyp; 15 | else 16 | code = jtyp.code; 17 | end 18 | 19 | switch code 20 | case 'Rx' % revolute X axis 21 | Xj = rotx(q); 22 | S = [1;0;0;0;0;0]; 23 | case 'Ry' % revolute Y axis 24 | Xj = roty(q); 25 | S = [0;1;0;0;0;0]; 26 | case {'R','Rz'} % revolute Z axis 27 | Xj = rotz(q); 28 | S = [0;0;1;0;0;0]; 29 | case 'Px' % prismatic X axis 30 | Xj = xlt([q 0 0]); 31 | S = [0;0;0;1;0;0]; 32 | case 'Py' % prismatic Y axis 33 | Xj = xlt([0 q 0]); 34 | S = [0;0;0;0;1;0]; 35 | case {'P','Pz'} % prismatic Z axis 36 | Xj = xlt([0 0 q]); 37 | S = [0;0;0;0;0;1]; 38 | case 'H' % helical (Z axis) 39 | Xj = rotz(q) * xlt([0 0 q*jtyp.pars.pitch]); 40 | S = [0;0;1;0;0;jtyp.pars.pitch]; 41 | case 'r' % planar revolute 42 | Xj = plnr( q, [0 0] ); 43 | S = [1;0;0]; 44 | case 'px' % planar prismatic X axis 45 | Xj = plnr( 0, [q 0] ); 46 | S = [0;1;0]; 47 | case 'py' % planar prismatic Y axis 48 | Xj = plnr( 0, [0 q] ); 49 | S = [0;0;1]; 50 | otherwise 51 | error( 'unrecognised joint code ''%s''', code ); 52 | end 53 | -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/FDab.m: -------------------------------------------------------------------------------- 1 | function qdd = FDab( model, q, qd, tau, f_ext ) 2 | 3 | % FDab Forward Dynamics via Articulated-Body Algorithm 4 | % FDab(model,q,qd,tau,f_ext,grav_accn) calculates the forward dynamics of 5 | % a kinematic tree via the articulated-body algorithm. q, qd and tau are 6 | % vectors of joint position, velocity and force variables; and the return 7 | % value is a vector of joint acceleration variables. f_ext is an optional 8 | % argument specifying the external forces acting on the bodies. It can be 9 | % omitted if there are no external forces. The format of f_ext is 10 | % explained in the source code of apply_external_forces. 11 | 12 | a_grav = get_gravity(model); 13 | 14 | for i = 1:model.NB 15 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q(i) ); 16 | vJ = S{i}*qd(i); 17 | Xup{i} = XJ * model.Xtree{i}; 18 | if model.parent(i) == 0 19 | v{i} = vJ; 20 | c{i} = zeros(size(a_grav)); % spatial or planar zero vector 21 | else 22 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 23 | c{i} = crm(v{i}) * vJ; 24 | end 25 | IA{i} = model.I{i}; 26 | pA{i} = crf(v{i}) * model.I{i} * v{i}; 27 | end 28 | 29 | if nargin == 5 30 | pA = apply_external_forces( model.parent, Xup, pA, f_ext ); 31 | end 32 | 33 | for i = model.NB:-1:1 34 | U{i} = IA{i} * S{i}; 35 | d{i} = S{i}' * U{i}; 36 | u{i} = tau(i) - S{i}'*pA{i}; 37 | if model.parent(i) ~= 0 38 | Ia = IA{i} - U{i}/d{i}*U{i}'; 39 | pa = pA{i} + Ia*c{i} + U{i} * u{i}/d{i}; 40 | IA{model.parent(i)} = IA{model.parent(i)} + Xup{i}' * Ia * Xup{i}; 41 | pA{model.parent(i)} = pA{model.parent(i)} + Xup{i}' * pa; 42 | end 43 | end 44 | 45 | for i = 1:model.NB 46 | if model.parent(i) == 0 47 | a{i} = Xup{i} * -a_grav + c{i}; 48 | else 49 | a{i} = Xup{i} * a{model.parent(i)} + c{i}; 50 | end 51 | qdd(i,1) = (u{i} - U{i}'*a{i})/d{i}; 52 | a{i} = a{i} + S{i}*qdd(i); 53 | end 54 | -------------------------------------------------------------------------------- /Examples/BouncingBall/Figure/compare_hsddp2saltation.m: -------------------------------------------------------------------------------- 1 | function compare_hsddp2saltation(hsddp_data) 2 | folderName = 'BouncingBall/Data/Nathan/'; 3 | fileName = 'saltation_matrix_trajectory_1_bounce.mat'; 4 | u_struct = load(strcat(folderName, fileName),'u_trj'); 5 | x_struct = load(strcat(folderName, fileName),'x_trj'); 6 | K_struct = load(strcat(folderName, fileName),'K_trj'); 7 | u_sal = u_struct.u_trj; 8 | x_sal = x_struct.x_trj; 9 | K_sal = K_struct.K_trj; 10 | 11 | x_hsddp = []; 12 | u_hsddp = []; 13 | for i = 1:length(hsddp_data.xopt) 14 | x_traj_i = cell2mat(hsddp_data.xopt{i}); 15 | len = length(hsddp_data.xopt{i}); 16 | if i < length(hsddp_data.xopt) 17 | len = length(hsddp_data.xopt{i}) - 1; 18 | end 19 | x_hsddp = [x_hsddp, x_traj_i(:,1:len)]; 20 | u_hsddp = [u_hsddp,cell2mat(hsddp_data.uopt{i})]; 21 | end 22 | x_hsddp = x_hsddp'; 23 | u_hsddp = u_hsddp'; 24 | Kopt = hsddp_data.Kopt; 25 | Kopt{1}(end) = []; 26 | Kopt{2}(end) = []; 27 | Kopt = cat(2,Kopt{:}); 28 | K_hsddp = cell2mat(Kopt(:)); 29 | K_sal = cell2mat(K_sal(:)); 30 | 31 | %% 32 | t = (0:999)*hsddp_data.dt; 33 | figure 34 | subplot(3,1,1) 35 | plot(t, x_sal(:,1)); 36 | hold on 37 | plot(t, x_hsddp(:,1),'--'); 38 | xlabel('time (s)'); 39 | ylabel('z (m)'); 40 | legend('Saltation', 'HSDDP'); 41 | 42 | subplot(3,1,2) 43 | plot(t, x_sal(:,2)); 44 | hold on; 45 | plot(t, x_hsddp(:,2),'--'); 46 | xlabel('time (s)'); 47 | ylabel('zd (m/s)'); 48 | 49 | subplot(3,1,3) 50 | plot(t(1:end-1), u_sal); 51 | hold on; 52 | plot(t(1:end-1), u_hsddp,'--'); 53 | xlabel('time (s)'); 54 | ylabel('u (N)'); 55 | 56 | figure 57 | subplot(2,1,1) 58 | plot(t(1:end-1), K_sal(:,1)); 59 | hold on 60 | plot(t(1:end-1), K_hsddp(:,1),'--'); 61 | xlabel('time (s)'); 62 | ylabel('K1') 63 | subplot(2,1,2) 64 | plot(t(1:end-1), K_sal(:,2)); 65 | hold on 66 | plot(t(1:end-1), K_hsddp(:,2),'--'); 67 | xlabel('time (s)'); 68 | ylabel('K2'); 69 | legend('Saltation', 'HSDDP'); 70 | 71 | end -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/apply_external_forces.m: -------------------------------------------------------------------------------- 1 | function f_out = apply_external_forces( parent, Xup, f_in, f_ext ) 2 | 3 | % apply_external_forces subtract f_ext from a given cell array of forces 4 | % f_out=apply_external_forces(parent,Xup,f_in,f_ext) incorporates the 5 | % external forces specified in f_ext into the calculations of a dynamics 6 | % algorithm. It does this by subtracting the contents of f_ext from an 7 | % array of forces supplied by the calling function (f_in) and returning the 8 | % result. f_ext has the following format: (1) it must either be an empty 9 | % cell array, indicating that there are no external forces, or else a cell 10 | % array containing NB elements such that f_ext{i} is the external force 11 | % acting on body i; (2) f_ext{i} must either be an empty array, indicating 12 | % that there is no external force acting on body i, or else a spatial or 13 | % planar vector (as appropriate) giving the external force expressed in 14 | % absolute coordinates. apply_external_forces performs the calculation 15 | % f_out = f_in - transformed f_ext, where f_out and f_in are cell arrays of 16 | % forces expressed in link coordinates; so f_ext has to be transformed to 17 | % link coordinates before use. The arguments parent and Xup contain the 18 | % parent array and link-to-link coordinate transforms for the model to 19 | % which the forces apply, and are used to work out the coordinate 20 | % transforms. 21 | 22 | % Note: the possibility exists to allow various formats for f_ext; 23 | % e.g. 6/3xNB matrix, or structure with shortened cell array f_ext and a 24 | % list of body numbers (f_ext{i} applies to body b(i)) 25 | 26 | f_out = f_in; 27 | 28 | if length(f_ext) > 0 29 | for i = 1:length(parent) 30 | if parent(i) == 0 31 | Xa{i} = Xup{i}; 32 | else 33 | Xa{i} = Xup{i} * Xa{parent(i)}; 34 | end 35 | if length(f_ext{i}) > 0 36 | f_out{i} = f_out{i} - Xa{i}' \ f_ext{i}; 37 | end 38 | end 39 | end 40 | -------------------------------------------------------------------------------- /Common/spatial_v2/models/scissor.m: -------------------------------------------------------------------------------- 1 | function model = scissor 2 | 3 | % SCISSOR free-floating two-body system resembling a pair of scissors 4 | % scissor creates a model data structure for a free-floating two body 5 | % system consisting of two identical flat rectangular bodies connected 6 | % together at the middle by a revolute joint in a manner resembling a pair 7 | % of scissors. In the zero position, both bodies extend from -0.1 to 0.1 8 | % in the x direction, and from -0.5 to 0.5 in the z direction, but one 9 | % extends from 0 to -0.1 and the other from 0 to +0.1 in the y direction, 10 | % and the connecting joint lies on the y axis. Both bodies have unit 11 | % mass. This mechanism is used in Simulink Example 5. 12 | 13 | % For efficient use with Simulink, this function builds a new model only 14 | % the first time it is called. Thereafter, it returns this stored copy. 15 | 16 | persistent memory; 17 | 18 | if length(memory) ~= 0 19 | model = memory; 20 | return 21 | end 22 | 23 | model.NB = 2; 24 | model.parent = [0 1]; 25 | 26 | model.jtype = {'R', 'Ry'}; % 1st joint will be replaced by floatbase 27 | model.Xtree = {eye(6), eye(6)}; 28 | 29 | model.gravity = [0 0 0]; % zero gravity is not the default, 30 | % so it must be stated explicitly 31 | 32 | % centroidal inertia of a unit-mass box with dimensions 0.2, 0.1 and 1: 33 | 34 | Ic = diag( [1.01 1.04 0.05] / 12 ); 35 | 36 | model.I = { mcI(1,[0 -0.05 0],Ic), mcI(1,[0 0.05 0],Ic) }; 37 | 38 | % Draw the bodies 39 | 40 | model.appearance.body{1} = ... 41 | { 'box', [-0.1 -0.1 -0.5; 0.1 0 0.5], ... 42 | 'colour', [0.6 0.3 0.8], ... 43 | 'cyl', [0 -0.13 0; 0 0.13 0], 0.07 }; 44 | 45 | model.appearance.body{2} = ... 46 | { 'box', [-0.1 0 -0.5; 0.1 0.1 0.5] }; 47 | 48 | model.camera.direction = [0 -3 1]; % a better viewing angle 49 | model.camera.zoom = 0.9; 50 | 51 | model = floatbase(model); % replace joint 1 with a chain of 6 52 | % joints emulating a floating base 53 | memory = model; 54 | -------------------------------------------------------------------------------- /HSDDP/Trajectory.m: -------------------------------------------------------------------------------- 1 | classdef Trajectory < handle 2 | properties 3 | len 4 | Ubar 5 | Xbar 6 | U 7 | X 8 | Y 9 | G 10 | H 11 | K 12 | dU 13 | V 14 | dV 15 | l 16 | lpar 17 | phi 18 | phi_par 19 | A 20 | B 21 | C 22 | D 23 | Px 24 | dt 25 | end 26 | 27 | methods 28 | function T = Trajectory(xsize, usize, ysize, N_horizon, dt) 29 | T.len = N_horizon; 30 | T.Ubar = repmat({zeros(usize, 1)}, 1, N_horizon - 1); 31 | T.Xbar = repmat({zeros(xsize, 1)}, 1, N_horizon); 32 | T.U = repmat({zeros(usize, 1)}, 1, N_horizon - 1); 33 | T.dU = repmat({zeros(usize, 1)}, 1, N_horizon); 34 | T.X = repmat({zeros(xsize, 1)}, 1, N_horizon); 35 | T.Y = repmat({zeros(ysize, 1)}, 1, N_horizon); 36 | T.G = repmat({zeros(xsize, 1)}, 1, N_horizon); 37 | T.H = repmat({zeros(xsize, xsize)}, 1, N_horizon); 38 | T.K = repmat({zeros(usize, xsize)}, 1, N_horizon); 39 | T.A = repmat({zeros(xsize, xsize)}, 1, N_horizon); 40 | T.B = repmat({zeros(xsize, usize)}, 1, N_horizon); 41 | T.C = repmat({zeros(ysize, xsize)}, 1, N_horizon); 42 | T.D = repmat({zeros(ysize, usize)}, 1, N_horizon); 43 | T.Px = []; 44 | T.l = repmat({0}, 1, N_horizon - 1); 45 | T.lpar = repmat({RCostPartialData(xsize, usize, ysize)}, 1, N_horizon - 1); 46 | T.phi = 0; 47 | T.phi_par = TCostPartialData(xsize); 48 | T.V = 0; 49 | T.dV = 0; 50 | T.dt = dt; 51 | end 52 | 53 | function updateNominal(T) 54 | T.Xbar = T.X; 55 | T.Ubar = T.U; 56 | T.dU = repmat({zeros(size(T.dU{1}))}, 1, length(T.dU)); 57 | end 58 | end 59 | end -------------------------------------------------------------------------------- /Examples/Quadurped/Controllers/HeuristicPD/heuristic_bounding_controller.m: -------------------------------------------------------------------------------- 1 | function heuristic_bounding_controller(hybridT, problem) 2 | nWBPhase = problem.nWBPhase; 3 | phaseSeq = problem.phaseSeq; 4 | if nWBPhase <= 0 5 | fprintf('No whole-body trajectory is found. May need to check problem setup \n'); 6 | fprintf('Heuristic controller is not executed \n'); 7 | return 8 | end 9 | % Build quadruped model 10 | model = PlanarQuadruped(problem.dt); 11 | build2DminiCheetah(model); 12 | % Get a library of modes for bounding 13 | boundLib = BoundingModeLib(); 14 | %% flight phase reference angles 15 | the_ref = [pi/4,-pi*7/12,pi/4,-pi*7/12]'; 16 | NomSpringLen = 0.2462; 17 | kp = 5*diag([8,1,12,10]); 18 | kd = 1*diag([1,1,1,1]); 19 | Kspring = 2200; % spring stiffness 20 | 21 | 22 | for idx = 1:nWBPhase 23 | mode = phaseSeq(idx).mode; 24 | nmode = boundLib.getNextMode(phaseSeq(idx)).mode; 25 | if mode == 1 %bs 26 | scale = 3; 27 | legid = 2; 28 | elseif mode == 3 %fs 29 | scale = 2.2; 30 | legid = 1; 31 | end 32 | for k = 1:hybridT(idx).len - 1 33 | xk = hybridT(idx).Xbar{k}; 34 | qk = xk(1:7,1); 35 | switch mode 36 | case {1,3} 37 | [Jc,~] = model.getFootJacobian(xk, mode); 38 | Jc(:,1:3) = []; 39 | v = getSpringVec(qk,model,legid); 40 | Fk = -v/norm(v)*Kspring*(norm(v)-NomSpringLen); 41 | uk = Jc'*Fk*scale; 42 | 43 | case {2,4} 44 | the_k = xk(4:7,1); 45 | thed_k = xk(11:end,1); 46 | uk = kp*(the_ref-the_k) - kd*thed_k; % first flight 47 | end 48 | [xk_next, ~] = model.dynamics(xk, uk, mode); 49 | hybridT(idx).Xbar{k+1} = xk_next; 50 | hybridT(idx).Ubar{k} = uk; 51 | end 52 | if idx < nWBPhase 53 | xk_next = model.resetmap(hybridT(idx).Xbar{end}, nmode); 54 | hybridT(idx+1).Xbar{1} = xk_next; 55 | end 56 | end 57 | end 58 | 59 | -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/Constraint/support/Back_TouchDown_Constraint.m: -------------------------------------------------------------------------------- 1 | function [h_FL2,hx_FL2,hxx_FL2] = WB_FL2_terminal_constr(in1) 2 | %WB_FL2_TERMINAL_CONSTR 3 | % [H_FL2,HX_FL2,HXX_FL2] = WB_FL2_TERMINAL_CONSTR(IN1) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 01-Dec-2020 17:20:42 7 | 8 | q2 = in1(2,:); 9 | q3 = in1(3,:); 10 | q6 = in1(6,:); 11 | q7 = in1(7,:); 12 | t2 = sin(q3); 13 | t3 = cos(q3); 14 | t4 = cos(q6); 15 | t5 = sin(q6); 16 | t6 = cos(q7); 17 | t7 = t3.*t5; 18 | t8 = t2.*t4; 19 | t9 = t7+t8; 20 | t10 = sin(q7); 21 | t11 = t2.*t5; 22 | t16 = t3.*t4; 23 | t12 = t11-t16; 24 | t13 = t3.*t5.*(2.09e2./1.0e3); 25 | t14 = t2.*t4.*(2.09e2./1.0e3); 26 | t15 = t6.*t9.*(3.9e1./2.0e2); 27 | t17 = t2.*(1.9e1./1.0e2); 28 | t18 = t2.*t5.*(2.09e2./1.0e3); 29 | t19 = t6.*t12.*(3.9e1./2.0e2); 30 | t20 = t9.*t10.*(3.9e1./2.0e2); 31 | h_FL2 = q2+t17+t18+t19+t20-t3.*t4.*(2.09e2./1.0e3)+1.01e2./2.5e2; 32 | if nargout > 1 33 | hx_FL2 = [0.0,1.0,t3.*(1.9e1./1.0e2)+t13+t14+t15-t10.*t12.*(3.9e1./2.0e2),0.0,0.0,t13+t14+t15-t10.*t12.*(3.9e1./2.0e2),t15-t10.*t12.*(3.9e1./2.0e2),0.0,0.0,0.0,0.0,0.0,0.0,0.0]; 34 | end 35 | if nargout > 2 36 | t21 = t3.*t4.*(2.09e2./1.0e3); 37 | t22 = -t18-t19-t20+t21; 38 | t23 = -t19-t20; 39 | hxx_FL2 = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-t17-t18-t19-t20+t21,0.0,0.0,t22,t23,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t22,0.0,0.0,t22,t23,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t23,0.0,0.0,t23,t23,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[14,14]); 40 | end 41 | -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/Constraint/support/Front_TouchDown_Constraint.m: -------------------------------------------------------------------------------- 1 | function [h_FL1,hx_FL1,hxx_FL1] = WB_FL1_terminal_constr(in1) 2 | %WB_FL1_TERMINAL_CONSTR 3 | % [H_FL1,HX_FL1,HXX_FL1] = WB_FL1_TERMINAL_CONSTR(IN1) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 01-Dec-2020 17:20:41 7 | 8 | q2 = in1(2,:); 9 | q3 = in1(3,:); 10 | q4 = in1(4,:); 11 | q5 = in1(5,:); 12 | t2 = sin(q3); 13 | t3 = cos(q3); 14 | t4 = cos(q4); 15 | t5 = sin(q4); 16 | t6 = cos(q5); 17 | t7 = t3.*t5; 18 | t8 = t2.*t4; 19 | t9 = t7+t8; 20 | t10 = sin(q5); 21 | t11 = t2.*t5; 22 | t16 = t3.*t4; 23 | t12 = t11-t16; 24 | t13 = t3.*t5.*(2.09e2./1.0e3); 25 | t14 = t2.*t4.*(2.09e2./1.0e3); 26 | t15 = t6.*t9.*(3.9e1./2.0e2); 27 | t17 = t2.*t5.*(2.09e2./1.0e3); 28 | t18 = t6.*t12.*(3.9e1./2.0e2); 29 | t19 = t9.*t10.*(3.9e1./2.0e2); 30 | h_FL1 = q2-t2.*(1.9e1./1.0e2)+t17+t18+t19-t3.*t4.*(2.09e2./1.0e3)+1.01e2./2.5e2; 31 | if nargout > 1 32 | hx_FL1 = [0.0,1.0,t3.*(-1.9e1./1.0e2)+t13+t14+t15-t10.*t12.*(3.9e1./2.0e2),t13+t14+t15-t10.*t12.*(3.9e1./2.0e2),t15-t10.*t12.*(3.9e1./2.0e2),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]; 33 | end 34 | if nargout > 2 35 | t20 = t3.*t4.*(2.09e2./1.0e3); 36 | t21 = -t17-t18-t19+t20; 37 | t22 = -t18-t19; 38 | hxx_FL1 = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t2.*(1.9e1./1.0e2)-t17-t18-t19+t20,t21,t22,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t21,t21,t22,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t22,t22,t22,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[14,14]); 39 | end 40 | -------------------------------------------------------------------------------- /Common/spatial_v2/models/rollingdisc.m: -------------------------------------------------------------------------------- 1 | function model = rollingdisc( npt ) 2 | 3 | % rollingdisc disc with ground contact points (Simulink Example 7) 4 | % rollingdisc(npt) creates a model data structure for a polyhedral 5 | % approximation to a thick circular disc (i.e., a cylinder) that can make 6 | % contact with, bounce, and roll over the ground (the x-y plane). The 7 | % argument is the number of vertices in the polygon approximating a circle. 8 | % The model contains 2*npt ground-contact points -- npt around each end 9 | % face of the cylinder. See Simulink Example 7. 10 | 11 | % For efficient use with Simulink, this function builds a new model only 12 | % if it doesn't have a stored copy of the right model. 13 | 14 | persistent last_model last_npt; 15 | 16 | if length(last_model) ~= 0 && last_npt == npt 17 | model = last_model; 18 | return 19 | end 20 | 21 | model.NB = 1; 22 | model.parent = 0; 23 | 24 | model.jtype = {'R'}; % sacrificial joint replaced by floatbase 25 | model.Xtree = {eye(6)}; 26 | 27 | % Disc dimensions and inertia 28 | 29 | R = 0.05; % radius 30 | T = 0.01; % thickness 31 | 32 | density = 3000; % kg / m^3 33 | mass = pi * R^2 * T * density; 34 | Ibig = mass * R^2 / 2; 35 | Ismall = mass * (3*R^2 + T^2) / 12; 36 | 37 | model.I = { mcI( mass, [0 0 0], diag([Ismall Ibig Ismall]) ) }; 38 | 39 | % Drawing instructions 40 | 41 | model.appearance.base = { 'tiles', [-0.1 1; -0.3 0.3; 0 0], 0.1 }; 42 | 43 | model.appearance.body{1} = { 'facets', npt, 'cyl', [0 -T/2 0; 0 T/2 0], R }; 44 | 45 | % Camera settings 46 | 47 | model.camera.body = 1; 48 | model.camera.direction = [1 0 0.1]; 49 | model.camera.locus = [0 0.5]; 50 | 51 | % Ground contact points (have to match how the cylinder is drawn) 52 | 53 | ang = (0:npt-1) * 2*pi / npt; 54 | 55 | Y = ones(1,npt) * T/2; 56 | X = sin(ang) * R; 57 | Z = cos(ang) * R; 58 | 59 | model.gc.point = [ X X; -Y Y; Z Z ]; 60 | model.gc.body = ones(1,2*npt); 61 | 62 | % Final step: float the base 63 | 64 | model = floatbase(model); % replace joint 1 with a chain of 6 65 | % joints emulating a floating base 66 | last_model = model; 67 | last_npt = npt; 68 | -------------------------------------------------------------------------------- /Examples/Quadurped/Kinematics/Link1Jacobian_par.m: -------------------------------------------------------------------------------- 1 | function [Jx,Jdx] = Link1Jacobian_par(in1,in2) 2 | %LINK1JACOBIAN_PAR 3 | % [JX,JDX] = LINK1JACOBIAN_PAR(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 29-Nov-2020 17:42:38 7 | 8 | p1_1 = in2(1); 9 | p2_1 = in2(2); 10 | q3 = in1(3,:); 11 | qd3 = in1(10,:); 12 | t2 = conj(p2_1); 13 | t3 = cos(q3); 14 | t4 = conj(p1_1); 15 | t5 = sin(q3); 16 | t6 = t4.*t5; 17 | t8 = t3.*t4; 18 | t9 = t2.*t5; 19 | t7 = -t8-t9; 20 | t10 = t6-t2.*t3; 21 | Jx = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t7,t10,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2,7,14]); 22 | if nargout > 1 23 | Jdx = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,qd3.*t10,qd3.*(t8+t9),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t7,t10,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2,7,14]); 24 | end 25 | -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/HandC.m: -------------------------------------------------------------------------------- 1 | function [H,C] = HandC( model, q, qd, f_ext ) 2 | 3 | % HandC Calculate coefficients of equation of motion 4 | % [H,C]=HandC(model,q,qd,f_ext) calculates the coefficients of the 5 | % joint-space equation of motion, tau=H(q)qdd+C(d,qd,f_ext), where q, qd 6 | % and qdd are the joint position, velocity and acceleration vectors, H is 7 | % the joint-space inertia matrix, C is the vector of gravity, 8 | % external-force and velocity-product terms, and tau is the joint force 9 | % vector. Algorithm: recursive Newton-Euler for C, and 10 | % Composite-Rigid-Body for H. f_ext is an optional argument specifying the 11 | % external forces acting on the bodies. It can be omitted if there are no 12 | % external forces. The format of f_ext is explained in the source code of 13 | % apply_external_forces. 14 | 15 | a_grav = get_gravity(model); 16 | 17 | for i = 1:model.NB 18 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q(i) ); 19 | vJ = S{i}*qd(i); 20 | Xup{i} = XJ * model.Xtree{i}; 21 | if model.parent(i) == 0 22 | v{i} = vJ; 23 | avp{i} = Xup{i} * -a_grav; 24 | else 25 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 26 | avp{i} = Xup{i}*avp{model.parent(i)} + crm(v{i})*vJ; 27 | end 28 | fvp{i} = model.I{i}*avp{i} + crf(v{i})*model.I{i}*v{i}; 29 | end 30 | 31 | if nargin == 4 32 | fvp = apply_external_forces( model.parent, Xup, fvp, f_ext ); 33 | end 34 | 35 | for i = model.NB:-1:1 36 | C(i,1) = S{i}' * fvp{i}; 37 | if model.parent(i) ~= 0 38 | fvp{model.parent(i)} = fvp{model.parent(i)} + Xup{i}'*fvp{i}; 39 | end 40 | end 41 | 42 | IC = model.I; % composite inertia calculation 43 | 44 | for i = model.NB:-1:1 45 | if model.parent(i) ~= 0 46 | IC{model.parent(i)} = IC{model.parent(i)} + Xup{i}'*IC{i}*Xup{i}; 47 | end 48 | end 49 | 50 | % original code does not handle symbolic operation 51 | if isnumeric(q) 52 | H = zeros(model.NB); 53 | else 54 | H = sym(zeros(model.NB)); 55 | end 56 | 57 | 58 | for i = 1:model.NB 59 | fh = IC{i} * S{i}; 60 | H(i,i) = S{i}' * fh; 61 | j = i; 62 | while model.parent(j) > 0 63 | fh = Xup{j}' * fh; 64 | j = model.parent(j); 65 | H(i,j) = S{j}' * fh; 66 | H(j,i) = H(i,j); 67 | end 68 | end 69 | -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/BoundingReference.m: -------------------------------------------------------------------------------- 1 | classdef BoundingReference < handle 2 | properties 3 | hybridR % structure array of trajectories 4 | GRF 5 | end 6 | methods 7 | function R = BoundingReference(hybridR_in) 8 | R.hybridR = hybridR_in; 9 | R.GRF = [0, 80.9521]'; 10 | end 11 | 12 | function update(R, x0, problem) 13 | dt = problem.dt; 14 | vd = problem.vd; 15 | hd = problem.hd; 16 | assert(length(R.hybridR) == problem.nPhase, 'Hybrid trajectory of phase object array have unmatched dimension'); 17 | wbstate{1} = [[0,-0.1432,-pi/25,0.35*pi,-0.65*pi,0.35*pi,-0.6*pi]'; 18 | vd;1;zeros(5,1)]; 19 | wbstate{2} = [[0,-0.1418,pi/35,0.2*pi,-0.58*pi,0.25*pi,-0.7*pi]'; 20 | vd;-1;zeros(5,1)]; 21 | wbstate{3} = [[0,-0.1325,-pi/40,0.33*pi,-0.48*pi,0.33*pi,-0.75*pi]'; 22 | vd;1;zeros(5,1)]; 23 | wbstate{4} = [[0,-0.1490,-pi/25,0.35*pi,-0.7*pi,0.25*pi,-0.60*pi]'; 24 | vd;-1;zeros(5,1)]; 25 | k = 1; 26 | for i = 1:length(R.hybridR) 27 | for j = 1:R.hybridR(i).len 28 | R.hybridR(i).xd{j}(1) = x0(1) + (k - 1)*dt*vd; 29 | R.hybridR(i).xd{j}(2) = hd; 30 | if i <= problem.nWBPhase 31 | R.hybridR(i).xd{j}(4:7) = [0.3*pi,-0.7*pi,0.3*pi,-0.7*pi]'; 32 | R.hybridR(i).xd{j}(8) = vd; 33 | R.hybridR(i).yd{j} = [R.GRF;R.GRF]; 34 | else 35 | R.hybridR(i).xd{j}(4) = vd; 36 | R.hybridR(i).ud{j} =[R.GRF;R.GRF]; 37 | end 38 | if j < R.hybridR(i).len 39 | k = k + 1; 40 | end 41 | end 42 | if i <= problem.nWBPhase 43 | R.hybridR(i).xd{end} = wbstate{problem.phaseSeq(i).mode}; 44 | end 45 | end 46 | end 47 | end 48 | end -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/Cost/FBCost.m: -------------------------------------------------------------------------------- 1 | classdef FBCost < handle 2 | properties 3 | dt 4 | ref 5 | Q 6 | R 7 | S 8 | Qf 9 | end 10 | methods 11 | function C = FBCost(dt) 12 | C.dt = dt; 13 | C.Q{1} = 0.01*diag([0,10,5, 2,1,0.01]); 14 | C.R{1} = blkdiag(zeros(2),0.01*eye(2)); 15 | C.S{1} = zeros(4); 16 | C.Qf{1} = 100*diag([1,20,8, 3,1,0.01]); 17 | 18 | % FL1 19 | C.Q{2} = 0.01*diag([0,10,5, 2,1,0.01]); 20 | C.R{2} = zeros(4); 21 | C.S{2} = zeros(4); 22 | C.Qf{2} = 100*diag([1,20,8, 3,1,0.01]); 23 | 24 | % FS 25 | C.Q{3} = 0.01*diag([0,10,5, 2,1,0.01]); 26 | C.R{3} = blkdiag(0.01*eye(2), zeros(2)); 27 | C.S{3} = zeros(4); 28 | C.Qf{3} = 100*diag([1,20,8, 3,1,0.01]); 29 | 30 | % FL2 31 | C.Q{4} = 0.01*diag([0,10,5, 2,1,0.01]); 32 | C.R{4} = zeros(4); 33 | C.S{4} = zeros(4); 34 | C.Qf{4} = 100*diag([1,20,8, 3,1,0.01]); 35 | end 36 | end 37 | methods 38 | function l = running_cost(C, k, x,u,y,mode,r) 39 | l = wsquare(x - r.xd{k}, C.Q{mode}); 40 | l = l + wsquare(u - r.ud{k}, C.R{mode}); 41 | l = l + wsquare(y - r.yd{k}, C.S{mode}); 42 | l = l * C.dt; 43 | end 44 | function lpar = running_cost_partial(C, k, x, u, y, mode,r) 45 | lpar.lx = 2 * C.dt * C.Q{mode} * (x-r.xd{k}); % column vec 46 | lpar.lu = 2 * C.dt * C.R{mode} * (u-r.ud{k}); 47 | lpar.ly = 2 * C.dt * C.S{mode} * (y-r.yd{k}); 48 | lpar.lxx = 2 * C.dt * C.Q{mode}; 49 | lpar.lux = zeros(length(u),length(x)); 50 | lpar.luu = 2 * C.dt * C.R{mode}; 51 | lpar.lyy = 2 * C.dt * C.S{mode}; 52 | end 53 | end 54 | 55 | methods 56 | function phi = terminal_cost(C, x, mode,r) 57 | phi = 0.5*wsquare(x - r.xd{end}, C.Qf{mode}); 58 | end 59 | function phi_par = terminal_cost_partial(C, x, mode,r) 60 | phi_par.G = C.Qf{mode} * (x - r.xd{end}); 61 | phi_par.H = C.Qf{mode}; 62 | end 63 | end 64 | end -------------------------------------------------------------------------------- /Common/spatial_v2/3D/rq.m: -------------------------------------------------------------------------------- 1 | function out = rq( in ) 2 | 3 | % rq unit quaternion <--> 3x3 coordinate rotation matrix 4 | % E=rq(q) and q=rq(E) convert between a unit quaternion q, representing 5 | % the orientation of a coordinate frame B relative to frame A, and the 3x3 6 | % coordinate rotation matrix E that transforms from A to B coordinates. 7 | % For example, if B is rotated relative to A about their common X axis by 8 | % an angle h, then q=[cos(h/2);sin(h/2);0;0] and rq(q) produces the same 9 | % matrix as rx(h). If the argument is a 3x3 matrix then it is assumed to 10 | % be E, otherwise it is assumed to be q. rq(E) expects E to be accurately 11 | % orthonormal, and returns a quaternion in a 4x1 matrix; but rq(q) accepts 12 | % any nonzero quaternion, contained in either a row or a column vector, and 13 | % normalizes it before use. As both q and -q represent the same rotation, 14 | % rq(E) returns the value that satisfies q(1)>0. If q(1)==0 then it picks 15 | % the value such that the largest-magnitude element is positive. In the 16 | % event of a tie, the smaller index wins. 17 | 18 | if all(size(in)==[3 3]) 19 | out = Etoq( in ); 20 | else 21 | out = qtoE( in ); 22 | end 23 | 24 | 25 | function E = qtoE( q ) 26 | 27 | q = q / norm(q); 28 | 29 | q0s = q(1)*q(1); 30 | q1s = q(2)*q(2); 31 | q2s = q(3)*q(3); 32 | q3s = q(4)*q(4); 33 | q01 = q(1)*q(2); 34 | q02 = q(1)*q(3); 35 | q03 = q(1)*q(4); 36 | q12 = q(2)*q(3); 37 | q13 = q(4)*q(2); 38 | q23 = q(3)*q(4); 39 | 40 | E = 2 * [ q0s+q1s-0.5, q12 + q03, q13 - q02; 41 | q12 - q03, q0s+q2s-0.5, q23 + q01; 42 | q13 + q02, q23 - q01, q0s+q3s-0.5 ]; 43 | 44 | 45 | function q = Etoq( E ) 46 | 47 | % for sufficiently large q0, this function formulates 2*q0 times the 48 | % correct return value; otherwise, it formulates 4*|q1| or 4*|q2| or 4*|q3| 49 | % times the correct value. The final normalization step yields the correct 50 | % return value. 51 | 52 | tr = trace(E); % trace is 4*q0^2-1 53 | v = -skew(E); % v is 2*q0 * [q1;q2;q3] 54 | 55 | if tr > 0 56 | q = [ (tr+1)/2; v ]; 57 | else 58 | E = E - (tr-1)/2 * eye(3); 59 | E = E + E'; 60 | if E(1,1) >= E(2,2) && E(1,1) >= E(3,3) 61 | q = [ 2*v(1); E(:,1) ]; 62 | elseif E(2,2) >= E(3,3) 63 | q = [ 2*v(2); E(:,2) ]; 64 | else 65 | q = [ 2*v(3); E(:,3) ]; 66 | end 67 | if q(1) < 0 68 | q = -q; 69 | end 70 | end 71 | 72 | q = q / norm(q); 73 | -------------------------------------------------------------------------------- /Common/Gait.m: -------------------------------------------------------------------------------- 1 | classdef Gait < handle 2 | properties 3 | name 4 | gait 5 | basicTimings 6 | bounding_gait = [1,2,3,4] % one bounding gait cycle with unique phase index 7 | end 8 | 9 | properties % left for future use 10 | gait_Enabled 11 | 12 | period_time 13 | timeStance 14 | timeSwing 15 | timeStanceRemaining 16 | timeSwingRemaning 17 | 18 | switchingPhase 19 | phaseVariable 20 | phaseStance 21 | phaseSwing 22 | phaseScale 23 | phaseOffset 24 | 25 | contactStateScheduled 26 | contactStatePrev 27 | touchdownScheduled 28 | liftoffScheduled 29 | end 30 | methods 31 | function G = Gait(name) 32 | G.name = name; 33 | if strcmp(name, 'bounding') 34 | G.gait = G.bounding_gait; 35 | end 36 | end 37 | 38 | function phaseSeq = get_gaitSeq(G, currentPhase, n_Phases) 39 | phaseSeq = zeros(1,n_Phases); 40 | phaseSeq(1) = currentPhase; 41 | for i = 2:n_Phases 42 | phaseSeq(i) = G.get_nextPhase(phaseSeq(i-1)); 43 | end 44 | end 45 | 46 | function timeSeq = get_timeSeq(G, currentPhase, n_Phases) 47 | timeSeq = zeros(1, n_Phases); 48 | phaseSeq = G.get_gaitSeq(currentPhase, n_Phases); 49 | for pidx = 1:n_Phases 50 | timeSeq(pidx) = G.basicTimings(phaseSeq(pidx)==G.gait); 51 | end 52 | end 53 | 54 | function nextPhase = get_nextPhase(G, currentPhase) 55 | currentIdx = find(G.gait == currentPhase); 56 | if currentIdx == length(G.gait) 57 | nextPhase = G.gait(1); 58 | else 59 | nextPhase = G.gait(currentIdx+1); 60 | end 61 | end 62 | 63 | function setBasicTimings(G, timings) 64 | G.basicTimings = timings; 65 | end 66 | 67 | function define_your_gait(G, yourGait) 68 | C = unique(yourGait); 69 | if length(C) ~= length(yourGait) 70 | error('Phase index has to be unique in one gait cycle.') 71 | end 72 | G.gait = yourGait; 73 | end 74 | end 75 | end -------------------------------------------------------------------------------- /Common/spatial_v2/models/floatbase.m: -------------------------------------------------------------------------------- 1 | function fbmodel = floatbase( model ) 2 | 3 | % floatbase construct the floating-base equivalent of a fixed-base model 4 | % floatbase(model) converts a fixed-base spatial kinematic tree to a 5 | % floating-base kinematic tree as follows: old body 1 becomes new body 6, 6 | % and is regarded as the floating base; old joint 1 is discarded; six new 7 | % joints are added (three prismatic and three revolute, in that order, 8 | % arranged along the x, y and z axes, in that order); and five new 9 | % zero-mass bodies are added (numbered 1 to 5), to connect the new joints. 10 | % All other bodies and joints in the given model are preserved, but their 11 | % identification numbers are incremented by 5. The end result is that body 12 | % 6 has a full 6 degrees of motion freedom relative to the fixed base, with 13 | % joint variables 1 to 6 serving as a set of 3 Cartesian coordinates and 3 14 | % rotation angles (about x, y and z axes) specifying the position and 15 | % orientation of body 6's coordinate frame relative to the base coordinate 16 | % frame. CAUTION: A singularity occurs when q(5)=+-pi/2. If this is a 17 | % problem then use the special functions FDfb and IDfb instead of the 18 | % standard dynamics functions. floatbase requires that old joint 1 is the 19 | % only joint connected to the fixed base, and that Xtree{1} is the 20 | % identity. 21 | 22 | if size(model.Xtree{1},1) == 3 23 | error('floatbase applies to spatial models only'); 24 | end 25 | 26 | if any( model.parent(2:model.NB) == 0 ) 27 | error('only one connection to a fixed base allowed'); 28 | end 29 | 30 | if isfield( model, 'gamma_q' ) 31 | warning('floating a model with gamma_q (joint numbers will change)'); 32 | end 33 | 34 | if ~isequal( model.Xtree{1}, eye(6) ) 35 | warning('Xtree{1} not identity'); 36 | end 37 | 38 | fbmodel = model; 39 | 40 | fbmodel.NB = model.NB + 5; 41 | 42 | fbmodel.jtype = ['Px' 'Py' 'Pz' 'Rx' 'Ry' 'Rz' model.jtype(2:end)]; 43 | 44 | fbmodel.parent = [0 1 2 3 4 model.parent+5]; 45 | 46 | fbmodel.Xtree = [eye(6) eye(6) eye(6) eye(6) eye(6) model.Xtree]; 47 | 48 | fbmodel.I = [zeros(6) zeros(6) zeros(6) zeros(6) zeros(6) model.I]; 49 | 50 | if isfield( model, 'appearance' ) 51 | fbmodel.appearance.body = {{}, {}, {}, {}, {}, model.appearance.body{:}}; 52 | end 53 | 54 | if isfield( model, 'camera' ) && isfield( model.camera, 'body' ) 55 | if model.camera.body > 0 56 | fbmodel.camera.body = model.camera.body + 5; 57 | end 58 | end 59 | 60 | if isfield( model, 'gc' ) 61 | fbmodel.gc.body = model.gc.body + 5; 62 | end 63 | -------------------------------------------------------------------------------- /Examples/Quadurped/Dynamics/FB/PlanarFloatingBase.m: -------------------------------------------------------------------------------- 1 | classdef PlanarFloatingBase < BaseDyn 2 | properties (SetAccess=private, GetAccess=public) 3 | dt 4 | qsize = 3; 5 | xsize = 6; 6 | usize = 4; 7 | ysize = 4; 8 | p = zeros(4,1); % foothold location 9 | end 10 | 11 | properties % inertia params 12 | Inertia 13 | mass 14 | end 15 | 16 | methods 17 | % constructor 18 | function m = PlanarFloatingBase(dt) 19 | m.dt = dt; 20 | mc3D_param = get3DMCParams(); 21 | mc2D_param = get2DMCParams(mc3D_param); 22 | m.recomputeInertia(mc2D_param); 23 | end 24 | end 25 | 26 | methods 27 | function [x_next, y] = dynamics(m,x,u,s) 28 | s = s(:); 29 | fcon = FBDynamics(x,u,m.p,s); 30 | x_next = x + fcon * m.dt; 31 | y = u; 32 | end 33 | 34 | function [x_next, y] = resetmap(m,x) 35 | x_next = x; 36 | y = zeros(m.ysize, 1); 37 | end 38 | end 39 | 40 | methods 41 | function [A,B,C,D] = dynamics_par(m,x,u,s) 42 | s = s(:); 43 | [fconx,fconu] = FBDynamics_par(x,u,m.p,s); 44 | A = eye(m.xsize) + fconx*m.dt; 45 | B = fconu*m.dt; 46 | C = zeros(m.ysize, m.xsize); 47 | D = eye(m.usize); 48 | end 49 | 50 | function Px = resetmap_par(m,x) 51 | Px = eye(m.xsize); 52 | gx = zeros(m.ysize, m.xsize); 53 | end 54 | end 55 | 56 | methods 57 | function recomputeInertia(m, wbQuad) 58 | ihat = [1 0 0]'; 59 | 60 | m.mass = wbQuad.robotMass; 61 | 62 | dist_CoM_hip = dot(wbQuad.hipLoc{1} + (ry(-pi/2))'*wbQuad.hipLinkCoM, ihat); 63 | dist_CoM_knee = dist_CoM_hip + norm(wbQuad.kneeLinkCoM); 64 | 65 | eqv_hipRotInertia = wbQuad.hipRotInertia + wbQuad.hipLinkMass*dist_CoM_hip^2*0.85; 66 | eqv_kneeRotInertia = wbQuad.kneeRotInertia + wbQuad.kneeLinkMass*dist_CoM_knee^2*0.6; 67 | 68 | eqv_bodyRotInertia = wbQuad.bodyRotInertia + 2*eqv_hipRotInertia + 2*eqv_kneeRotInertia; 69 | m.Inertia = eqv_bodyRotInertia(2,2); 70 | end 71 | function set_foothold(m, p) 72 | m.p = p; 73 | end 74 | end 75 | end -------------------------------------------------------------------------------- /Examples/Quadurped/Kinematics/Link4Jacobian_par.m: -------------------------------------------------------------------------------- 1 | function [Jx,Jdx] = Link4Jacobian_par(in1,in2) 2 | %LINK4JACOBIAN_PAR 3 | % [JX,JDX] = LINK4JACOBIAN_PAR(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 29-Nov-2020 17:42:51 7 | 8 | p1_1 = in2(1); 9 | p2_1 = in2(2); 10 | q3 = in1(3,:); 11 | q6 = in1(6,:); 12 | qd3 = in1(10,:); 13 | qd6 = in1(13,:); 14 | t2 = cos(q3); 15 | t3 = sin(q6); 16 | t4 = cos(q6); 17 | t5 = sin(q3); 18 | t6 = conj(p1_1); 19 | t7 = t2.*t4; 20 | t13 = t3.*t5; 21 | t8 = t7-t13; 22 | t9 = conj(p2_1); 23 | t10 = t2.*t3; 24 | t11 = t4.*t5; 25 | t12 = t10+t11; 26 | t15 = t6.*t8; 27 | t16 = t9.*t12; 28 | t14 = -t15-t16; 29 | t17 = t6.*t12; 30 | t19 = t8.*t9; 31 | t18 = t17-t19; 32 | t20 = qd6.*t18; 33 | t21 = t2.*(1.9e1./1.0e2); 34 | Jx = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t21-t6.*t8-t9.*t12,t5.*(-1.9e1./1.0e2)+t17-t8.*t9,0.0,0.0,0.0,0.0,t14,t18,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t14,t18,0.0,0.0,0.0,0.0,t14,t18,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2,7,14]); 35 | if nargout > 1 36 | t22 = qd3.*t18; 37 | t23 = t20+t22; 38 | t24 = t15+t16; 39 | t25 = qd6.*t24; 40 | t26 = t5.*(1.9e1./1.0e2); 41 | t27 = qd3.*t24; 42 | t28 = t25+t27; 43 | Jdx = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t20-qd3.*(-t17+t19+t26),t25+qd3.*(t15+t16-t21),0.0,0.0,0.0,0.0,t23,t28,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t23,t28,0.0,0.0,0.0,0.0,t23,t28,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-t15-t16+t21,t17-t19-t26,0.0,0.0,0.0,0.0,t14,t18,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t14,t18,0.0,0.0,0.0,0.0,t14,t18,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2,7,14]); 44 | end 45 | -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/BoundingOptimization.m: -------------------------------------------------------------------------------- 1 | clear all 2 | clc 3 | % Model Hierarchy Trajectory Optimization Problem Setup 4 | boundingModeLib = BoundingModeLib(); 5 | problem.cPhase = boundingModeLib.lib(1); 6 | problem.nPhase = 8; 7 | problem.nWBPhase = 8; 8 | problem.nFBPhase = 0; 9 | problem.phaseSeq = boundingModeLib.genModeSequence(problem.cPhase, problem.nPhase); 10 | problem.vd = 1.5; 11 | problem.hd = -0.13; 12 | problem.dt = 0.001; 13 | % Set options of HSDDP solver 14 | options.alpha = 0.1; % linear search update param 15 | options.gamma = 0.01; % scale the expected cost reduction 16 | options.beta_penalty = 8; % penalty update param 17 | options.beta_relax = 0.1; % relaxation update param 18 | options.beta_reg = 2; % regularization update param 19 | options.beta_ReB = 7; 20 | options.max_DDP_iter = 5; % maximum DDP iterations 21 | options.max_AL_iter = 4; % maximum AL iterations 22 | options.DDP_thresh = 0.001; % Inner loop opt convergence threshold 23 | options.AL_thresh = 1e-3; % Outer loop opt convergence threshold 24 | options.tconstraint_active = 1; % Augmented Lagrangian active 25 | options.pconstraint_active = 1; % Reduced barrier active 26 | options.feedback_active = 1; 27 | options.smooth_active = 0; % Smoothness active 28 | options.parCalc_active = 1; % compute cost and dynamics partials in forward sweep (default) 29 | options.Debug = 1; % Debug active 30 | options.one_more_sweep = 1; % set penalty to zero, recalculate feedback 31 | 32 | %% Build Hybrid trajectory optimization problem 33 | % Set initial condition 34 | q0 = [0,-0.1093,-0.1542 1.0957 -2.2033 0.9742 -1.7098]'; 35 | qd0 = [0.9011 0.2756 0.7333 0.0446 0.0009 1.3219 2.7346]'; 36 | x0 = [q0;qd0]; 37 | 38 | % Create a bounding trajectory optimization problem 39 | [phases, hybridTraj, hybridRef] = createBoundingTask(problem); 40 | 41 | % Create reference trajectory generator 42 | refObj = BoundingReference(hybridRef); 43 | 44 | % Set trajectory initial condition 45 | hybridTraj(1).Xbar{1} = x0; 46 | hybridTraj(1).X{1} = x0; 47 | 48 | % Generate reference 49 | refObj.update(x0, problem); 50 | 51 | % Build solver 52 | hsddp = HSDDP(phases, hybridTraj); 53 | hsddp.set_initial_condition(x0); 54 | heuristic_bounding_controller(hybridTraj, problem); % Initialize the trajectory 55 | [xopt, uopt, Kopt] = hsddp.solve(options); 56 | 57 | %% Visulization 58 | traj_G = []; 59 | for i = 1:problem.nWBPhase 60 | traj_G = [traj_G, cell2mat(xopt{i})]; 61 | end 62 | G = QuadGraphics(); 63 | G.addTrajectory(traj_G); 64 | G.visualizeTrajectory(); 65 | -------------------------------------------------------------------------------- /Examples/Quadurped/Kinematics/Link2Jacobian_par.m: -------------------------------------------------------------------------------- 1 | function [Jx,Jdx] = Link2Jacobian_par(in1,in2) 2 | %LINK2JACOBIAN_PAR 3 | % [JX,JDX] = LINK2JACOBIAN_PAR(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 29-Nov-2020 17:42:41 7 | 8 | p1_1 = in2(1); 9 | p2_1 = in2(2); 10 | q3 = in1(3,:); 11 | q4 = in1(4,:); 12 | qd3 = in1(10,:); 13 | qd4 = in1(11,:); 14 | t2 = cos(q3); 15 | t3 = sin(q4); 16 | t4 = cos(q4); 17 | t5 = sin(q3); 18 | t6 = conj(p1_1); 19 | t7 = t2.*t4; 20 | t13 = t3.*t5; 21 | t8 = t7-t13; 22 | t9 = conj(p2_1); 23 | t10 = t2.*t3; 24 | t11 = t4.*t5; 25 | t12 = t10+t11; 26 | t15 = t6.*t8; 27 | t16 = t9.*t12; 28 | t14 = -t15-t16; 29 | t17 = t6.*t12; 30 | t19 = t8.*t9; 31 | t18 = t17-t19; 32 | t20 = t5.*(1.9e1./1.0e2); 33 | Jx = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t2.*(-1.9e1./1.0e2)-t6.*t8-t9.*t12,t17+t20-t8.*t9,t14,t18,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t14,t18,t14,t18,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2,7,14]); 34 | if nargout > 1 35 | t21 = qd4.*t18; 36 | t22 = qd3.*t18; 37 | t23 = t21+t22; 38 | t24 = t15+t16; 39 | t25 = qd4.*t24; 40 | t26 = t17-t19+t20; 41 | t27 = qd3.*t24; 42 | t28 = t25+t27; 43 | Jdx = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t21+qd3.*t26,t25+qd3.*(t2.*(1.9e1./1.0e2)+t15+t16),t23,t28,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t23,t28,t23,t28,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t2.*(-1.9e1./1.0e2)-t15-t16,t26,t14,t18,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t14,t18,t14,t18,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2,7,14]); 44 | end 45 | -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/Cost/WBCost.m: -------------------------------------------------------------------------------- 1 | classdef WBCost < handle 2 | properties 3 | dt 4 | ref 5 | Q 6 | R 7 | S 8 | Qf 9 | r 10 | end 11 | 12 | methods % constructor 13 | function C = WBCost(dt) 14 | C.dt = dt; 15 | % weightings for WB (whole-body) model 16 | % BS 17 | C.Q{1} = 0.01*diag([0,10,5,4,4,4,4, 2,1,.01,6,6,6,6]); 18 | C.R{1} = .5*diag([5,5,1,1]); 19 | C.S{1} = blkdiag(zeros(2), 0.3*eye(2)); 20 | C.Qf{1} = 100*diag([0,20,8,3,3,3,3, 3,2,0.01,5,5,0.01,0.01]); 21 | 22 | % FL1 23 | C.Q{2} = 0.01*diag([0,10,5,4,4,4,4, 2,1,.01,6,6,6,6]); 24 | C.R{2} = .5*eye(4); 25 | C.S{2} = zeros(4); 26 | C.Qf{2} = 100*diag([0,20,8,3,3,3,3, 3,2,0.01,5,5,5,5]); 27 | 28 | % FS 29 | C.Q{3} = 0.01*diag([0,10,5,4,4,4,4, 2,1,.01,6,6,6,6]); 30 | C.R{3} = .5*diag([1,1,5,5]); 31 | C.S{3} = blkdiag(0.15*eye(2), zeros(2)); 32 | C.Qf{3} = 100*diag([0,20,8,3,3,3,3, 3,2,0.01,0.01,0.01,5,5]); 33 | 34 | % FL2 35 | C.Q{4} = 0.01*diag([0,10,5,4,4,4,4, 2,1,.01,6,6,6,6]); 36 | C.R{4} = .5*eye(4); 37 | C.S{4} = zeros(4); 38 | C.Qf{4} = 100*diag([0,20,8,3,3,3,3, 3,2,0.01,5,5,5,5]); 39 | end 40 | 41 | end 42 | 43 | methods 44 | function l = running_cost(C,k,x,u,y,mode,r) 45 | l = wsquare(x - r.xd{k}, C.Q{mode}); 46 | l = l + wsquare(u - r.ud{k}, C.R{mode}); 47 | l = l + wsquare(y - r.yd{k}, C.S{mode}); 48 | l = l * C.dt; 49 | end 50 | function lpar = running_cost_partial(C, k, x, u, y, mode, r) 51 | lpar.lx = 2 * C.dt * C.Q{mode} * (x-r.xd{k}); % column vec 52 | lpar.lu = 2 * C.dt * C.R{mode} * (u-r.ud{k}); 53 | lpar.ly = 2 * C.dt * C.S{mode} * (y-r.yd{k}); 54 | lpar.lxx = 2 * C.dt * C.Q{mode}; 55 | lpar.lux = zeros(length(u),length(x)); 56 | lpar.luu = 2 * C.dt * C.R{mode}; 57 | lpar.lyy = 2 * C.dt * C.S{mode}; 58 | end 59 | end 60 | 61 | methods 62 | function phi = terminal_cost(C, x, mode, r) 63 | phi = 0.5* wsquare(x - r.xd{end}, C.Qf{mode}); 64 | end 65 | function phi_par = terminal_cost_partial(C, x, mode, r) 66 | phi_par.G = C.Qf{mode} * (x - r.xd{end}); 67 | phi_par.H = C.Qf{mode}; 68 | end 69 | end 70 | end -------------------------------------------------------------------------------- /Common/spatial_v2/models/autoTree.m: -------------------------------------------------------------------------------- 1 | function model = autoTree( nb, bf, skew, taper ) 2 | 3 | % autoTree Create System Models of Kinematic Trees 4 | % autoTree(nb,bf,skew,taper) creates system models of kinematic trees 5 | % having revolute joints, mainly for the purpose of testing dynamics 6 | % functions. nb and bf specify the number of bodies in the tree, and the 7 | % branching factor, respectively. The latter is the average number of 8 | % children of a nonterminal node, and must be >=1. bf=1 produces an 9 | % unbranched tree; bf=2 produces a binary tree; and non-integer values 10 | % produce trees in which the number of children alternates between 11 | % floor(bf) and ceil(bf) in such a way that the average is bf. Trees are 12 | % constructed (and numbered) breadth-first. Link i is a thin-walled 13 | % cylindrical tube of length l(i), radius l(i)/20, and mass m(i), lying 14 | % between 0 and l(i) on the x axis of its local coordinate system. The 15 | % values of l(i) and m(i) are determined by the tapering coefficient: 16 | % l(i)=taper^(i-1) and m(i)=taper^(3*(i-1)). Thus, if taper=1 then 17 | % m(i)=l(i)=1 for all i. The inboard joint axis of link i lies on the 18 | % local z axis, and its outboard axis passes through the point (l(i),0,0) 19 | % and is rotated about the x axis by an angle of skew radians relative to 20 | % the inboard axis. If the link has more than one outboard joint then they 21 | % all have the same axis. If skew=0 then the mechanism is planar. The 22 | % final one, two or three arguments can be omitted, in which case they 23 | % assume default values of taper=1, skew=0 and bf=1. 24 | 25 | if nargin < 4 26 | taper = 1; 27 | end 28 | if nargin < 3 29 | skew = 0; 30 | end 31 | if nargin < 2 32 | bf = 1; 33 | end 34 | 35 | model.NB = nb; 36 | 37 | for i = 1:nb 38 | model.jtype{i} = 'R'; 39 | model.parent(i) = floor((i-2+ceil(bf))/bf); 40 | if model.parent(i) == 0 41 | model.Xtree{1} = xlt([0 0 0]); 42 | else 43 | model.Xtree{i} = rotx(skew) * xlt([len(model.parent(i)),0,0]); 44 | end 45 | len(i) = taper^(i-1); 46 | mass = taper^(3*(i-1)); 47 | CoM = len(i) * [0.5,0,0]; 48 | Icm = mass * len(i)^2 * diag([0.0025,1.015/12,1.015/12]); 49 | model.I{i} = mcI( mass, CoM, Icm ); 50 | end 51 | 52 | % drawing instructions 53 | 54 | model.appearance.base = ... 55 | { 'box', [-0.2 -0.3 -0.2; 0.2 0.3 -0.06] }; 56 | 57 | p0 = -1; 58 | for i = 1:nb 59 | p1 = model.parent(i); 60 | tap = taper^(i-1); 61 | if p1 == 0 62 | ptap = 1; 63 | else 64 | ptap = taper^(p1-1); 65 | end 66 | if ( p1 > p0 ) 67 | model.appearance.body{i} = ... 68 | { 'cyl', [0 0 0; 1 0 0]*tap, 0.05*tap, ... 69 | 'cyl', [0 0 -0.07; 0 0 0.07]*ptap, 0.08*ptap }; 70 | p0 = p1; 71 | else 72 | model.appearance.body{i} = ... 73 | { 'cyl', [0 0 0; 1 0 0]*tap, 0.05*tap }; 74 | end 75 | end 76 | -------------------------------------------------------------------------------- /Common/spatial_v2/models/doubleElbow.m: -------------------------------------------------------------------------------- 1 | function robot = doubleElbow 2 | 3 | % doubleElbow planar robot with geared 2R elbow joint (Simulink Example 6) 4 | % doubleElbow creates a two-link planar robot that resembles planar(2), 5 | % except that the elbow joint has been replaced by a pair of revolute 6 | % joints close together, which are geared 1:1. This kind of joint can be 7 | % implemented with a pair of gears, or a pair of pulleys and a figure-eight 8 | % cable. This robot therefore has three revolute joints in total, but only 9 | % two independent degrees of fredom. The gearing constraint is implemented 10 | % by a constraint function provided in gamma_q. This robot is used in 11 | % Simulink Example 6. 12 | 13 | % For efficient use with Simulink, this function creates a model data 14 | % structure the first time it is called, and thereafter returns the stored 15 | % value below. 16 | 17 | persistent memory; 18 | 19 | if length(memory) ~= 0 20 | robot = memory; 21 | return 22 | end 23 | 24 | % This robot is implemented as a 3-link chain with 3 revolute joints, 25 | % but the joints are subject to the constraint q(2)==q(3). 26 | 27 | robot.NB = 3; 28 | robot.parent = [0 1 2]; 29 | robot.jtype = { 'r', 'r', 'r' }; 30 | 31 | robot.gamma_q = @gamma_q; % constraint-imposing function 32 | 33 | robot.Xtree = { eye(3), plnr(0,[1,0]), plnr(0,[0.25,0]) }; 34 | 35 | Ilink = mcI(1,[0.5,0],1/12); % inertia of the two main links 36 | 37 | robot.I = { Ilink, zeros(3), Ilink }; 38 | 39 | robot.appearance.base = ... 40 | { 'box', [-0.2 -0.3 -0.2; 0.2 0.3 -0.07] }; 41 | 42 | robot.appearance.body{1} = ... 43 | { 'box', [0 -0.07 -0.04; 1 0.07 0.04], ... 44 | 'cyl', [0 0 -0.07; 0 0 0.07], 0.1, ... 45 | 'cyl', [1 0 -0.05; 1 0 0.05], 0.125 }; 46 | 47 | robot.appearance.body{2} = ... 48 | { 'box', [0 -0.06 0.05; 0.25 0.06 0.08], ... 49 | 'box', [0 -0.06 -0.05; 0.25 0.06 -0.08], ... 50 | 'cyl', [0 0 -0.08; 0 0 0.08], 0.06, ... 51 | 'cyl', [0.25 0 -0.08; 0.25 0 0.08], 0.06 }; 52 | 53 | robot.appearance.body{3} = ... 54 | { 'box', [0 -0.07 -0.04; 1 0.07 0.04], ... 55 | 'cyl', [0 0 -0.05; 0 0 0.05], 0.125 }; 56 | 57 | 58 | % The function below implements the constraint q(2)==q(3). If qo and qdo 59 | % satisfy the constraints exactly then they are returned as q and qd. 60 | % Otherwise, qo(2) and qdo(2) are taken as correct, qo(3) and qdo(3) as 61 | % erroneous, and the returned value of gs includes a constraint 62 | % stabilization term that causes qo(3) and qdo(3) to converge towards qo(2) 63 | % and qdo(2) as the simulation proceeds. 64 | 65 | function [q,qd,G,gs] = gamma_q( model, qo, qdo ) 66 | 67 | q = [ qo(1); qo(2); qo(2) ]; % satisfies constraint exactly 68 | 69 | qd = [ qdo(1); qdo(2); qdo(2) ]; % satisfies constraint exactly 70 | 71 | G = [ 1 0; 0 1; 0 1 ]; 72 | 73 | Tstab = 0.1; 74 | 75 | gs = 2/Tstab*(qd-qdo) + 1/Tstab^2*(q-qo); 76 | -------------------------------------------------------------------------------- /Common/spatial_v2/3D/rv.m: -------------------------------------------------------------------------------- 1 | function out = rv( in ) 2 | 3 | % rv 3D rotation vector <--> 3x3 coordinate rotation matrix 4 | % E=rv(v) and v=rv(E) convert between a rotation vector v, whose magnitude 5 | % and direction describe the angle and axis of rotation of a coordinate 6 | % frame B relative to frame A, and the 3x3 coordinate rotation matrix E 7 | % that transforms from A to B coordinates. For example, if v=[theta;0;0] 8 | % then rv(v) produces the same matrix as rx(theta). If the argument is a 9 | % 3x3 matrix then it is assumed to be E, otherwise it is assumed to be v. 10 | % rv(E) expects E to be accurately orthonormal, and returns a column vector 11 | % with a magnitude in the range [0,pi]. If the magnitude is exactly pi 12 | % then the sign of the return value is unpredictable, since pi*u and -pi*u, 13 | % where u is any unit vector, both represent the same rotation. rv(v) will 14 | % accept a row or column vector of any magnitude. 15 | 16 | if all(size(in)==[3 3]) 17 | out = Etov( in ); 18 | else 19 | out = vtoE( [in(1);in(2);in(3)] ); 20 | end 21 | 22 | 23 | function E = vtoE( v ) 24 | 25 | theta = norm(v); 26 | if theta == 0 27 | E = eye(3); 28 | else 29 | s = sin(theta); 30 | c = cos(theta); 31 | c1 = 2 * sin(theta/2)^2; % 1-cos(h) == 2sin^2(h/2) 32 | u = v/theta; 33 | E = c*eye(3) - s*skew(u) + c1*u*u'; 34 | end 35 | 36 | 37 | function v = Etov( E ) 38 | 39 | % This function begins by extracting the skew-symmetric component of E, 40 | % which, as can be seen from the previous function, is -s*skew(v/theta). 41 | % For angles sufficiently far from pi, v is then calculated directly from 42 | % this quantity. However, for angles close to pi, E is almost symmetric, 43 | % and so extracting the skew-symmetric component becomes numerically 44 | % ill-conditioned, and provides an increasingly inaccurate value for the 45 | % direction of v. Therefore, the component c1*u*u' is extracted as well, 46 | % and used to get an accurate value for the direction of v. If the angle 47 | % is exactly pi then the sign of v will be indeterminate, since +v and -v 48 | % both represent the same rotation, but the direction will still be 49 | % accurate. 50 | 51 | w = -skew(E); % w == s/theta * v 52 | s = norm(w); 53 | c = (trace(E)-1)/2; 54 | theta = atan2(s,c); 55 | 56 | if s == 0 57 | v = [0;0;0]; 58 | elseif theta < 0.9*pi % a somewhat arbitrary threshold 59 | v = theta/s * w; 60 | else % extract v*v' component from E and 61 | E = E - c * eye(3); % pick biggest column (best chance 62 | E = E + E'; % to get sign right) 63 | if E(1,1) >= E(2,2) && E(1,1) >= E(3,3) 64 | if w(1) >= 0 65 | v = E(:,1); 66 | else 67 | v = -E(:,1); 68 | end 69 | elseif E(2,2) >= E(3,3) 70 | if w(2) >= 0 71 | v = E(:,2); 72 | else 73 | v = -E(:,2); 74 | end 75 | else 76 | if w(3) >= 0 77 | v = E(:,3); 78 | else 79 | v = -E(:,3); 80 | end 81 | end 82 | v = theta/norm(v) * v; 83 | end 84 | -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/HD.m: -------------------------------------------------------------------------------- 1 | function [qdd_out,tau_out] = HD( model, fd, q, qd, qdd, tau, f_ext ) 2 | 3 | % HD Articulated-Body Hybrid Dynamics Algorithm 4 | % [qdd_out,tau_out]=HD(model,fd,q,qd,qdd,tau,f_ext) calculates the hybrid 5 | % dynamics of a kinematic tree using the articulated-body algorithm. fd is 6 | % an array of boolean values such that fd(i)==1 if joint i is a 7 | % forward-dynamics joint, and fd(i)==0 otherwise. If fd(i)==1 then tau(i) 8 | % contains the given force at joint i, and the value of qdd(i) is ignored; 9 | % and if fd(i)==0 then qdd(i) contains the given acceleration at joint i, 10 | % and the value of tau(i) is ignored. Likewise, if fd(i)==1 then 11 | % qdd_out(i) contains the calculated acceleration at joint i, and 12 | % tau_out(i) contains the given force copied from tau(i); and if fd(i)==0 13 | % then tau_out(i) contains the calculated force and qdd_out(i) the given 14 | % acceleration copied from qdd(i). Thus, the two output vectors are always 15 | % fully instantiated. f_ext is an optional argument specifying the 16 | % external forces acting on the bodies. It can be omitted if there are no 17 | % external forces. The format of f_ext is explained in the source code of 18 | % apply_external_forces. 19 | 20 | a_grav = get_gravity(model); 21 | 22 | for i = 1:model.NB 23 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q(i) ); 24 | vJ = S{i}*qd(i); 25 | Xup{i} = XJ * model.Xtree{i}; 26 | if model.parent(i) == 0 27 | v{i} = vJ; 28 | c{i} = zeros(size(a_grav)); % spatial or planar zero vector 29 | else 30 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 31 | c{i} = crm(v{i}) * vJ; 32 | end 33 | if fd(i) == 0 34 | c{i} = c{i} + S{i} * qdd(i); 35 | end 36 | IA{i} = model.I{i}; 37 | pA{i} = crf(v{i}) * model.I{i} * v{i}; 38 | end 39 | 40 | if nargin == 7 41 | pA = apply_external_forces( model.parent, Xup, pA, f_ext ); 42 | end 43 | 44 | for i = model.NB:-1:1 45 | if fd(i) == 0 46 | if model.parent(i) ~= 0 47 | Ia = IA{i}; 48 | pa = pA{i} + IA{i}*c{i}; 49 | IA{model.parent(i)} = IA{model.parent(i)} + Xup{i}' * Ia * Xup{i}; 50 | pA{model.parent(i)} = pA{model.parent(i)} + Xup{i}' * pa; 51 | end 52 | else 53 | U{i} = IA{i} * S{i}; 54 | d{i} = S{i}' * U{i}; 55 | u{i} = tau(i) - S{i}'*pA{i}; 56 | if model.parent(i) ~= 0 57 | Ia = IA{i} - U{i}/d{i}*U{i}'; 58 | pa = pA{i} + Ia*c{i} + U{i} * u{i}/d{i}; 59 | IA{model.parent(i)} = IA{model.parent(i)} + Xup{i}' * Ia * Xup{i}; 60 | pA{model.parent(i)} = pA{model.parent(i)} + Xup{i}' * pa; 61 | end 62 | end 63 | end 64 | 65 | for i = 1:model.NB 66 | if model.parent(i) == 0 67 | a{i} = Xup{i} * -a_grav + c{i}; 68 | else 69 | a{i} = Xup{i} * a{model.parent(i)} + c{i}; 70 | end 71 | if fd(i) == 0 72 | qdd_out(i,1) = qdd(i); 73 | tau_out(i,1) = S{i}'*(IA{i}*a{i} + pA{i}); 74 | else 75 | qdd_out(i,1) = (u{i} - U{i}'*a{i})/d{i}; 76 | tau_out(i,1) = tau(i); 77 | a{i} = a{i} + S{i}*qdd_out(i); 78 | end 79 | end 80 | -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/IDfb.m: -------------------------------------------------------------------------------- 1 | function [xdfb,tau] = IDfb( model, xfb, q, qd, qdd, f_ext ) 2 | 3 | % IDfb Floating-Base Inverse Dynamics (=Hybrid Dynamics) 4 | % [xdfb,tau]=IDfb(model,xfb,q,qd,qdd,f_ext) calculates the inverse dynamics 5 | % of a floating-base kinematic tree via the algorithm in Table 9.6 of RBDA 6 | % (which is really a special case of hybrid dynamics), using the same 7 | % singularity-free representation of the motion of the floating base as 8 | % used by FDfb. xfb is a 13-element column vector containing: a unit 9 | % quaternion specifying the orientation of the floating base (=body 6)'s 10 | % coordinate frame relative to the fixed base; a 3D vector specifying the 11 | % position of the origin of the floating base's coordinate frame in 12 | % fixed-base coordinates; and a spatial vector giving the velocity of the 13 | % floating base in fixed-base coordinates. The return value xdfb is the 14 | % time-derivative of xfb. The arguments q, qd and qdd contain the 15 | % position, velocity and acceleration variables for the real joints in the 16 | % system (i.e., joints 7 onwards in the system model); so q(i), qd(i) and 17 | % qdd(i) all apply to joint i+6. The return value tau is the vector of 18 | % force variables required to produce the given acceleration qdd. f_ext is 19 | % an optional argument specifying the external forces acting on the bodies. 20 | % It can be omitted if there are no external forces. If supplied, it must 21 | % be a cell array of length model.NB, of which the first 5 elements are 22 | % ignored, and f_ext{6} onward specify the forces acting on the floating 23 | % base (body 6) onward. The format of f_ext is explained in the source 24 | % code of apply_external_forces. 25 | 26 | a_grav = get_gravity(model); 27 | 28 | qn = xfb(1:4); % unit quaternion fixed-->f.b. 29 | r = xfb(5:7); % position of f.b. origin 30 | Xup{6} = plux( rq(qn), r ); % xform fixed --> f.b. coords 31 | 32 | vfb = xfb(8:end); 33 | v{6} = Xup{6} * vfb; % f.b. vel in f.b. coords 34 | 35 | a{6} = zeros(6,1); 36 | 37 | IC{6} = model.I{6}; 38 | pC{6} = model.I{6}*a{6} + crf(v{6})*model.I{6}*v{6}; 39 | 40 | for i = 7:model.NB 41 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q(i-6) ); 42 | vJ = S{i}*qd(i-6); 43 | Xup{i} = XJ * model.Xtree{i}; 44 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 45 | a{i} = Xup{i}*a{model.parent(i)} + S{i}*qdd(i-6) + crm(v{i})*vJ; 46 | IC{i} = model.I{i}; 47 | pC{i} = IC{i}*a{i} + crf(v{i})*IC{i}*v{i}; 48 | end 49 | 50 | if nargin == 6 && length(f_ext) > 0 51 | prnt = model.parent(6:end) - 5; 52 | pC(6:end) = apply_external_forces(prnt, Xup(6:end), pC(6:end), f_ext(6:end)); 53 | end 54 | 55 | for i = model.NB:-1:7 56 | IC{model.parent(i)} = IC{model.parent(i)} + Xup{i}'*IC{i}*Xup{i}; 57 | pC{model.parent(i)} = pC{model.parent(i)} + Xup{i}'*pC{i}; 58 | end 59 | 60 | a{6} = - IC{6} \ pC{6}; % floating-base acceleration 61 | % without gravity 62 | for i = 7:model.NB 63 | a{i} = Xup{i} * a{model.parent(i)}; 64 | tau(i-6,1) = S{i}'*(IC{i}*a{i} + pC{i}); 65 | end 66 | 67 | qnd = rqd( vfb(1:3), qn ); % derivative of qn 68 | rd = Vpt( vfb, r ); % lin vel of flt base origin 69 | afb = Xup{6} \ a{6} + a_grav; % f.b. accn in fixed-base coords 70 | 71 | xdfb = [ qnd; rd; afb ]; 72 | -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/fbanim.m: -------------------------------------------------------------------------------- 1 | function Q = fbanim( X, Qr ) 2 | 3 | % fbanim Floating Base Inverse Kinematics for Animation 4 | % Q=fbanim(X) and Q=fbanim(X,Qr) calculate matrices of joint position data 5 | % for use in showmotion animations. Q=fbanim(X) calculates a 6xN matrix Q 6 | % from a 13x1xN or 13xN or 7x1xN or 7xN matrix X, such that each column of 7 | % X contains at least the first 7 elements of a 13-element singularity-free 8 | % state vector used by FDfb and IDfb, and the corresponding column of Q 9 | % contains the position variables of the three prismatic and three revolute 10 | % joints that form the floating base. Each column of Q is calculated using 11 | % fbkin; but fbanim then adjusts the three revolute joint variables to 12 | % remove the pi and 2*pi jumps that occur when the angles wrap around or 13 | % pass through a kinematic singularity. As a result, some or all of the 14 | % revolute joint angles may grow without limit. This is essential for a 15 | % smooth animation. Q=fbanim(X,Qr) performs the same calculation as just 16 | % described, but then appends Qr to Q. Qr must be an MxN or Mx1xN matrix 17 | % of joint position data for the real joints in a floating-base mechanism, 18 | % and the resulting (6+M)xN matrix Q contains the complete set of joint 19 | % data required by showmotion. Note: the algorithm used to remove the 20 | % jumps makes continuity assumptions (specifically, less than pi/2 changes 21 | % from one column to the next, except when passing through a singularity) 22 | % that are not guaranteed to be true. Therefore, visible glitches in an 23 | % animation are still possible. 24 | 25 | if length(size(X)) == 3 % collapse 3D -> 2D array 26 | tmp(:,:) = X(:,1,:); 27 | X = tmp; 28 | end 29 | 30 | % apply kinematic transform using fbkin 31 | 32 | for i = 1 : size(X,2) 33 | Q(:,i) = fbkin( X(1:7,i) ); 34 | end 35 | 36 | % This code removes wrap-arounds and step-changes on passing through a 37 | % singularity. Whenever q6 or q4 wrap, they jump by 2*pi. However, when 38 | % q5 hits pi/2 (or -pi/2), q4 and q6 both jump by pi, and q5 turns around. 39 | % To undo this, the code looks to see whether n is an even or odd number, 40 | % and replaces q5 with pi-q5 whenever n is odd. q4 is calculated via the 41 | % sum or difference of q4 and q6 on the grounds that the sum well defined 42 | % at the singularity at q5=pi/2 and the difference is well defined at 43 | % q5=-pi/2. 44 | 45 | for i = 2 : size(X,2) 46 | n = round( (Q(6,i-1) - Q(6,i)) / pi ); 47 | q6 = Q(6,i) + n*pi; 48 | if Q(5,i) >= 0 49 | q46 = Q(4,i) + Q(6,i); 50 | q46 = q46 + 2*pi * round( (Q(4,i-1)+Q(6,i-1) - q46) / (2*pi) ); 51 | Q(4,i) = q46 - q6; 52 | else 53 | q46 = Q(4,i) - Q(6,i); 54 | q46 = q46 + 2*pi * round( (Q(4,i-1)-Q(6,i-1) - q46) / (2*pi) ); 55 | Q(4,i) = q46 + q6; 56 | end 57 | Q(6,i) = q6; 58 | if mod(n,2) == 0 59 | q5 = Q(5,i); 60 | else 61 | q5 = pi - Q(5,i); 62 | end 63 | Q(5,i) = q5 + 2*pi * round( (Q(5,i-1) - q5) / (2*pi) ); 64 | end 65 | 66 | % add the rest of the joint data, if argument Qr has been supplied 67 | 68 | if nargin == 2 69 | if length(size(Qr)) == 3 % collapse 3D -> 2D array 70 | tmp = zeros(size(Qr,1),size(Qr,3)); 71 | tmp(:,:) = Qr(:,1,:); 72 | Qr = tmp; 73 | end 74 | Q = [ Q; Qr ]; 75 | end 76 | -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/FDfb.m: -------------------------------------------------------------------------------- 1 | function [xdfb,qdd] = FDfb( model, xfb, q, qd, tau, f_ext ) 2 | 3 | % FDfb Floating-Base Forward Dynamics via Articulated-Body Algorithm 4 | % [xdfb,qdd]=FDfb(model,xfb,q,qd,tau,f_ext) calculates the forward dynamics 5 | % of a floating-base kinematic tree via the articulated-body algorithm. 6 | % This function avoids the kinematic singularity in the six-joint chain 7 | % created by floatbase to mimic a true 6-DoF joint. xfb is a 13-element 8 | % column vector containing: a unit quaternion specifying the orientation of 9 | % the floating base (=body 6)'s coordinate frame relative to the fixed 10 | % base; a 3D vector specifying the position of the origin of the floating 11 | % base's coordinate frame in fixed-base coordinates; and a spatial vector 12 | % giving the velocity of the floating base in fixed-base coordinates. The 13 | % return value xdfb is the time-derivative of this vector. The arguments 14 | % q, qd and tau contain the position, velocity and force variables for the 15 | % real joints in the system (i.e., joints 7 onwards in the system model); 16 | % so q(i), qd(i) and tau(i) all apply to joint i+6. The return value qdd 17 | % is the time-derivative of qd. f_ext is an optional argument specifying 18 | % the external forces acting on the bodies. It can be omitted if there are 19 | % no external forces. If supplied, it must be a cell array of length 20 | % model.NB, of which the first 5 elements are ignored, and f_ext{6} onward 21 | % specify the forces acting on the floating base (body 6) onward. The 22 | % format of f_ext is explained in the source code of apply_external_forces. 23 | 24 | a_grav = get_gravity(model); 25 | 26 | qn = xfb(1:4); % unit quaternion fixed-->f.b. 27 | r = xfb(5:7); % position of f.b. origin 28 | Xup{6} = plux( rq(qn), r ); % xform fixed --> f.b. coords 29 | 30 | vfb = xfb(8:end); 31 | v{6} = Xup{6} * vfb; % f.b. vel in f.b. coords 32 | 33 | IA{6} = model.I{6}; 34 | pA{6} = crf(v{6}) * model.I{6} * v{6}; 35 | 36 | for i = 7:model.NB 37 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q(i-6) ); 38 | vJ = S{i}*qd(i-6); 39 | Xup{i} = XJ * model.Xtree{i}; 40 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 41 | c{i} = crm(v{i}) * vJ; 42 | IA{i} = model.I{i}; 43 | pA{i} = crf(v{i}) * model.I{i} * v{i}; 44 | end 45 | 46 | if nargin == 6 && length(f_ext) > 0 47 | prnt = model.parent(6:end) - 5; 48 | pA(6:end) = apply_external_forces(prnt, Xup(6:end), pA(6:end), f_ext(6:end)); 49 | end 50 | 51 | for i = model.NB:-1:7 52 | U{i} = IA{i} * S{i}; 53 | d{i} = S{i}' * U{i}; 54 | u{i} = tau(i-6) - S{i}'*pA{i}; 55 | Ia = IA{i} - U{i}/d{i}*U{i}'; 56 | pa = pA{i} + Ia*c{i} + U{i} * u{i}/d{i}; 57 | IA{model.parent(i)} = IA{model.parent(i)} + Xup{i}' * Ia * Xup{i}; 58 | pA{model.parent(i)} = pA{model.parent(i)} + Xup{i}' * pa; 59 | end 60 | 61 | a{6} = - IA{6} \ pA{6}; % floating base accn without gravity 62 | 63 | qdd = zeros(0,1); % avoids a matlab warning when NB==6 64 | 65 | for i = 7:model.NB 66 | a{i} = Xup{i} * a{model.parent(i)} + c{i}; 67 | qdd(i-6,1) = (u{i} - U{i}'*a{i})/d{i}; 68 | a{i} = a{i} + S{i}*qdd(i-6); 69 | end 70 | 71 | qnd = rqd( vfb(1:3), qn ); % derivative of qn 72 | rd = Vpt( vfb, r ); % lin vel of flt base origin 73 | afb = Xup{6} \ a{6} + a_grav; % true f.b. accn in fixed-base coords 74 | 75 | xdfb = [ qnd; rd; afb ]; 76 | -------------------------------------------------------------------------------- /Examples/Quadurped/Kinematics/Link3Jacobian_par.m: -------------------------------------------------------------------------------- 1 | function [Jx,Jdx] = Link3Jacobian_par(in1,in2) 2 | %LINK3JACOBIAN_PAR 3 | % [JX,JDX] = LINK3JACOBIAN_PAR(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 29-Nov-2020 17:42:48 7 | 8 | p1_1 = in2(1); 9 | p2_1 = in2(2); 10 | q3 = in1(3,:); 11 | q4 = in1(4,:); 12 | q5 = in1(5,:); 13 | qd3 = in1(10,:); 14 | qd4 = in1(11,:); 15 | qd5 = in1(12,:); 16 | t2 = cos(q3); 17 | t3 = cos(q4); 18 | t4 = sin(q3); 19 | t5 = sin(q4); 20 | t6 = cos(q5); 21 | t7 = t2.*t5; 22 | t8 = t3.*t4; 23 | t9 = t7+t8; 24 | t10 = sin(q5); 25 | t11 = t2.*t3; 26 | t16 = t4.*t5; 27 | t12 = t11-t16; 28 | t13 = t2.*t5.*(2.09e2./1.0e3); 29 | t14 = t3.*t4.*(2.09e2./1.0e3); 30 | t15 = conj(p1_1); 31 | t17 = t6.*t12; 32 | t23 = t9.*t10; 33 | t18 = t17-t23; 34 | t19 = conj(p2_1); 35 | t20 = t6.*t9; 36 | t21 = t10.*t12; 37 | t22 = t20+t21; 38 | t25 = t15.*t18; 39 | t26 = t19.*t22; 40 | t24 = t13+t14-t25-t26; 41 | t27 = -t25-t26; 42 | t28 = t2.*t3.*(2.09e2./1.0e3); 43 | t29 = t15.*t22; 44 | t31 = t4.*t5.*(2.09e2./1.0e3); 45 | t32 = t18.*t19; 46 | t30 = t28+t29-t31-t32; 47 | t33 = t29-t32; 48 | t34 = t4.*(1.9e1./1.0e2); 49 | Jx = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t2.*(-1.9e1./1.0e2)+t13+t14-t15.*t18-t19.*t22,t28+t29+t34-t4.*t5.*(2.09e2./1.0e3)-t18.*t19,t24,t30,t27,t33,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t24,t30,t24,t30,t27,t33,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-t15.*t18-t19.*t22,t29-t18.*t19,t27,t33,t27,t33,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2,7,14]); 50 | if nargout > 1 51 | t35 = qd5.*t33; 52 | t36 = qd4.*t30; 53 | t37 = qd3.*t30; 54 | t38 = t35+t36+t37; 55 | t39 = qd3.*t33; 56 | t40 = qd4.*t33; 57 | t41 = t35+t39+t40; 58 | t42 = t25+t26; 59 | t43 = qd5.*t42; 60 | t44 = t28+t29-t31-t32+t34; 61 | t46 = qd3.*t24; 62 | t47 = qd4.*t24; 63 | t45 = t43-t46-t47; 64 | t48 = qd3.*t42; 65 | t49 = qd4.*t42; 66 | t50 = t43+t48+t49; 67 | Jdx = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t35+t36+qd3.*t44,t43-qd4.*t24+qd3.*(t2.*(1.9e1./1.0e2)-t13-t14+t25+t26),t38,t45,t41,t50,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t38,t45,t38,t45,t41,t50,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t41,t50,t41,t50,t41,t50,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t2.*(-1.9e1./1.0e2)+t13+t14-t25-t26,t44,t24,t30,t27,t33,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t24,t30,t24,t30,t27,t33,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t27,t33,t27,t33,t27,t33,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2,7,14]); 68 | end 69 | -------------------------------------------------------------------------------- /Examples/Quadurped/Kinematics/Link5Jacobian_par.m: -------------------------------------------------------------------------------- 1 | function [Jx,Jdx] = Link5Jacobian_par(in1,in2) 2 | %LINK5JACOBIAN_PAR 3 | % [JX,JDX] = LINK5JACOBIAN_PAR(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 29-Nov-2020 17:42:57 7 | 8 | p1_1 = in2(1); 9 | p2_1 = in2(2); 10 | q3 = in1(3,:); 11 | q6 = in1(6,:); 12 | q7 = in1(7,:); 13 | qd3 = in1(10,:); 14 | qd6 = in1(13,:); 15 | qd7 = in1(14,:); 16 | t2 = cos(q3); 17 | t3 = cos(q6); 18 | t4 = sin(q3); 19 | t5 = sin(q6); 20 | t6 = cos(q7); 21 | t7 = t2.*t5; 22 | t8 = t3.*t4; 23 | t9 = t7+t8; 24 | t10 = sin(q7); 25 | t11 = t2.*t3; 26 | t16 = t4.*t5; 27 | t12 = t11-t16; 28 | t13 = t2.*t5.*(2.09e2./1.0e3); 29 | t14 = t3.*t4.*(2.09e2./1.0e3); 30 | t15 = conj(p1_1); 31 | t17 = t6.*t12; 32 | t23 = t9.*t10; 33 | t18 = t17-t23; 34 | t19 = conj(p2_1); 35 | t20 = t6.*t9; 36 | t21 = t10.*t12; 37 | t22 = t20+t21; 38 | t25 = t15.*t18; 39 | t26 = t19.*t22; 40 | t24 = t13+t14-t25-t26; 41 | t27 = -t25-t26; 42 | t28 = t2.*t3.*(2.09e2./1.0e3); 43 | t29 = t15.*t22; 44 | t31 = t4.*t5.*(2.09e2./1.0e3); 45 | t32 = t18.*t19; 46 | t30 = t28+t29-t31-t32; 47 | t33 = t29-t32; 48 | t34 = qd7.*t33; 49 | t35 = qd6.*t30; 50 | t36 = t2.*(1.9e1./1.0e2); 51 | Jx = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t13+t14+t36-t15.*t18-t19.*t22,t4.*(-1.9e1./1.0e2)+t28+t29-t4.*t5.*(2.09e2./1.0e3)-t18.*t19,0.0,0.0,0.0,0.0,t24,t30,t27,t33,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t24,t30,0.0,0.0,0.0,0.0,t24,t30,t27,t33,0.0,0.0,0.0,0.0,-t15.*t18-t19.*t22,t29-t18.*t19,0.0,0.0,0.0,0.0,t27,t33,t27,t33,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2,7,14]); 52 | if nargout > 1 53 | t37 = qd3.*t30; 54 | t38 = t34+t35+t37; 55 | t39 = qd3.*t33; 56 | t40 = qd6.*t33; 57 | t41 = t34+t39+t40; 58 | t42 = t13+t14-t25-t26+t36; 59 | t43 = t25+t26; 60 | t44 = qd7.*t43; 61 | t45 = t4.*(1.9e1./1.0e2); 62 | t47 = qd3.*t24; 63 | t48 = qd6.*t24; 64 | t46 = t44-t47-t48; 65 | t49 = qd3.*t43; 66 | t50 = qd6.*t43; 67 | t51 = t44+t49+t50; 68 | Jdx = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t34+t35-qd3.*(-t28-t29+t31+t32+t45),t44-qd6.*t24-qd3.*t42,0.0,0.0,0.0,0.0,t38,t46,t41,t51,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t38,t46,0.0,0.0,0.0,0.0,t38,t46,t41,t51,0.0,0.0,0.0,0.0,t41,t51,0.0,0.0,0.0,0.0,t41,t51,t41,t51,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t42,t28+t29-t31-t32-t45,0.0,0.0,0.0,0.0,t24,t30,t27,t33,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t24,t30,0.0,0.0,0.0,0.0,t24,t30,t27,t33,0.0,0.0,0.0,0.0,t27,t33,0.0,0.0,0.0,0.0,t27,t33,t27,t33],[2,7,14]); 69 | end 70 | -------------------------------------------------------------------------------- /Examples/Quadurped/Bounding/createBoundingTask.m: -------------------------------------------------------------------------------- 1 | function [phases, hybridTraj, hybridRef] = createBoundingTask(problem) 2 | phaseSeq = problem.phaseSeq; 3 | len_horizons = zeros(length(phaseSeq)); 4 | for i = 1:length(len_horizons) 5 | len_horizons(i) = floor(phaseSeq(i).duration/problem.dt); 6 | end 7 | 8 | % Build dynamics object 9 | wbModel = PlanarQuadruped(problem.dt); 10 | build2DminiCheetah(wbModel); 11 | fbModel = PlanarFloatingBase(problem.dt); 12 | footholdPlanner = FootholdPlanner(fbModel); 13 | 14 | % Build cost object 15 | wbCost = WBCost(problem.dt); 16 | fbCost = FBCost(problem.dt); 17 | 18 | 19 | % Build multiple phases and trajectory data 20 | for i = 1:problem.nPhase 21 | if i <= problem.nWBPhase 22 | hybridRef(i) = TrajReference(14,4,4,len_horizons(i)); 23 | hybridTraj(i) = Trajectory(14,4,4,len_horizons(i),problem.dt); 24 | else 25 | hybridRef(i) = TrajReference(6,4,4,len_horizons(i)); 26 | hybridTraj(i) = Trajectory(6,4,4,len_horizons(i),problem.dt); 27 | end 28 | end 29 | 30 | for i = 1:problem.nPhase 31 | phases(i) = Phase; 32 | mode = phaseSeq(i).mode; 33 | ctact = phaseSeq(i).ctact; 34 | i_next = i + 1; 35 | if i_next > problem.nPhase 36 | i_next = 1; 37 | end 38 | nmode = phaseSeq(i_next).mode; 39 | nctact = phaseSeq(i_next).ctact; 40 | duration = phaseSeq(i).duration; 41 | if i <= problem.nWBPhase 42 | % Whole-body phase 43 | phases(i).set_dynamics({@(x,u) wbModel.dynamics(x,u,mode),... 44 | @(x,u) wbModel.dynamics_par(x,u,mode)}); 45 | phases(i).set_resetmap({@(x) wbModel.resetmap(x,nmode),... 46 | @(x) wbModel.resetmap_par(x,nmode)}); 47 | if i+1 > problem.nWBPhase 48 | phases(i).set_resetmap({@(x) wbModel.contractmap(x,nmode),... 49 | @(x) wbModel.contractmap_par(x,nmode)}); 50 | end 51 | phases(i).set_running_cost({@(k,x,u,y) wbCost.running_cost(k,x,u,y,mode,hybridRef(i)),... 52 | @(k,x,u,y) wbCost.running_cost_partial(k,x,u,y,mode,hybridRef(i))}); 53 | phases(i).set_terminal_cost({@(x) wbCost.terminal_cost(x,mode,hybridRef(i)),... 54 | @(x) wbCost.terminal_cost_partial(x,mode,hybridRef(i))}); 55 | phases(i).add_path_constraints(torque_limit()); 56 | if any(mode == [1 3]) 57 | phases(i).add_path_constraints(GRF_constraint(ctact)); 58 | end 59 | if any(nmode == [1 3]) 60 | phases(i).add_terminal_constraints(SwitchingConstraint(nctact)); 61 | end 62 | else 63 | % Floating-base phase 64 | phases(i).set_dynamics({@(x,u) fbModel.dynamics(x,u,ctact),... 65 | @(x,u) fbModel.dynamics_par(x,u,ctact)}); 66 | phases(i).set_resetmap({@fbModel.resetmap,... 67 | @fbModel.resetmap_par}); 68 | phases(i).set_running_cost({@(k,x,u,y) fbCost.running_cost(k,x,u,y,mode,hybridRef(i)),... 69 | @(k,x,u,y) fbCost.running_cost_partial(k,x,u,y,mode, hybridRef(i))}); 70 | phases(i).set_terminal_cost({@(x) fbCost.terminal_cost(x,mode,hybridRef(i)),... 71 | @(x) fbCost.terminal_cost_partial(x,mode,hybridRef(i))}); 72 | if any(mode == [1 3]) 73 | phases(i).set_additional_initialization(@(x) footholdPlanner.foothold_prediction(x,duration,problem.vd)); 74 | end 75 | end 76 | end 77 | end -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/FDgq.m: -------------------------------------------------------------------------------- 1 | function qdd = FDgq( model, q, qd, tau, f_ext ) 2 | 3 | % FDgq Forward Dynamics via CRBA + constraint function gamma_q 4 | % FDgq(model,q,qd,tau,f_ext) calculates the forward dynamics of a 5 | % kinematic tree, subject to the kinematic constraints embodied in the 6 | % function model.gamma_q, via the composite-rigid-body algorithm. q, qd 7 | % and tau are vectors of joint position, velocity and force variables; and 8 | % the return value is a vector of joint acceleration variables. q and qd do 9 | % not have to satisfy the constraints exactly, but are expected to be 10 | % close. qdd will typically contain a constraint-stabilization component 11 | % that tends to reduce constraint violation over time. f_ext is an optional 12 | % argument specifying the external forces acting on the bodies. It can be 13 | % omitted if there are no external forces. The format of f_ext is 14 | % explained in the source code of apply_external_forces. A detailed 15 | % description of gamma_q appears at the end of this source code. 16 | 17 | [q,qd,G,g] = model.gamma_q( model, q, qd ); 18 | 19 | if nargin == 4 20 | [H,C] = HandC( model, q, qd ); 21 | else 22 | [H,C] = HandC( model, q, qd, f_ext ); 23 | end 24 | 25 | qdd = G * ((G'*H*G) \ (G'*(tau-C-H*g))) + g; % cf. eq 3.20 in RBDA 26 | 27 | 28 | 29 | 30 | %{ 31 | How to Create Your Own gamma_q 32 | ------------------------------ 33 | 34 | [See also Section 3.2 of "Rigid Body Dynamics Algorithms", and maybe also 35 | Section 8.3.] 36 | 37 | The purpose of this function is to define a set of algebraic constraints among 38 | the elements of q, and therefore also among the elements of qd and qdd. These 39 | constraints could be due to kinematic loops, gears, pulleys, and so on. To 40 | create a function gamma_q, proceed as follows: 41 | 42 | 1. Identify a set of independent variables, y. Typically, y will be a subset 43 | of the variables in q. 44 | 45 | 2. Define a function, gamma, that maps y to q; i.e., q=gamma(y). The 46 | calculated value of q must satisfy the constraints exactly. 47 | 48 | 3. Define an inverse function, gamma^-1, that maps q to y. This function 49 | must satisfy gamma^-1(gamma(y))==y for all y, and gamma(gamma^-1(q))==q 50 | for all q that satisfy the constraints exactly. 51 | 52 | 4. Optional: For some kinds of constraints, gamma may be ambiguous (i.e., 53 | multi-valued). In these cases, gamma can be modified to take a second 54 | argument, q0, which is a vector satisfying gamma^-1(q0)==y. This vector 55 | plays the role of a disambiguator---it identifies which one of multiple 56 | possible values of gamma(y) is the correct one in the present context. 57 | For differentiation purposes, this extra argument is regarded as a 58 | constant. 59 | 60 | 5. Having defined gamma, G is the matrix partial d/dy gamma (i.e., it is 61 | the Jacobian of gamma). 62 | 63 | 6. G provides the following relationship between qd and yd: qd = G * yd, 64 | which can be used to calculate qd from yd. Alternatively, qd could be 65 | calculated directly from d/dt gamma(y). 66 | 67 | 7. To obtain yd from qd, use yd = d/dt gamma^-1(q). In general, the 68 | right-hand side will be a function of both q and qd. However, if y is a 69 | subset of q then yd is the same subset of qd. 70 | 71 | 8. Given that qd = G * yd, it follows that qdd = G * ydd + dG/dt * yd. 72 | However, it is advisable to add a stabilization term to this formula, so 73 | that qdd = G * ydd + g, where g = dG/dt * yd + gs, and gs is a 74 | stabilization term defined as follows: gs = 2/Ts * qderr + 1/Ts^2 * qerr. 75 | Ts is a stabilization time constant, and qerr and qderr are measures of 76 | the degree to which the (given) prevailing values of q and qd fail to 77 | satisfy the constraints. If q0 and qd0 are the prevailing values then 78 | qerr = gamma(gamma^-1(q0)) - q0 and qderr = G*yd - qd0 (where yd is 79 | calculated from qd0). 80 | 81 | The function gamma_q can now be defined as follows: 82 | 83 | function call: [q,qd,G,g] = gamma_q( model, q0, qd0 ) 84 | 85 | where q0 and qd0 are the current values of the joint position and velocity 86 | variables, q and qd are new values that exactly satisfy the constraints, and G 87 | and g are as described above. 88 | 89 | function body: 90 | 91 | y = gamma^-1(q0); 92 | q = gamma(y); % or gamma(y,q0) 93 | 94 | G = Jacobian of gamma; 95 | 96 | yd = d/dt gamma^-1(q0); % a function of q0 and qd0 97 | qd = G * yd; % or d/dt gamma(y) 98 | 99 | Ts = some suitable value, such as 0.1 or 0.01; 100 | 101 | gs = 2/Ts * (qd - qd0) + 1/Ts^2 * (q - q0); 102 | 103 | g = dG/dt * yd + gs; 104 | 105 | Tip: gamma_q can be used at the beginning of a simulation run to initialize q 106 | and qd to values that satisfy the constraints exactly. 107 | %} 108 | -------------------------------------------------------------------------------- /Common/spatial_v2/dynamics/fbkin.m: -------------------------------------------------------------------------------- 1 | function [o1, o2, o3] = fbkin( i1, i2, i3 ) 2 | 3 | % fbkin Forward and Inverse Kinematics of Floating Base 4 | % [x,xd]=fbkin(q,qd,qdd) calculates the forward kinematics, and 5 | % [q,qd,qdd]=fbkin(x,xd) calculates the inverse kinematics, of a spatial 6 | % floating base constructed by floatbase. The vectors q, qd and qdd are 7 | % the joint position, velocity and acceleration variables for the six 8 | % joints forming the floating base; x is the 13-element singularity-free 9 | % state vector used by FDfb and IDfb; and xd is its derivative. The 10 | % component parts of x are: a unit quaternion specifying the orientation of 11 | % the floating-base frame relative to the fixed base, a 3D vector giving 12 | % the position of the origin of the floating-base frame, and the spatial 13 | % velocity of the floating base expressed in fixed-base coordinates. If 14 | % the acceleration calculation is not needed then use the simpler calls 15 | % x=fbkin(q,qd) and [q,qd]=fbkin(x); and if the velocity calculation is 16 | % also not needed then use p=fbkin(q) and q=fbkin(p) (or q=fbkin(x)), where 17 | % p contains the first 7 elements of x. If the length of the first 18 | % argument is 7 or 13 then it is assumed to be p or x, otherwise it is 19 | % assumed to be q. All vectors are expected to be column vectors. The 20 | % returned value of q(5) is normalized to the range -pi/2 to pi/2; and the 21 | % returned values of q(4) and q(6) are normalized to the range -pi to pi. 22 | % The calculation of qd and qdd fails if the floating base is in one of its 23 | % two kinematic singularities, which occur when q(5)==+/-pi/2. However, 24 | % the calculation of q is robust, and remains accurate both in and close to 25 | % the singularities. 26 | 27 | if length(i1) == 13 || length(i1) == 7 28 | if nargout == 3 29 | [o1, o2, o3] = invkin( i1, i2 ); 30 | elseif nargout == 2 31 | [o1, o2] = invkin( i1 ); 32 | else 33 | o1 = invkin( i1 ); 34 | end 35 | else 36 | if nargout == 2 37 | [o1, o2] = fwdkin( i1, i2, i3 ); 38 | elseif nargin == 2 39 | o1 = fwdkin( i1, i2 ); 40 | else 41 | o1 = fwdkin( i1 ); 42 | end 43 | end 44 | 45 | 46 | 47 | function [x, xd] = fwdkin( q, qd, qdd ) 48 | 49 | c4 = cos(q(4)); s4 = sin(q(4)); 50 | c5 = cos(q(5)); s5 = sin(q(5)); 51 | c6 = cos(q(6)); s6 = sin(q(6)); 52 | 53 | E = [ c5*c6, c4*s6+s4*s5*c6, s4*s6-c4*s5*c6; 54 | -c5*s6, c4*c6-s4*s5*s6, s4*c6+c4*s5*s6; 55 | s5, -s4*c5, c4*c5 ]; 56 | 57 | qn = rq(E); % unit quaternion fixed-->floating 58 | r = q(1:3); % position of floating-base origin 59 | 60 | x(1:4,1) = qn; 61 | x(5:7) = r; 62 | 63 | if nargin > 1 % do velocity calculation 64 | 65 | S = [ 1 0 s5; 66 | 0 c4 -s4*c5; 67 | 0 s4 c4*c5 ]; 68 | 69 | omega = S*qd(4:6); 70 | rd = qd(1:3); % lin vel of floating-base origin 71 | 72 | v = [ omega; rd+cross(r,omega) ]; % spatial vel in fixed-base coords 73 | 74 | x(8:13) = v; 75 | end 76 | 77 | if nargout == 2 % calculate xd 78 | 79 | c4d = -s4*qd(4); s4d = c4*qd(4); 80 | c5d = -s5*qd(5); s5d = c5*qd(5); 81 | 82 | Sd = [ 0 0 s5d; 83 | 0 c4d -s4d*c5-s4*c5d; 84 | 0 s4d c4d*c5+c4*c5d ]; 85 | 86 | omegad = S*qdd(4:6) + Sd*qd(4:6); 87 | rdd = qdd(1:3); 88 | % spatial accn in fixed-base coords 89 | a = [ omegad; rdd+cross(rd,omega)+cross(r,omegad) ]; 90 | 91 | xd(1:4,1) = rqd( omega, rq(E) ); 92 | xd(5:7) = rd; 93 | xd(8:13) = a; 94 | end 95 | 96 | 97 | 98 | function [q, qd, qdd] = invkin( x, xd ) 99 | 100 | E = rq(x(1:4)); % coord xfm fixed-->floating 101 | r = x(5:7); % position of floating-base origin 102 | 103 | q(1:3,1) = r; 104 | 105 | q(5) = atan2( E(3,1), sqrt(E(1,1)*E(1,1)+E(2,1)*E(2,1)) ); 106 | q(6) = atan2( -E(2,1), E(1,1) ); 107 | if E(3,1) > 0 108 | q(4) = atan2( E(2,3)+E(1,2), E(2,2)-E(1,3) ) - q(6); 109 | else 110 | q(4) = atan2( E(2,3)-E(1,2), E(2,2)+E(1,3) ) + q(6); 111 | end 112 | if q(4) > pi 113 | q(4) = q(4) - 2*pi; 114 | elseif q(4) < -pi 115 | q(4) = q(4) + 2*pi; 116 | end 117 | 118 | if nargout > 1 % calculate qd 119 | 120 | c4 = cos(q(4)); s4 = sin(q(4)); 121 | c5 = cos(q(5)); s5 = sin(q(5)); 122 | 123 | S = [ 1 0 s5; 124 | 0 c4 -s4*c5; 125 | 0 s4 c4*c5 ]; 126 | 127 | omega = x(8:10); 128 | rd = x(11:13) - cross(r,omega); % lin vel of floating-base origin 129 | 130 | qd(1:3,1) = rd; 131 | qd(4:6) = S \ omega; % this will fail at a singularity 132 | end 133 | 134 | if nargout == 3 % calculate qdd 135 | 136 | c4d = -s4*qd(4); s4d = c4*qd(4); 137 | c5d = -s5*qd(5); s5d = c5*qd(5); 138 | 139 | Sd = [ 0 0 s5d; 140 | 0 c4d -s4d*c5-s4*c5d; 141 | 0 s4d c4d*c5+c4*c5d ]; 142 | 143 | omegad = xd(8:10); 144 | rdd = xd(11:13) - cross(rd,omega) - cross(r,omegad); 145 | 146 | qdd(1:3,1) = rdd; 147 | qdd(4:6) = S \ (omegad - Sd*qd(4:6)); % this will fail at a singularity 148 | end 149 | -------------------------------------------------------------------------------- /Common/spatial_v2/models/vee.m: -------------------------------------------------------------------------------- 1 | function robot = vee( L2 ) 2 | 3 | % VEE free-floating 1R robot consisting of two rods 4 | % vee(L2) creates a free-floating robot consisting of two thin rods 5 | % connected by a revolute joint with an axis at 45 degrees to the axis of 6 | % each rod. When the joint variables are zero, rod 1 lies on the X axis, 7 | % rod 2 lies on the Y axis, and the connecting joint lies on the line x=y, 8 | % z=0. Rod 1 has a length of 1, and rod 2 has a length of L2. If the 9 | % length argument is omitted then it defaults to 1. The rods have a radius 10 | % of 0.01, and their masses equal their lengths. 11 | 12 | if nargin < 1 13 | L2 = 1; 14 | end 15 | 16 | % For efficient use with Simulink, this function builds a new model the 17 | % first time it is called, and stores it in the persistent variable 18 | % 'memory'; and it stores the corresponding length argument in 'memory_L2'. 19 | % On each subsequent occasion that it is called, it compares the L2 value 20 | % with memory_L2, and if they are the same then it simply returns the 21 | % stored model instead of creating a new one. 22 | 23 | persistent memory memory_L2; 24 | 25 | if length(memory) ~= 0 && memory_L2 == L2 26 | robot = memory; 27 | return 28 | end 29 | 30 | % To make a new vee model, we first create a two-link robot with two 31 | % revolute joints. The first joint is just a place-holder, as it will be 32 | % replaced with a chain of six joints when floatbase is called near the end 33 | % of this function. When that happens, link 1 (=rod 1) will become link 6, 34 | % link 2 will become link 7, and joint 2 (the real joint) will become joint 35 | % 7. For now, the first step is to define the connectivity and kinematics 36 | % of a two-link robot. 37 | 38 | robot.NB = 2; 39 | robot.parent = [0 1]; 40 | robot.jtype = {'R', 'R'}; 41 | 42 | robot.Xtree = { eye(6), roty(pi/2) * rotz(pi/4) }; 43 | 44 | robot.gravity = [0 0 0]; % zero gravity is not the default, 45 | % so it must be stated explicitly 46 | 47 | % The next step is to calculate the inertias of the two links, assuming 48 | % they are uniform solid cylinders of radius 0.01. Note that L2 is both 49 | % the length and mass of link 2. One little complication here is that rod 50 | % 1 lies on the X axis of its local coordinate system, but rod 2 lies on 51 | % the line x=0, y=z of its local coordinate system. We get over this by 52 | % first defining the inertia of rod 2 as if it were lying on the Y axis, 53 | % and then rotate it by pi/4 about the X axis. 54 | 55 | Ibig1 = 1.0003/12; 56 | Ibig2 = L2*(L2^2+0.0003)/12; 57 | Ismall1 = 0.00005; 58 | Ismall2 = L2*0.00005; 59 | rod1 = mcI( 1, [0.5,0,0], diag([Ismall1,Ibig1,Ibig1]) ); 60 | rod2 = mcI( L2, [0,L2/2,0], diag([Ibig2,Ismall2,Ibig2]) ); 61 | 62 | % The next two lines rotate rod 2 by pi/4 about the X axis. Note that X2 63 | % is actually a cordinate transform, but it is being used as a rotation 64 | % operator on this occasion. 65 | 66 | X2 = rotx(pi/4); 67 | rod2 = X2' * rod2 * X2; 68 | 69 | robot.I = { rod1, rod2 }; 70 | 71 | % The next step is to draw some visual guide lines in the fixed coordinate 72 | % system, to help the viewer understand the motion. Lines are drawn on the 73 | % X axis, the Y axis, a line at 45 degrees to these axes, and a line 74 | % joining the two centres of mass. 75 | 76 | robot.appearance.base = ... 77 | { 'line', [1.1 0 0; 0 0 0; 0 1.1 0], ... 78 | 'line', [0 0 0; 0.8 0.8 0], ... 79 | 'line', [0.55 -L2*0.05 0; -0.05 L2*0.55 0] }; 80 | 81 | % The next step is to draw the two bodies. A small red sphere marks each 82 | % centre of mass. Observe that rod 2 lies at 45 degrees to the Y and Z 83 | % axes in its local coordinate system, so the cylinder representing it is 84 | % drawn at [0 0 0; 0 L2 L2]/sqrt(2). Similar comments apply to the 85 | % cylinder in link 1 that models the appearance of the revolute joint. 86 | 87 | robot.appearance.body{1} = ... 88 | { 'cyl', [0 0 0; 1 0 0], 0.01, ... 89 | 'cyl', [0.02 0.02 0; -0.02 -0.02 0]/sqrt(2), 0.02, ... 90 | 'colour', [0.9 0 0], ... 91 | 'sphere', [0.5 0 0], 0.015 }; 92 | 93 | robot.appearance.body{2} = ... 94 | { 'cyl', [0 0 0; 0 L2 L2]/sqrt(2), 0.01, ... 95 | 'cyl', [0 0 0.02; 0 0 0.06], 0.02, ... 96 | 'colour', [0.9 0 0], ... 97 | 'sphere', [0 L2/2 L2/2]/sqrt(2), 0.015 }; 98 | 99 | % To further assist the viewer, if L2 takes a value other than 1 then three 100 | % small orange spheres are added to the drawing instructions: one at the 101 | % system centre of mass, and one embedded in each rod at the point where 102 | % that rod intersects with the system CoM. 103 | 104 | if L2 ~= 1 105 | 106 | Lcm = (1+L2^2) / (1+L2) / 2; 107 | robot.appearance.body{1} = ... 108 | [ robot.appearance.body{1}, ... 109 | { 'colour', [0.9 0.5 0], 'sphere', [Lcm 0 0], 0.015 } ]; 110 | 111 | robot.appearance.body{2} = ... 112 | [ robot.appearance.body{2}, ... 113 | { 'colour', [0.9 0.5 0], ... 114 | 'sphere', [0 sqrt(0.5) sqrt(0.5)]*Lcm, 0.015 } ]; 115 | 116 | sys_CoM = [0.5 L2^2/2 0] / (1+L2); % system centre of mass 117 | robot.appearance.base = ... 118 | [ robot.appearance.base, ... 119 | { 'colour', [0.9 0.5 0], 'sphere', sys_CoM, 0.015 } ]; 120 | end 121 | 122 | % And finally, we call floatbase to replace joint 1 with a chain of six 123 | % joints (three prismatic and three revolute), and store a copy of the new 124 | % robot in 'memory'. 125 | 126 | robot = floatbase(robot); 127 | 128 | memory = robot; 129 | memory_L2 = L2; 130 | -------------------------------------------------------------------------------- /Utils/Graphics/QuadGraphics.m: -------------------------------------------------------------------------------- 1 | classdef QuadGraphics < handle 2 | properties 3 | quad 4 | bodyLength 5 | bodyHeight 6 | thighLength 7 | shankLength 8 | linkWidth 9 | trajs = [] 10 | im 11 | end 12 | methods 13 | function G = QuadGraphics() 14 | quadParams = get3DMCParams(); 15 | G.quad = PlanarQuadruped(0.001); 16 | build2DminiCheetah(G.quad); 17 | 18 | %% link data w.r.t. local frame 19 | G.bodyLength = quadParams.bodyLength+0.04; 20 | G.bodyHeight = quadParams.bodyHeight+0.04; 21 | G.thighLength = quadParams.hipLinkLength; 22 | G.shankLength = quadParams.kneeLinkLength; 23 | G.linkWidth = 0.03; 24 | end 25 | end 26 | 27 | methods 28 | function addTrajectory(G, T) 29 | G.trajs{end+1} = T; 30 | end 31 | 32 | function visualizeTrajectory(G, idx) 33 | if nargin < 2 34 | idx = 1; 35 | end 36 | if isempty(G.trajs) 37 | fprintf('No trajectories exisit \n') 38 | return 39 | end 40 | T = G.trajs{idx}; 41 | 42 | figure(198); 43 | ground = drawGround(-0.404); 44 | hold on; 45 | quadG = G.drawQuad(); 46 | ylim([-1 3]); 47 | axis([-1 3 -1 1]); 48 | axis equal 49 | axis manual 50 | 51 | % update quadruped graphics 52 | for k=1:size(T,2) 53 | pos = T(1:2, k); 54 | pitch = T(3, k); 55 | q = T(4:7, k); 56 | G.updateQuad(quadG, pos, pitch, q); 57 | pause(0.001); 58 | end 59 | end 60 | 61 | function compareTrajectory(G, indices) 62 | if nargin < 2 63 | indices = [1, 2]; 64 | end 65 | 66 | if length(G.trajs) < 2 67 | fprintf('Not sufficient trajectories exist for comparing \n'); 68 | fprintf('Visualizing the first trajectory \n'); 69 | G.visualizeTrajectory(); 70 | return 71 | end 72 | 73 | Ts= G.trajs(indices); 74 | num_trajs = length(Ts); 75 | 76 | f = figure(200); 77 | ground = drawGround(-0.404); 78 | hold on; 79 | quadGs = {}; 80 | for i = 1:num_trajs 81 | quadGs{i} = G.drawQuad(); 82 | end 83 | ylim([-1 3]); 84 | axis([-1 3 -1 1]); 85 | axis equal 86 | axis manual 87 | 88 | [~, nc] = cellfun(@size, Ts); 89 | maxlen = max(nc); 90 | for k = 1:maxlen 91 | for i = 1:num_trajs 92 | if k <= size(Ts{i}, 2) 93 | pos = Ts{i}(1:2,k); 94 | pitch = Ts{i}(3,k); 95 | q = Ts{i}(4:7,k); 96 | G.updateQuad(quadGs{i}, pos, pitch, q); 97 | end 98 | end 99 | pause(0.001); 100 | im_idx = floor(k/10)+1; 101 | frame{im_idx} = getframe(f); 102 | im{im_idx} = frame2im(frame{im_idx}); 103 | im_idx = im_idx + 1; 104 | end 105 | G.im = im; 106 | end 107 | 108 | function save_animation(G) 109 | filename = 'mc_animation.gif'; 110 | for k = 1:length(G.im) 111 | [imind,cm] = rgb2ind(G.im{k},256); 112 | if k == 1 113 | imwrite(imind,cm,filename,'gif', 'Loopcount',inf,'DelayTime',0.05); 114 | elseif k==length(G.im) 115 | imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',1); 116 | else 117 | imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',0.05); 118 | end 119 | end 120 | end 121 | end 122 | methods 123 | function quadGraphics = drawQuad(G) 124 | % Draw body 125 | quadGraphics(1) = drawBody2D(G.bodyLength, G.bodyHeight, [0 0.4470 0.7410]); 126 | % Draw front thigh 127 | quadGraphics(2) = drawLink2D(G.linkWidth, G.thighLength, [0.7647,0.7686,0.7882]); 128 | % Draw front shank 129 | quadGraphics(3) = drawLink2D(G.linkWidth, G.shankLength, [0.7647,0.7686,0.7882]); 130 | % Draw back thigh 131 | quadGraphics(4) = drawLink2D(G.linkWidth, G.thighLength, [0.7647,0.7686,0.7882]); 132 | % Draw back shank 133 | quadGraphics(5) = drawLink2D(G.linkWidth, G.shankLength, [0.7647,0.7686,0.7882]); 134 | end 135 | 136 | function updateQuad(G, quados, pos, pitch, q) 137 | C = [pos; pitch; q]; 138 | for idx = 1:5 139 | v = G.quad.getPosition(C, idx, quados(idx).v'); 140 | set(quados(idx).p, 'vertices', v'); 141 | set(quados(idx).p, 'visible', 'on'); 142 | end 143 | end 144 | end 145 | end -------------------------------------------------------------------------------- /HSDDP/Phase.m: -------------------------------------------------------------------------------- 1 | classdef Phase< handle 2 | properties 3 | dynamics 4 | resetmap 5 | dynamics_partial 6 | resetmap_partial 7 | running_cost 8 | terminal_cost 9 | running_cost_partial 10 | terminal_cost_partial 11 | constraint 12 | additional_initialization = []; 13 | AL_params = []; 14 | ReB_params = []; 15 | end 16 | 17 | methods 18 | % Default constructor 19 | function P = Phase() 20 | P.constraint = ConstraintContainer(); 21 | end 22 | end 23 | 24 | methods 25 | function set_dynamics(P, dynamics_funcs) 26 | % dynamics_funcs: 1x2 cell array of function handles of dynamics and dynamics 27 | % partials 28 | P.dynamics = dynamics_funcs{1}; 29 | P.dynamics_partial = dynamics_funcs{2}; 30 | end 31 | function set_resetmap(P, reset_funcs) 32 | P.resetmap = reset_funcs{1}; 33 | P.resetmap_partial = reset_funcs{2}; 34 | end 35 | function set_running_cost(P, rcost_funcs) 36 | P.running_cost = rcost_funcs{1}; 37 | P.running_cost_partial = rcost_funcs{2}; 38 | end 39 | function set_terminal_cost(P, tcost_funcs) 40 | P.terminal_cost = tcost_funcs{1}; 41 | P.terminal_cost_partial = tcost_funcs{2}; 42 | end 43 | function add_path_constraints(P, pconstrObj) 44 | P.constraint.add_pathConstraint(pconstrObj); 45 | end 46 | function add_terminal_constraints(P, tconstrObj) 47 | P.constraint.add_terminalConstraint(tconstrObj); 48 | end 49 | function initialize_parameters(P) 50 | P.AL_params = P.constraint.get_al_params(); 51 | P.ReB_params = P.constraint.get_reb_params(); 52 | end 53 | function set_additional_initialization(P, init_func) 54 | P.additional_initialization = init_func; 55 | end 56 | end 57 | methods (Access = private) 58 | function p_array = compute_pConstraints(P, x, u, y) 59 | p_array = P.constraint.compute_pConstraints(x,u,y); 60 | end 61 | function t_array = compute_tConstraints(P, x) 62 | t_array = P.constraint.compute_tConstraints(x); 63 | end 64 | end 65 | methods 66 | function [h, success] = forwardsweep(P, traj, eps, options) 67 | N = traj.len; 68 | Xbar = traj.Xbar; 69 | Ubar = traj.Ubar; 70 | dU = traj.dU; 71 | K = traj.K; 72 | traj.V = 0; 73 | success = 1; 74 | for k = 1:N-1 75 | x = traj.X{k}; 76 | u = Ubar{k} + eps*dU{k} + K{k} * (x - Xbar{k}); 77 | [x_next, y] = P.dynamics(x, u); 78 | l = P.running_cost(k, x, u, y); 79 | lpar = P.running_cost_partial(k, x, u, y); 80 | [A, B, C, D] = P.dynamics_partial(x, u); 81 | if options.pconstraint_active 82 | pconstrData = P.compute_pConstraints(x, u, y); 83 | [l, lpar] = update_rcost_with_pconstraint(l, lpar, pconstrData, P.ReB_params, traj.dt); 84 | end 85 | if any(isnan(u)) 86 | success = 0; 87 | break; 88 | end 89 | traj.U{k} = u; 90 | traj.X{k+1} = x_next; 91 | traj.Y{k} = y; 92 | traj.l{k} = l; 93 | traj.lpar{k} = lpar; 94 | traj.A{k} = A; 95 | traj.B{k} = B; 96 | traj.C{k} = C; 97 | traj.D{k} = D; 98 | traj.V = traj.V + l; 99 | end 100 | phi = P.terminal_cost(traj.X{end}); 101 | phi_par = P.terminal_cost_partial(traj.X{end}); 102 | tconstrData = P.compute_tConstraints(traj.X{end}); 103 | if options.tconstraint_active 104 | [phi, phi_par] = update_tcost_with_tconstraint(phi, phi_par, tconstrData, P.AL_params); 105 | end 106 | traj.phi = phi; 107 | traj.phi_par = phi_par; 108 | traj.V = traj.V + phi; 109 | h = []; 110 | if ~isempty(tconstrData) 111 | h = [tconstrData(:).h]; % row vector if not empty 112 | end 113 | end 114 | 115 | function success = backwardsweep(P, traj, Gprime, Hprime, dVprime, regularization) 116 | N = traj.len; 117 | success = 1; 118 | traj.G{end} = traj.phi_par.G + Gprime; 119 | traj.H{end} = traj.phi_par.H + Hprime; 120 | traj.dV = dVprime; 121 | for k = N-1:-1:1 122 | % Gradient and Hessian of value approx at next step 123 | Gk_next = traj.G{k+1}; 124 | Hk_next = traj.H{k+1}; 125 | 126 | % Dynamics linearization at current step 127 | Ak = traj.A{k}; Bk = traj.B{k}; 128 | Ck = traj.C{k}; Dk = traj.D{k}; 129 | 130 | % Compute Q info 131 | Qx = traj.lpar{k}.lx + Ak'*Gk_next + Ck'*traj.lpar{k}.ly; 132 | Qu = traj.lpar{k}.lu + Bk'*Gk_next + Dk'*traj.lpar{k}.ly; 133 | Qxx = traj.lpar{k}.lxx + Ck'*traj.lpar{k}.lyy*Ck + Ak'*Hk_next*Ak; 134 | Quu = traj.lpar{k}.luu + Dk'*traj.lpar{k}.lyy*Dk + Bk'*Hk_next*Bk; 135 | Qux = traj.lpar{k}.lux + Dk'*traj.lpar{k}.lyy*Ck + Bk'*Hk_next*Ak; 136 | 137 | % regularization 138 | Qxx = Qxx + eye(size(Qxx))*regularization; 139 | Quu = Quu + eye(size(Quu))*regularization; 140 | 141 | [~, p] = chol(Quu-eye(size(Quu))*1e-9); 142 | if p ~= 0 143 | success = 0; 144 | break; 145 | end 146 | 147 | % Standard equations 148 | Quu_inv = Sym(eye(size(Quu))/Quu); 149 | traj.dU{k} = -Quu_inv*Qu; 150 | traj.K{k} = -Quu_inv*Qux; 151 | traj.G{k} = Qx - (Qux')*(Quu_inv* Qu ); 152 | traj.H{k} = Qxx - (Qux')*(Quu_inv* Qux); 153 | traj.dV = traj.dV - Qu'*(Quu\Qu); 154 | 155 | if any(isnan(traj.dU{k})) || any(isnan(traj.G{k})) 156 | error('error: du or Vx has nan at k = %d', k); 157 | end 158 | 159 | end 160 | end 161 | end 162 | end -------------------------------------------------------------------------------- /Common/spatial_v2/models/gfxExample.m: -------------------------------------------------------------------------------- 1 | function robot = gfxExample(arg) 2 | 3 | % gfxExample example of how to produce showmotion drawing instructions 4 | % gfxExample creates a simple planar 2R robot, kinematically identical to 5 | % that produced by planar(2), but adorned with a variety of extra graphics. 6 | % Read the source code to see how it's done. To view this robot, type 7 | % showmotion(gfxExample). 8 | 9 | % The following code creates a simple planar 2R robot, identical to planar(2). 10 | 11 | robot.NB = 2; 12 | robot.parent = [0,1]; 13 | 14 | robot.jtype{1} = 'r'; 15 | robot.jtype{2} = 'r'; 16 | 17 | robot.Xtree{1} = plnr( 0, [0 0]); 18 | robot.Xtree{2} = plnr( 0, [1 0]); 19 | 20 | robot.I{1} = mcI( 1, [0.5 0], 1/12 ); 21 | robot.I{2} = mcI( 1, [0.5 0], 1/12 ); 22 | 23 | % From here on, it's graphics 24 | 25 | % To illustrate the use of 'vertices' and 'triangles' in making a general 26 | % polyhedron, here is some code to make a small roof-like shape. First, we 27 | % define a 6x3 array containing the coordinates of the 6 vertices. Vertex 28 | % numbers 1, 2, 3 and 4 define the rectangular base, and vertices 5 and 6 29 | % define the top edge. 30 | 31 | roofV = [-0.2 -0.1 0.4; ... 32 | 0.1 -0.1 0.4; ... 33 | 0.1 0.1 0.4; ... 34 | -0.2 0.1 0.4; ... 35 | -0.1 0 0.5; ... 36 | 0 0 0.5]; 37 | 38 | % The roof consists of one rectangle, two quadrilaterals and two 39 | % triangles. The rectangle and quadrilaterals each require two triangles, 40 | % so the whole roof consists of 8 triangles. However, we are going to draw 41 | % the roof in two colours; so we must create one list of triangles for each 42 | % colour. In "roofT1" below, the first two triangles make the -y facing 43 | % quadrilateral; the next two make the +y facing quadrilateral; and the 44 | % final two make the rectangular base. The two triangular faces at each 45 | % end of the roof are defined in "roofT2". Observe that each triangle's 46 | % vertices are listed in counter-clockwise order as seen from outside 47 | % the roof. 48 | 49 | roofT1 = [1 2 6; 1 6 5; 3 4 5; 3 5 6; 3 2 1; 3 1 4]; 50 | roofT2 = [2 3 6; 4 1 5]; 51 | 52 | % Note: in the default initial view, +x is pointing to the right, +z is up, 53 | % and +y is pointing away from the camera. 54 | 55 | % The next command creates the drawing instructions for the base. To 56 | % illustrate how to use 'tiles' to create rectangles facing in different 57 | % directions, the first line creates a large floor with large tiles, and 58 | % the next five lines create a tiled rectangular platform for the robot to 59 | % sit on. Specifically, lines 2 to 5 create the sides of the platform 60 | % facing -y, +y, -x and +x, in that order, and line 6 makes the top of the 61 | % platform. The next line makes a box representing the robot's base. The 62 | % bottom of this box is a little below the top of the platform. Finally, 63 | % the last four lines do the following: declare the roof's vertices; make 64 | % the quadrilateral faces of the roof; change the colour; and make the 65 | % triangular faces. Both the box and the quadrilateral faces of the roof 66 | % appear in the default colour for body 0, which is brown, but tiled 67 | % surfaces are always grey regardless of the current colour. 68 | 69 | robot.appearance.base = ... 70 | { 'tiles', [-1 2; -1 1; -0.3 -0.3], 0.4, ... 71 | 'tiles', [-0.4 0.45; -0.4 -1; -0.3 -0.15], 0.1, ... 72 | 'tiles', [-0.4 0.45; 0.4 0.4; -0.3 -0.15], 0.1, ... 73 | 'tiles', [-0.4 -1; -0.4 0.4; -0.3 -0.15], 0.1, ... 74 | 'tiles', [0.45 0.45; -0.4 0.4; -0.3 -0.15], 0.1, ... 75 | 'tiles', [-0.4 0.45; -0.4 0.4; -0.15 -0.15], 0.1, ... 76 | 'box', [-0.2 -0.3 -0.2; 0.2 0.3 -0.07], ... 77 | 'vertices', roofV, ... 78 | 'triangles', roofT1, ... 79 | 'colour', [0.45 0.2 0.35], ... 80 | 'triangles', roofT2 }; 81 | 82 | % For body 1, we are going to use some sublists. The following sublist 83 | % creates a very crude sphere, with a very low facet level of 8. 84 | 85 | crude = { 'facets', 8, ... 86 | 'sphere', [0.2 0 0.2], 0.1 }; 87 | 88 | % Observe that "crude" is a cell array. Because of this, it will be 89 | % interpreted as a sublist when it is included in the main list of drawing 90 | % instructions; and so the scope of the 'facets' instruction includes only 91 | % the one sphere that follows it. 92 | 93 | % Let us also create a high-quality two-tone sphere: 94 | 95 | posh = { 'facets', 80, ... 96 | 'colour', [0 0.5 0; 0.4 0.8 0.4], ... 97 | 'sphere', [0.4 0 0.2], 0.08 }; 98 | 99 | % Once again, this is a cell array, and so the 'facets' and 'colour' 100 | % instructions will affect only the one sphere following it. 101 | 102 | % The next command creates the drawing instructions for body 1. The first 103 | % two lines in the list create a box and a cylinder, which together make a 104 | % sensible shape for link 1 of a planar 2R robot. The next line is a 105 | % sublist that begins with a 'colour' command to change the current colour 106 | % to blue. The next item in this sublist is the cell array in the variable 107 | % "posh", which is therefore a subsublist in this context. As the cell 108 | % array in "posh" specifies both a colour (actually two colours) and a 109 | % value for facets, it is drawn in the specified colour (two shades of 110 | % green) and at the specified facet level of 80. The last item in the 111 | % sublist is the cell array in the variable "crude". As this subsublist 112 | % specifies a facet level but not a colour, the crude sphere will be drawn 113 | % at the specified facet level of 8, but in the inherited current colour. 114 | % Now, the colour declaration in "posh" is local to the subsublist it 115 | % appears in, so the current colour for the crude sphere is blue. The 116 | % final two items in the main list draw a line and a sphere. Observe that 117 | % they are both drawn in the default colour for body 1 (red), and that the 118 | % sphere is drawn at the default facet level of 24. 119 | 120 | robot.appearance.body{1} = ... 121 | { 'box', [0 -0.07 -0.04; 1 0.07 0.04], ... 122 | 'cyl', [0 0 -0.07; 0 0 0.07], 0.1, ... 123 | { 'colour', [0.1 0.3 0.6], posh, crude }, ... 124 | 'line', [0 0 0; 0 0 0.2; 0.8 0 0.2], ... 125 | 'sphere', [0.6 0 0.2], 0.06 }; 126 | 127 | % For anything remotely complicated, or anything that might be needed more 128 | % than once, it is a good idea to define a function. As an example, the 129 | % function below makes the drawing instructions for a circle. 130 | 131 | function circ = circle( centre, radius, direction ) 132 | % first make sure centre and direction are row vectors 133 | centre = [centre(1) centre(2) centre(3)]; 134 | direction = [direction(1) direction(2) direction(3)]; 135 | % now work out the plane of the circle 136 | Z = direction/norm(direction); 137 | if abs(Z(3)) < 0.5 138 | Y = [0 0 1]; 139 | else 140 | Y = [0 1 0]; 141 | end 142 | Y = Y - Z * dot(Y,Z); 143 | Y = Y / norm(Y); 144 | X = cross(Y,Z); 145 | % unit vectors X and Y now span the circle's plane 146 | % next step: make the coordinates of a unit circle 147 | ang = (0:100)' * 2*pi / 100; 148 | coords = cos(ang)*X + sin(ang)*Y; 149 | % now scale it to correct radius and shift it to correct position 150 | coords = radius*coords + ones(101,1)*centre; 151 | % finally, return a cell array with the drawing instructions 152 | circ = { 'line', coords }; 153 | end 154 | 155 | % The next command makes the drawing instructions for body 2. The first 156 | % two lines in the list make a box and a cylinder, which together make a 157 | % sensible shape for link 2 in a planar 2R robot. The next several lines 158 | % then make circles in various locations and colours. The final two lines 159 | % make a brilliant white triangle with apex at point (1.1,0,0) in link 2 160 | % coordinates, which is relevant to the camera commands below. 161 | 162 | robot.appearance.body{2} = ... 163 | { 'box', [0 -0.07 -0.04; 1 0.07 0.04], ... 164 | 'cyl', [0 0 -0.07; 0 0 0.07], 0.1, ... 165 | 'colour', [0 0.8 0.8], ... 166 | circle( [0.2 0 0], 0.15, [1 0 0] ), ... 167 | 'colour', [0 0.75 0.8], ... 168 | circle( [0.25 0 0], 0.14, [1 0 0] ), ... 169 | 'colour', [0 0.7 0.8], ... 170 | circle( [0.3 0 0], 0.13, [1 0 0] ), ... 171 | 'colour', [0 0.65 0.8], ... 172 | circle( [0.35 0 0], 0.12, [1 0 0] ), ... 173 | 'colour', [0 0.6 0.8], ... 174 | circle( [0.4 0 0], 0.11, [1 0 0] ), ... 175 | 'colour', [0 0.55 0.8], ... 176 | circle( [0.45 0 0], 0.1, [1 0 0] ), ... 177 | 'colour', [0.8 0.5 0.7], ... 178 | circle( [0 0 0], 0.2, [1 0 1] ), ... 179 | 'colour', [1 1 1], ... 180 | 'line', [1.1 0 0; 1.02 0.04 0; 1.02 -0.04 0; 1.1 0 0] }; 181 | 182 | % Showmotion supports two kinds of camera: a fixed camera and a tracking 183 | % camera. The latter tracks a specified point in a specified body as it 184 | % moves. To be more accurate, it tracks the horizontal component of the 185 | % specified point's motion. You get a fixed camera by default. To get a 186 | % tracking camera, you have to include in the model a field called camera, 187 | % which is a structure containing various view-related fields. To specify 188 | % a tracking camera, you have to set .camera.body to a nonzero body number 189 | % and .camera.trackpoint to the coordinates of the point you want to 190 | % track. If you call gfxExample with an argument -- any argument -- then 191 | % it will give you a tracking camera that follows the apex of the white 192 | % triangle on body 2. The code that does this is below. Try zooming in on 193 | % the white triangle, and try watching it from different viewpoints. (Also 194 | % try slowing down the animation, and don't watch it too many times, or 195 | % you'll get sea-sick. Tracking cameras work best with mobile robots.) 196 | 197 | if nargin > 0 198 | robot.camera.body = 2; 199 | robot.camera.trackpoint = [1.1 0 0]; 200 | end 201 | 202 | % Other camera fields you might want to experiment with are: .direction, 203 | % .up, .zoom and .locus. 204 | 205 | end 206 | -------------------------------------------------------------------------------- /HSDDP/HSDDP.m: -------------------------------------------------------------------------------- 1 | classdef HSDDP < handle 2 | properties %(Access = private) 3 | x0 4 | n_phases 5 | len_phases 6 | phases 7 | hybridT % array of phase trajectory 8 | end 9 | 10 | properties 11 | V = 0; 12 | dV = 0; 13 | h_all % terminal constraint collected in cell array 14 | maxViolation = 0; 15 | end 16 | 17 | 18 | methods %(constructor) 19 | function DDP = HSDDP(phases_in, hybridT_in) 20 | if nargin > 0 21 | DDP.Initialization(phases_in, hybridT_in); 22 | end 23 | end 24 | 25 | function Initialization(DDP, phases_in, hybridT_in) 26 | assert(length(phases_in) == length(hybridT_in), 'Fail to construct HSDDP. Dimensions of Phases and HybridTrajectories do not match.'); 27 | DDP.phases = phases_in; 28 | DDP.hybridT = hybridT_in; 29 | DDP.n_phases = length(phases_in); 30 | DDP.len_phases = [hybridT_in(:).len]; 31 | DDP.h_all = cell(DDP.n_phases,1); 32 | end 33 | end 34 | 35 | methods 36 | function success = forwardsweep(DDP, eps, options) 37 | DDP.V = 0; 38 | success = 1; 39 | assert(isequal(DDP.x0, DDP.hybridT(1).Xbar{1}), 'Initial conditions do not match.'); 40 | for idx = 1:DDP.n_phases 41 | if idx == 1 42 | xinit = DDP.x0; 43 | else 44 | xinit = DDP.phases(idx-1).resetmap(DDP.hybridT(idx-1).X{end}); 45 | DDP.hybridT(idx-1).Px = DDP.phases(idx-1).resetmap_partial(DDP.hybridT(idx-1).X{end}); 46 | end 47 | DDP.hybridT(idx).X{1} = xinit; 48 | if ~isempty(DDP.phases(idx).additional_initialization) 49 | % This executes foothold planner under the scene for trunk model 50 | DDP.phases(idx).additional_initialization (xinit); 51 | end 52 | 53 | [DDP.h_all{idx}, success] = DDP.phases(idx).forwardsweep(DDP.hybridT(idx), eps, options); 54 | if ~success 55 | break; 56 | end 57 | % Accumulates the total actual cost 58 | DDP.V = DDP.V + DDP.hybridT(idx).V; 59 | end 60 | % Maximum terminal constraint violation of all types over all 61 | % time instants 62 | DDP.maxViolation = max(cellfun(@(x) norm(x, Inf), DDP.h_all)); 63 | if isempty(DDP.maxViolation) 64 | DDP.maxViolation = 0; 65 | end 66 | end 67 | 68 | function forwarditeration(DDP, options) 69 | eps = 1; 70 | Vprev = DDP.V; 71 | options.parCalc_active = 0; 72 | while eps > 1e-10 73 | success = DDP.forwardsweep(eps, options); 74 | 75 | if options.Debug 76 | fprintf('\t eps=%.3e \t cost change=%.3e \t min=%.3e\n',eps, DDP.V-Vprev, options.gamma* eps*(1-eps/2)*DDP.dV ); 77 | if ~success 78 | fprintf('forward sweep fails because of Nan \n'); 79 | fprintf('reduce step size \n'); 80 | end 81 | end 82 | 83 | if success 84 | % If forward_sweep succeeds 85 | % check if V is small enough to accept the step 86 | if DDP.V < Vprev + options.gamma*eps*(1-eps/2)*DDP.dV 87 | break; 88 | end 89 | end 90 | 91 | % Else backtrack 92 | eps = options.alpha*eps; 93 | end 94 | end 95 | 96 | function success = backwardsweep(DDP, regularization) 97 | success = 1; 98 | for idx = DDP.n_phases:-1:1 99 | if idx == DDP.n_phases 100 | Gprime = zeros(size(DDP.hybridT(idx).G{1})); 101 | Hprime = zeros(size(DDP.hybridT(idx).H{1})); 102 | dVprime = 0; 103 | else 104 | Gprime = DDP.hybridT(idx+1).G{1}; 105 | Hprime = DDP.hybridT(idx+1).H{1}; 106 | dVprime = DDP.hybridT(idx+1).dV; 107 | if ~isempty(DDP.phases(idx).resetmap) 108 | [Gprime, Hprime] = impact_aware_step(DDP.hybridT(idx).Px, Gprime, Hprime); 109 | end 110 | end 111 | success = DDP.phases(idx).backwardsweep(DDP.hybridT(idx), Gprime, Hprime, dVprime, regularization); 112 | if ~success 113 | break; 114 | end 115 | end 116 | DDP.dV = DDP.hybridT(1).dV; 117 | end 118 | 119 | function reg = backwardsweep_w_reg(DDP, reg, options) 120 | success = 0; 121 | while ~success 122 | if options.Debug 123 | fprintf('\t reg=%.3e\n',reg); 124 | end 125 | if reg> 1e04 126 | error('Regularization exeeds allowed value') 127 | end 128 | success = DDP.backwardsweep(reg); 129 | if success 130 | break; 131 | end 132 | reg= max(reg*options.beta_reg, 1e-3); 133 | end 134 | end 135 | 136 | function [xopt, uopt, Kopt, Info] = solve(DDP, options) 137 | Info.ou_iters = 0; 138 | Info.in_iters = []; 139 | Info.max_violations = []; 140 | Info.al_params = []; 141 | 142 | xopt = cell(1,DDP.n_phases); 143 | uopt = cell(1,DDP.n_phases); 144 | Kopt = cell(1,DDP.n_phases); 145 | DDP.initialize_params(); 146 | ou_iter = 0; 147 | pconstraint_active = options.pconstraint_active; 148 | while 1 % Implement AL and ReB in outer loop 149 | ou_iter = ou_iter + 1; 150 | if options.Debug 151 | fprintf('====================================================\n'); 152 | fprintf('\t Outer loop Iteration %3d\n',ou_iter); 153 | end 154 | 155 | % Initial sweep 156 | if pconstraint_active 157 | if ((DDP.maxViolation > 0.05) || (ou_iter == 1)) 158 | % Path constraint is not considered in the first AL 159 | % iteration and when switching constraint violation is 160 | % too large 161 | options.pconstraint_active = 0; 162 | else 163 | options.pconstraint_active = 1; 164 | end 165 | end 166 | 167 | Info.ou_iters = ou_iter; 168 | Info.al_params{end+1} = DDP.phases(:).AL_params; 169 | 170 | DDP.forwardsweep(1, options); 171 | DDP.updateNominalTrajectory(); 172 | 173 | if ou_iter == 1 174 | % Maximum violation with initial guess 175 | Info.max_violations(end+1) = DDP.maxViolation; 176 | end 177 | 178 | Vprev = DDP.V; 179 | regularization = 0; 180 | in_iter = 0; 181 | while 1 % DDP in inner loop 182 | in_iter = in_iter + 1; 183 | if options.Debug 184 | fprintf('================================================\n'); 185 | fprintf('\t Inner loop Iteration %3d\n',in_iter); 186 | end 187 | 188 | % backward sweep with regularization 189 | regularization = DDP.backwardsweep_w_reg(regularization, options); 190 | 191 | % reduce regularization term 192 | regularization = regularization/20; 193 | if regularization < 1e-6 194 | regularization = 0; 195 | end 196 | % linear search 197 | DDP.forwarditeration(options); 198 | % Update nominal trajectory once linear search is 199 | % successful 200 | DDP.updateNominalTrajectory(); 201 | 202 | if (in_iter>=options.max_DDP_iter) || (Vprev - DDP.V < options.DDP_thresh) 203 | Info.in_iters(end+1) = in_iter; 204 | Info.max_violations(end+1) = DDP.maxViolation; 205 | break 206 | end 207 | Vprev = DDP.V; 208 | end 209 | fprintf('Maximum terminal constraint violation %.4f\n',DDP.maxViolation); 210 | if (ou_iter>=options.max_AL_iter) || (DDP.maxViolation < options.AL_thresh) 211 | if options.one_more_sweep 212 | DDP.mod_al_params_last_sweep(); 213 | DDP.forwardsweep(1, options); 214 | DDP.backwardsweep_w_reg(regularization, options); 215 | end 216 | break; 217 | end 218 | ou_iter = ou_iter + 1; 219 | DDP.update_ReB_params(options); 220 | DDP.update_AL_params(options); 221 | end 222 | 223 | for idx = 1:DDP.n_phases 224 | xopt{idx} = DDP.hybridT(idx).Xbar; 225 | uopt{idx} = DDP.hybridT(idx).Ubar; 226 | Kopt{idx} = DDP.hybridT(idx).K; 227 | end 228 | end 229 | end 230 | 231 | methods 232 | function set_initial_condition(DDP, x0) 233 | DDP.x0 = x0; 234 | end 235 | 236 | function updateNominalTrajectory(DDP) 237 | for idx = 1:DDP.n_phases 238 | DDP.hybridT(idx).updateNominal(); 239 | end 240 | end 241 | end 242 | 243 | methods 244 | function initialize_params(DDP) 245 | for i = 1:DDP.n_phases 246 | DDP.phases(i).initialize_parameters(); 247 | end 248 | end 249 | function update_ReB_params(DDP, options) 250 | if ~options.pconstraint_active 251 | % If path constraint is not active, do nothing 252 | return 253 | end 254 | for i = 1:DDP.n_phases 255 | % get ReB parameters for the current phase 256 | reb_params = DDP.phases(i).ReB_params; 257 | for j = 1:length(reb_params) 258 | reb_params(j).eps = options.beta_ReB* reb_params(j).eps; 259 | reb_params(j).delta = options.beta_relax*reb_params(j).delta; 260 | if reb_params(j).delta < reb_params(j).delta_min 261 | reb_params(j).delta = reb_params(j).delta_min; 262 | end 263 | end 264 | DDP.phases(i).ReB_params = reb_params; 265 | end 266 | end 267 | function update_AL_params(DDP, options) 268 | if ~options.tconstraint_active 269 | % If terminal constraint is not active, do nothing 270 | return 271 | end 272 | for i = 1:DDP.n_phases 273 | h = DDP.h_all{i}; % get terminal constraints at current phase 274 | if (~isempty(DDP.phases(i).AL_params)) && (~isempty(h)) 275 | for j = 1:length(DDP.phases(i).AL_params) 276 | lambda = DDP.phases(i).AL_params(j).lambda; 277 | sigma = DDP.phases(i).AL_params(j).sigma; 278 | lambda = lambda + h(j)*sigma; 279 | if abs(h(j)) > 0.01 280 | sigma = options.beta_penalty * sigma; 281 | end 282 | DDP.phases(i).AL_params(j).lambda = lambda; 283 | DDP.phases(i).AL_params(j).sigma = sigma; 284 | end 285 | end 286 | end 287 | end 288 | function mod_al_params_last_sweep(DDP) 289 | for i = 1:DDP.n_phases 290 | for j = 1:length(DDP.phases(i).AL_params) 291 | DDP.phases(i).AL_params(j).sigma = 0; 292 | end 293 | end 294 | end 295 | end 296 | end --------------------------------------------------------------------------------