├── ControlDesign ├── InverseKinematics │ ├── animateFootGait.m │ ├── calculateInvKin.mlx │ ├── evalFootGait.m │ └── legInvKin.m ├── MPCHumanoid │ ├── FIFOBuffer.m │ ├── designMPCWalkingPatternGen.mlx │ ├── invertedPendulum3D.png │ ├── lipm.jpg │ ├── setParams_sm_stick_humanoid.m │ ├── sphereData.mat │ └── testMPCWalkingPatternGen.slx ├── README.md ├── plotWalkingTraj.m ├── robotParametersCtrl.m ├── walkingRobot3DMPC.slx └── walkingRobot3DPath.slx ├── LIPM ├── README.md ├── animateInverseKinematics.mlx ├── animateLIPM.mlx ├── animateLIPMLocal.mlx ├── applicationLIPM.mlapp ├── defaultfootinfos.mat ├── defaultsimulationinputs.mat ├── defaultstepinfos.mat ├── getSwingFootTraj.m ├── interactiveRobotLIPM.slx ├── invKinBody2Foot.m └── walkingRobotLIPM.slx ├── Libraries ├── MultiphysicsLib │ ├── Libraries │ │ ├── +forcesPS │ │ │ ├── +hydraulic │ │ │ │ ├── PS_force_hydraulic_chamber_trans_comp.ssc │ │ │ │ ├── PS_force_hydraulic_chamber_trans_comp.svg │ │ │ │ ├── PS_force_hydraulic_chamber_trans_incomp.ssc │ │ │ │ ├── PS_force_hydraulic_chamber_trans_incomp.svg │ │ │ │ ├── lib.m │ │ │ │ └── sscprj │ │ │ │ │ ├── PS_force_hydraulic_chamber_trans_comp.pmdlg │ │ │ │ │ └── PS_force_hydraulic_chamber_trans_incomp.pmdlg │ │ │ ├── +math │ │ │ │ ├── PS_q_axs_to_qz.ssc │ │ │ │ ├── lib.m │ │ │ │ └── sscprj │ │ │ │ │ └── PS_q_axs_to_qz.pmdlg │ │ │ ├── +mechanical │ │ │ │ ├── PS_force_friction_rot.ssc │ │ │ │ ├── PS_force_friction_rot.svg │ │ │ │ ├── PS_force_friction_trans.ssc │ │ │ │ ├── PS_force_friction_trans.svg │ │ │ │ ├── PS_force_hardstop_rot.ssc │ │ │ │ ├── PS_force_hardstop_rot.svg │ │ │ │ ├── PS_force_hardstop_trans.ssc │ │ │ │ ├── PS_force_hardstop_trans.svg │ │ │ │ ├── lib.m │ │ │ │ └── sscprj │ │ │ │ │ ├── PS_force_friction_rot.pmdlg │ │ │ │ │ ├── PS_force_friction_trans.pmdlg │ │ │ │ │ ├── PS_force_hardstop_rot.pmdlg │ │ │ │ │ └── PS_force_hardstop_trans.pmdlg │ │ │ └── lib.m │ │ ├── Multibody_Multiphysics_Lib.slx │ │ ├── forcesPS_lib.slx │ │ └── slblocks.m │ ├── README.txt │ └── startup_sm_ssci.m └── walkingRobotUtils.slx ├── ModelingSimulation ├── Intermediate │ ├── initConditionPicker.mlapp │ ├── jointAngs.mat │ ├── walkingRobot_step1_mechanics.slx │ ├── walkingRobot_step2_motion.slx │ ├── walkingRobot_step3_contact.slx │ └── walkingRobot_step4_library.slx ├── README.md ├── compareActuatorTypes.m ├── robotParameters.m └── walkingRobot.slx ├── Optimization ├── README.md ├── SavedResults │ ├── optimizedData_01Nov19_1143.mat │ ├── optimizedData_03Nov19_1140.mat │ └── optimizedData_31Oct19_0741.mat ├── createSmoothTrajectory.m ├── doSpeedupTasks.m ├── optimizeRobotMotion.mlx ├── simulateWalkingRobot.m └── walkingRobotOptim.slx ├── README.md ├── ReinforcementLearning ├── README.md ├── createDDPGNetworks.m ├── createDDPGOptions.m ├── createWalkingAgent2D.m ├── createWalkingAgent3D.m ├── plotTrainingResults.m ├── robotParametersRL.m ├── savedAgents │ ├── trainedAgent_2D_1.mat │ ├── trainedAgent_2D_2.mat │ ├── trainedAgent_2D_3.mat │ ├── trainedAgent_2D_4.mat │ ├── trainedAgent_3D_1.mat │ ├── trainedAgent_3D_2.mat │ ├── trainedAgent_3D_3.mat │ ├── trainedAgent_3D_4.mat │ ├── trainedAgent_3D_5.mat │ ├── trainedAgent_3D_6.mat │ ├── trainingResults_2D_02_22_2019_1713.mat │ ├── trainingResults_2D_02_27_2019_1008.mat │ └── trainingResults_3D_11_04_2019_0102.mat ├── walkerInvKin.m ├── walkerResetFcn.m ├── walkingRobotRL2D.slx └── walkingRobotRL3D.slx └── startupWalkingRobot.m /ControlDesign/InverseKinematics/animateFootGait.m: -------------------------------------------------------------------------------- 1 | % Visualize Walking Gait and Inverse Kinematics 2 | % Copyright 2019 The MathWorks, Inc. 3 | 4 | close all 5 | robotParametersCtrl; 6 | 7 | %% Plot the foot trajectory 8 | gaitPeriod = 1; 9 | stepLength = 0.1; 10 | stepHeight = 0.025; 11 | numPoints = 150; 12 | 13 | tVec = linspace(0,gaitPeriod,numPoints); 14 | foot_height_offset = sqrt( (lower_leg_length+upper_leg_length)^2 ... 15 | - ((stepLength/2)*100)^2 ) - 1e-3; 16 | 17 | figure, hold on 18 | x = zeros(numPoints,1); 19 | y = zeros(numPoints,1); 20 | for idx = 1:numPoints 21 | [x(idx),y(idx)] = evalFootGait(tVec(idx),stepLength,stepHeight,gaitPeriod); 22 | end 23 | 24 | plot(x,y,'.-'); 25 | axis equal 26 | title('Foot Gait'); 27 | xlabel('x [m]') 28 | ylabel('y [m]') 29 | 30 | %% Calculate joint angles 31 | theta_hip = zeros(numPoints,1); 32 | theta_knee = zeros(numPoints,1); 33 | theta_ankle = zeros(numPoints,1); 34 | 35 | for idx = 1:numPoints 36 | 37 | pitch = 0; % Assume zero body pitch 38 | 39 | % Calculate inverse kinematics 40 | theta = legInvKin(upper_leg_length/100, lower_leg_length/100 , ... 41 | x(idx), y(idx) - (foot_height_offset/100)); 42 | 43 | % Address multiple solutions by preventing knee bending backwards 44 | if size(theta,1) == 2 45 | if theta(1,2) > 0 46 | t1 = theta(2,1); 47 | t2 = theta(2,2); 48 | else 49 | t1 = theta(1,1); 50 | t2 = theta(1,2); 51 | end 52 | else 53 | t1 = theta(1); 54 | t2 = theta(2); 55 | end 56 | 57 | % Pack the results. Ensure the ankle angle is set so the foot always 58 | % lands flat on the ground 59 | theta_hip(idx) = t1; 60 | theta_knee(idx) = t2; 61 | theta_ankle(idx) = -(t1+t2); 62 | 63 | end 64 | 65 | % Display joint angles 66 | figure 67 | subplot(311) 68 | plot(tVec,rad2deg(theta_hip)) 69 | title('Hip Angle [deg]'); 70 | subplot(312) 71 | plot(tVec,rad2deg(theta_knee)) 72 | title('Knee Angle [deg]'); 73 | subplot(313) 74 | plot(tVec,rad2deg(theta_ankle)) 75 | title('Ankle Angle [deg]'); 76 | 77 | %% Animate the walking gait 78 | figure(3), clf, hold on 79 | 80 | % Initialize plot 81 | plot(x,y-foot_height_offset/100,'k:','LineWidth',1); 82 | h1 = plot([0 0],[0 0],'r-','LineWidth',4); 83 | h2 = plot([0 0],[0 0],'b-','LineWidth',4); 84 | 85 | % Calculate knee and ankle (x,y) positions 86 | xKnee = sin(theta_hip)*upper_leg_length/100; 87 | yKnee = -cos(theta_hip)*upper_leg_length/100; 88 | xAnkle = xKnee + sin(theta_hip+theta_knee)*lower_leg_length/100; 89 | yAnkle = yKnee - cos(theta_hip+theta_knee)*lower_leg_length/100; 90 | 91 | % Define axis limits 92 | xMin = min([xKnee;xAnkle]) -0.025; 93 | xMax = max([xKnee;xAnkle]) +0.025; 94 | yMin = min([yKnee;yAnkle]) -0.025; 95 | yMax = max([0;yKnee;yAnkle]) +0.025; 96 | 97 | % Animate the walking gait 98 | numAnimations = 5; 99 | for anim = 1:numAnimations 100 | for idx = 1:numPoints 101 | set(h1,'xdata',[0 xKnee(idx)],'ydata',[0 yKnee(idx)]); 102 | set(h2,'xdata',[xKnee(idx) xAnkle(idx)],'ydata',[yKnee(idx) yAnkle(idx)]); 103 | xlim([xMin xMax]), ylim([yMin yMax]); 104 | title('Walking Gait Animation'); 105 | axis equal 106 | drawnow 107 | end 108 | end -------------------------------------------------------------------------------- /ControlDesign/InverseKinematics/calculateInvKin.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ControlDesign/InverseKinematics/calculateInvKin.mlx -------------------------------------------------------------------------------- /ControlDesign/InverseKinematics/evalFootGait.m: -------------------------------------------------------------------------------- 1 | function [x,y] = evalFootGait(t,stepLength,stepHeight,gaitPeriod) 2 | % EVALFOOTGAIT Evaluates foot gait at a specified time 3 | % 4 | % Copyright 2019 The MathWorks, Inc. 5 | 6 | % t = 0 : extended forward 7 | % 0 >= t > T/2 : dragging back 8 | % T/2 >= t > T : swinging forward 9 | 10 | tEff = mod(t,gaitPeriod); 11 | 12 | % Dragging back (y position is 0) 13 | if tEff < gaitPeriod/2 14 | x = stepLength * (gaitPeriod - 2*tEff)/gaitPeriod - stepLength/2; 15 | y = 0; 16 | 17 | % Swinging forward (y follows curve) 18 | else 19 | x = stepLength * (2*tEff - gaitPeriod)/gaitPeriod - stepLength/2; 20 | y = stepHeight*(1 - (4*abs(tEff - 3*gaitPeriod/4)/gaitPeriod)^2); 21 | end 22 | 23 | end -------------------------------------------------------------------------------- /ControlDesign/InverseKinematics/legInvKin.m: -------------------------------------------------------------------------------- 1 | function out1 = legInvKin(L1,L2,x,y) 2 | %LEGINVKIN 3 | % OUT1 = LEGINVKIN(L1,L2,X,Y) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.2. 6 | % 02-Nov-2018 22:21:58 7 | 8 | t2 = L1.^2; 9 | t3 = L2.^2; 10 | t4 = x.^2; 11 | t5 = y.^2; 12 | t6 = L1.*L2.*2.0; 13 | t7 = -t2-t3+t4+t5+t6; 14 | t8 = t2+t3-t4-t5+t6; 15 | t9 = t7.*t8; 16 | t10 = sqrt(t9); 17 | t11 = 1.0./t7; 18 | t12 = t2-t3+t4+t5-L1.*y.*2.0; 19 | t13 = 1.0./t12; 20 | t14 = L1.*x.*2.0; 21 | t15 = t4.*t10.*t11; 22 | t16 = t5.*t10.*t11; 23 | t17 = L1.*L2.*t10.*t11.*2.0; 24 | t18 = t10.*t11; 25 | t19 = atan(t18); 26 | out1 = reshape([atan(t13.*(t14+t15+t16+t17-t2.*t10.*t11-t3.*t10.*t11)).*2.0,atan(t13.*(t14-t15-t16-t17+t2.*t10.*t11+t3.*t10.*t11)).*2.0,t19.*-2.0,t19.*2.0],[2,2]); 27 | -------------------------------------------------------------------------------- /ControlDesign/MPCHumanoid/FIFOBuffer.m: -------------------------------------------------------------------------------- 1 | classdef FIFOBuffer < matlab.System & matlab.system.mixin.Propagates ... 2 | & matlab.system.mixin.CustomIcon & matlab.system.mixin.SampleTime 3 | % FIFOBuffer Implements a FIFO Buffer 4 | % 5 | % Copyright 2018-2019 The MathWorks, Inc. 6 | 7 | % Dependent properties 8 | properties(Access=private, Dependent) 9 | NumElements 10 | Elements 11 | end 12 | 13 | % Public, non-tunable properties 14 | properties(Nontunable) 15 | % Buffer capacity 16 | Capacity = 5; 17 | 18 | % Number of states 19 | NumStates = 2; 20 | 21 | % Sample time 22 | SampleTime = 5e-3; 23 | 24 | end 25 | 26 | properties(Logical, Nontunable) 27 | % Fill buffer at startup 28 | FillBufferAtStartup = false; 29 | end 30 | 31 | properties(Nontunable) 32 | % Buffer data at startup [numStates x capacity] 33 | BufferStartupFillupData = zeros(2,5); 34 | end 35 | 36 | properties(DiscreteState) 37 | 38 | end 39 | 40 | % Pre-computed constants 41 | properties(Access = private) 42 | pIndexIn = 1; 43 | pIndexOut = 1; 44 | pNumElements = 0; 45 | pBuffer; 46 | end 47 | 48 | methods 49 | % Constructor 50 | function obj = FIFOBuffer(varargin) 51 | % Support name-value pair arguments when constructing object 52 | setProperties(obj,nargin,varargin{:}) 53 | end 54 | end 55 | 56 | methods(Access = protected) 57 | %% My functions 58 | 59 | 60 | %% Common functions 61 | function setupImpl(obj) 62 | % Perform one-time calculations, such as computing constants 63 | obj.initialize(); 64 | 65 | % Fill buffer at startup 66 | if obj.FillBufferAtStartup 67 | obj.fillBuffer(obj.BufferStartupFillupData); 68 | end 69 | end 70 | 71 | function [buffer, isFull] = stepImpl(obj,dataIn) 72 | %stepImpl - Add one data sample to buffer and return the 73 | % whole buffer. The way we return the buffer is 74 | % [oldestState, ... newestState] 75 | % If the buffer is not full we add padding to represent 76 | % the oldest values. You can also fill the buffer at startup 77 | % by checking the flag FillBufferAtStartup if you are using 78 | % simulink. Otherwise you can use the fillBuffer method in 79 | % MATLAB. 80 | 81 | obj.push(dataIn); 82 | 83 | paddingValue = zeros(obj.NumStates,1); 84 | buffer = obj.getElementsWithPadding(paddingValue); 85 | 86 | %oldestState = obj.getOldestState(paddingValue); 87 | 88 | %buffer = obj.pBuffer; 89 | isFull = obj.isFull(); 90 | end 91 | 92 | function resetImpl(obj) 93 | % Initialize / reset discrete-state properties 94 | end 95 | 96 | %% Backup/restore functions 97 | function s = saveObjectImpl(obj) 98 | % Set properties in structure s to values in object obj 99 | 100 | % Set public properties and states 101 | s = saveObjectImpl@matlab.System(obj); 102 | 103 | % Set private and protected properties 104 | %s.myproperty = obj.myproperty; 105 | end 106 | 107 | function loadObjectImpl(obj,s,wasLocked) 108 | % Set properties in object obj to values in structure s 109 | 110 | % Set private and protected properties 111 | % obj.myproperty = s.myproperty; 112 | 113 | % Set public properties and states 114 | loadObjectImpl@matlab.System(obj,s,wasLocked); 115 | end 116 | 117 | %% Simulink functions 118 | function ds = getDiscreteStateImpl(obj) 119 | % Return structure of properties with DiscreteState attribute 120 | ds = struct([]); 121 | end 122 | 123 | function validateInputsImpl(obj,dataIn) 124 | % Validate inputs to the step method at initialization 125 | [numStates,~] = size(dataIn); 126 | if obj.NumStates ~= numStates 127 | error('Number of states of input data doesn''t match buffer number of states'); 128 | end 129 | end 130 | 131 | function flag = isInputSizeMutableImpl(obj,index) 132 | % Return false if input size cannot change 133 | % between calls to the System object 134 | flag = false; 135 | end 136 | 137 | function [out1,out2] = getOutputSizeImpl(obj) 138 | % Return size for each output port 139 | out1 = [obj.NumStates obj.Capacity]; 140 | out2 = 1; 141 | end 142 | 143 | function [out1,out2] = getOutputDataTypeImpl(obj) 144 | % Return data type for each output port 145 | out1 = "double"; 146 | out2 = "logical"; 147 | 148 | % Example: inherit data type from first input port 149 | % out = propagatedInputDataType(obj,1); 150 | end 151 | 152 | function [out1,out2] = isOutputComplexImpl(obj) 153 | % Return true for each output port with complex data 154 | out1 = false; 155 | out2 = false; 156 | 157 | % Example: inherit complexity from first input port 158 | % out = propagatedInputComplexity(obj,1); 159 | end 160 | 161 | function [out1,out2] = isOutputFixedSizeImpl(obj) 162 | % Return true for each output port with fixed size 163 | out1 = false; 164 | out2 = true; 165 | 166 | % Example: inherit fixed-size status from first input port 167 | % out = propagatedInputFixedSize(obj,1); 168 | end 169 | 170 | function sts = getSampleTimeImpl(obj) 171 | % Define sample time type and parameters 172 | %sts = obj.createSampleTime("Type", "Inherited"); 173 | 174 | % Example: specify discrete sample time 175 | sts = obj.createSampleTime("Type","Discrete","SampleTime",obj.SampleTime); 176 | end 177 | 178 | function icon = getIconImpl(obj) 179 | % Define icon for System block 180 | icon = mfilename("class"); % Use class name 181 | % icon = "My System"; % Example: text icon 182 | % icon = ["My","System"]; % Example: multi-line text icon 183 | % icon = matlab.system.display.Icon("myicon.jpg"); % Example: image file icon 184 | end 185 | end 186 | 187 | methods(Static, Access = protected) 188 | %% Simulink customization functions 189 | function header = getHeaderImpl 190 | % Define header panel for System block dialog 191 | header = matlab.system.display.Header(mfilename("class")); 192 | end 193 | 194 | function group = getPropertyGroupsImpl 195 | % Define property section(s) for System block dialog 196 | group = matlab.system.display.Section(mfilename("class")); 197 | end 198 | end 199 | 200 | methods 201 | function numElements = get.NumElements(obj) 202 | numElements = obj.pNumElements; 203 | end 204 | 205 | function elements = get.Elements(obj) 206 | if obj.pNumElements >= 1 207 | if obj.pIndexOut < obj.pIndexIn 208 | elements = obj.pBuffer(:,obj.pIndexOut:obj.pIndexIn-1); 209 | else 210 | elements = [obj.pBuffer(:,obj.pIndexOut:end) obj.pBuffer(:,1:obj.pIndexIn-1)]; 211 | end 212 | else 213 | elements = zeros(obj.NumStates,0); 214 | end 215 | end 216 | 217 | function elements = getElementsWithPadding(obj,paddingValue) 218 | elements = zeros(obj.NumStates,obj.Capacity); 219 | if obj.pNumElements >= 1 && obj.pNumElements == obj.Capacity 220 | elements = obj.Elements(:,1:obj.pNumElements); 221 | elseif obj.pNumElements >=1 && obj.NumElements < obj.Capacity 222 | elements = [repmat(paddingValue, 1, obj.Capacity-obj.pNumElements),obj.Elements(:,1:obj.pNumElements)]; 223 | else 224 | error('case not considered'); 225 | end 226 | end 227 | 228 | function rawBuffer = getRawBuffer(obj) 229 | rawBuffer = obj.pBuffer; 230 | end 231 | 232 | function initialize(obj) 233 | obj.pBuffer = zeros(obj.NumStates,obj.Capacity); 234 | obj.pIndexIn = 1; 235 | obj.pIndexOut = 1; 236 | obj.pNumElements = 0; 237 | end 238 | 239 | function isFull = isFull(obj) 240 | if(obj.pNumElements == obj.Capacity) 241 | isFull = true; 242 | else 243 | isFull = false; 244 | end 245 | end 246 | 247 | function success = push(obj, data) 248 | % Add new element to buffer 249 | obj.pBuffer(:,obj.pIndexIn) = data; 250 | 251 | % Update new dataIn index 252 | obj.pIndexIn = mod(obj.pIndexIn,obj.Capacity)+1; 253 | 254 | % If buffer is full we overwrite (Move outIndex in one) 255 | % If not full we increment the elements 256 | if obj.isFull() 257 | obj.pIndexOut = mod(obj.pIndexOut,obj.Capacity)+1; 258 | else 259 | % Update elements 260 | obj.pNumElements = obj.pNumElements + 1; 261 | end 262 | 263 | success = true; 264 | end 265 | 266 | function data = pop(obj) 267 | % If no elements do nothing 268 | if obj.pNumElements < 1 269 | data = zeros(obj.NumStates,0); 270 | else 271 | % If it has elements take out oldest item 272 | data = obj.pBuffer(:,obj.pIndexOut); 273 | % Update IndexOut 274 | obj.pIndexOut = mod(obj.pIndexOut,obj.Capacity)+1; 275 | % Update elements 276 | obj.pNumElements = obj.pNumElements - 1; 277 | end 278 | end 279 | 280 | function data = getOldestState(obj,paddingValue) 281 | if obj.pNumElements >= 1 282 | data = obj.pBuffer(:,obj.pIndexOut); 283 | else 284 | data = paddingValue; 285 | end 286 | end 287 | 288 | function fillBuffer(obj, bufferData) 289 | % Validate inputs 290 | [r,c] = size(bufferData); 291 | if r ~= obj.NumStates || c ~= obj.Capacity 292 | error('FIFOBuffer::Buffer data is not the correct size'); 293 | end 294 | 295 | % Clean buffer and reset index 296 | obj.initialize(); 297 | % Fill up buffer 298 | obj.pBuffer = bufferData; 299 | obj.pIndexOut = 1; 300 | obj.pIndexIn = 1; 301 | obj.pNumElements = obj.Capacity; 302 | end 303 | 304 | end 305 | end 306 | -------------------------------------------------------------------------------- /ControlDesign/MPCHumanoid/designMPCWalkingPatternGen.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ControlDesign/MPCHumanoid/designMPCWalkingPatternGen.mlx -------------------------------------------------------------------------------- /ControlDesign/MPCHumanoid/invertedPendulum3D.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ControlDesign/MPCHumanoid/invertedPendulum3D.png -------------------------------------------------------------------------------- /ControlDesign/MPCHumanoid/lipm.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ControlDesign/MPCHumanoid/lipm.jpg -------------------------------------------------------------------------------- /ControlDesign/MPCHumanoid/setParams_sm_stick_humanoid.m: -------------------------------------------------------------------------------- 1 | % Copyright 2018 The MathWorks, Inc. 2 | 3 | % Initialize parameters 4 | 5 | waistW = 2 * 0.037; % j_pelvis_l to base_link_to_body 6 | thighH = 0.093; % j_tibia_l to j_thigh2_l 7 | calfH = 0.093; % j_ankle1_l to j_tibia_l 8 | footL = 0.05; % TODO not measured yet 9 | footW = 0.045; % TODO not measured yet 0.06 10 | footH = 0.005; % TODO not measured yet 11 | footCom2ankle = 0; % TODO not measured yet 12 | trunkH = 0.09355; % j_pelvis_l to base_link_to_body 13 | neckH = 0.0235; % j_pan to base_link_to_body 14 | backW = 2*0.0635; % j_back_l to base_link_to_body 15 | armH = 0.06; % j_low_arm_l to j_high_arm_l 16 | radiusH = 0.048 + 0.048; % j_gripper_l to j_wrist_l to j_low_arm_l 17 | handH = 0.02; % TODO not measured yet 18 | handW = 0.005; % TODO not measured yet 19 | handL = 0.02; % TODO not measured yet 20 | headR = 0.025; % TODO not measured yet 21 | linkWidth = 0.015; % Width for all the links. 22 | 23 | stickModelMassLink = 0.2; % kg Only for stick model 24 | 25 | 26 | % Compute other parameters 27 | waistCom2hip = waistW/2; 28 | thighCom2hip = thighH/2; 29 | thighCom2knee = thighH/2; 30 | calfCom2knee = calfH/2; 31 | calfCom2ankle = calfH/2; 32 | footLF = 0.5*footL; % 0.7* 33 | footLB = footL - footLF; % back 34 | footWE = 0.4*footW; % 0.4 35 | footWI = footW - footWE; 36 | armCom2shoulder = armH/2; 37 | armCom2elbow = armH/2; 38 | radiusCom2elbow = radiusH/2; 39 | radiusCom2hand = radiusH/2; 40 | 41 | % Figure color 42 | human2color = [1.0 0 0]; -------------------------------------------------------------------------------- /ControlDesign/MPCHumanoid/sphereData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ControlDesign/MPCHumanoid/sphereData.mat -------------------------------------------------------------------------------- /ControlDesign/MPCHumanoid/testMPCWalkingPatternGen.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ControlDesign/MPCHumanoid/testMPCWalkingPatternGen.slx -------------------------------------------------------------------------------- /ControlDesign/README.md: -------------------------------------------------------------------------------- 1 | # Walking Robots -- Control Design 2 | ### Copyright 2019 The MathWorks, Inc. 3 | 4 | These files demonstrate 3D walking control for a bipedal humanoid robot. 5 | 6 | Requires MATLAB R2019b or later. 7 | 8 | ## MAIN FOLDER 9 | 10 | * `walkingRobot3DPath.slx` -- Simulink model showing the full 3D control. This robot has different parameters 11 | compared to the original example, to show the robustness of this control with a 12 | more realistic robot (thinner torso, smaller feet, less world damping, etc.). 13 | This uses the `robotParameters3D.m` data file. 14 | 15 | * `plotWalkingTraj.m` -- MATLAB script that takes logged data from the previous model and animates the 16 | trajectory for the torso and the feet. 17 | 18 | * `walkingRobot3DMPC.slx` -- Simulink model showing the full 3D control with a trajectory generated using 19 | Model Predictive Control (MPC). 20 | 21 | ## MPCHUMANOID FOLDER 22 | Contains utility files for the MPC trajectory generation example. 23 | 24 | * `designMPCWalkingPatternGen.mlx` -- Live Script showing how to design the MPC Controllers for stable walking patterns. 25 | 26 | * `testMPCWalkingPatternGen.slx` -- Simple linear inverted pendulum model to test the generated trajectories. 27 | 28 | ## INVERSEKINEMATICS FOLDER 29 | Contains utilities and examples for leg inverse kinematics, which are used by the main control design and reinforcement learning models. 30 | 31 | * `calculateInvKin.mlx` -- This Live Script is used to derive an analytical expression to compute leg 32 | inverse kinematics in the longitudinal/sagittal direction. It generates a MATLAB function named `legInvKin.m`. 33 | 34 | * `animateFootGait.m` -- Script that animates the walking gait according to the specified parameters, and 35 | tests the inverse kinematics calculation above. -------------------------------------------------------------------------------- /ControlDesign/plotWalkingTraj.m: -------------------------------------------------------------------------------- 1 | % Plot 2D Walking Robot Trajectory 2 | % Currently, this works with the logged data from walkingRobot3DPath.slx 3 | % 4 | % Copyright 2019 The MathWorks, Inc. 5 | 6 | %% Check if there is an existing data log 7 | if ~exist('logsout','var') 8 | error('No data log found. Run the simulation first.'); 9 | end 10 | 11 | %% Setup 12 | % Create figure and graphics objects 13 | figure(1), clf, hold on 14 | hRef = plot(pathX,pathY,'b-'); 15 | trajX = 0; 16 | trajY = 0; 17 | hTraj = plot(trajX,trajY,'k:','LineWidth',2); % Trajectory 18 | hCom = plot(0,0,'go','MarkerSize',10,'LineWidth',3); % Center of mass 19 | hRight = plot(0,0,'rs','MarkerSize',10,'LineWidth',3); % Right foot 20 | hLeft = plot(0,0,'bs','MarkerSize',10,'LineWidth',3); % Leg foot 21 | hHead = plot([0 0],[0 0],'g-','LineWidth',2); % Torso heading 22 | hLeg = plot([0 0],[0 0],'b-','LineWidth',2); % Leg line 23 | grid on 24 | 25 | % Get data logs 26 | % Footsteps are tracked at the sample time of the stepping 27 | tStep = get(logsout,'stepFwd').Values.Time; 28 | stepFwd = get(logsout,'stepFwd').Values.Data; 29 | stepLat = get(logsout,'stepLat').Values.Data; 30 | 31 | % Get the center of mass data and interpolate it to be an integer 32 | % multiple of the stepping sample time. 33 | % For example, N = 10 means 10 trajectory points 34 | % per footstep. 35 | N = 10; 36 | tTraj = linspace(0,tStep(end),1 + (numel(tStep)-1)*N)'; 37 | measBody = get(logsout,'measBody').Values; 38 | X = interp1(measBody.X.Time,measBody.X.Data,tTraj); 39 | Y = interp1(measBody.Y.Time,measBody.Y.Data,tTraj); 40 | Q = interp1(measBody.Q.Time,measBody.Q.Data,tTraj); 41 | 42 | %% Animate 43 | for idx = 2:numel(stepFwd) 44 | % Extract center of mass position and heading 45 | trajIdx = N*(idx-2) + 1; 46 | x = X(trajIdx + N-1); 47 | y = Y(trajIdx + N-1); 48 | R = quat2rotm(Q(trajIdx + N-1,:)); 49 | 50 | % Set graphics 51 | if mod(idx,2) % Left foot at steps 2, 4, 6, 8, ... 52 | footPos = R*[stepFwd(idx);-stepLat(idx);0]/100; 53 | set(hLeft,'xdata',x+footPos(1),'ydata',y+footPos(2)); % Left leg 54 | set(hRight,'xdata',[],'ydata',[]); 55 | else % Right foot at steps 1, 3, 5, 7, ... 56 | footPos = R*[stepFwd(idx);stepLat(idx);0]/100; 57 | set(hLeft,'xdata',[],'ydata',[]); 58 | set(hRight,'xdata',x+footPos(1),'ydata',y+footPos(2)); % Right leg 59 | end 60 | set(hLeg,'xdata',[x, x+footPos(1)], ... 61 | 'ydata',[y, y+footPos(2)]); 62 | set(hCom,'xdata',x,'ydata',y); 63 | angPos = R*[0.01;0;0]; 64 | set(hHead,'xdata',[x, x+angPos(1)],'ydata',[y, y+angPos(2)]); 65 | trajX = [trajX;X(trajIdx + (0:(N-1)))]; 66 | trajY = [trajY;Y(trajIdx + (0:(N-1)))]; 67 | set(hTraj,'xdata',trajX,'ydata',trajY); 68 | 69 | % Set titles, axis limits, and pause 70 | title(['Robot Walking Trajectory, t = ' num2str(tStep(idx))]); 71 | xlim(x + [-0.1 0.1]) 72 | ylim(y + [-0.1 0.1]) 73 | legend('CoM Reference','CoM Trajectory','CoM Position','Right Foot','Left Foot') 74 | pause(tWalk/2) 75 | 76 | end 77 | 78 | % Expand the view at the end 79 | xlim auto 80 | ylim auto -------------------------------------------------------------------------------- /ControlDesign/robotParametersCtrl.m: -------------------------------------------------------------------------------- 1 | % Walking Robot Parameters -- 3D 2 | % Copyright 2019 The MathWorks, Inc. 3 | 4 | %% General parameters 5 | density = 500; 6 | foot_density = 1000; 7 | if ~exist('actuatorType','var') 8 | actuatorType = 1; 9 | end 10 | world_damping = 1e-3; 11 | world_rot_damping = 5e-2; 12 | 13 | %% Contact/friction parameters 14 | contact_stiffness = 500; 15 | contact_damping = 50; 16 | mu_k = 0.7; 17 | mu_s = 0.9; 18 | mu_vth = 0.01; 19 | height_plane = 0.025; 20 | plane_x = 25; 21 | plane_y = 3; 22 | contact_point_radius = 1e-4; 23 | 24 | %% Foot parameters 25 | foot_x = 5; 26 | foot_y = 4; 27 | foot_z = 1; 28 | foot_offset = [-1 0 0]; 29 | 30 | %% Leg parameters 31 | leg_radius = 0.75; 32 | lower_leg_length = 10; 33 | upper_leg_length = 10; 34 | 35 | %% Torso parameters 36 | torso_y = 8; 37 | torso_x = 5; 38 | torso_z = 8; 39 | torso_offset_z = -2; 40 | torso_offset_x = -1; 41 | mass = (0.01^3)*torso_y*torso_x*torso_z*density; 42 | g = 9.80665; 43 | 44 | %% Derived parameters for initial conditions 45 | h = 18; 46 | init_height = foot_z + h + ... 47 | torso_z/2 + torso_offset_z + height_plane/2; 48 | 49 | init_angs = legInvKin(upper_leg_length/100,lower_leg_length/100,0,-h/100); 50 | init_angs = init_angs(2,:); 51 | init_angs_R = [0 -sum(init_angs) init_angs(2) 0 init_angs(1)]; 52 | init_angs_L = init_angs_R; 53 | 54 | %% Joint parameters 55 | joint_damping = 1; 56 | joint_stiffness = 1; 57 | motion_time_constant = 0.01; 58 | 59 | %% Joint controller parameters 60 | hip_servo_kp = 60; 61 | hip_servo_ki = 10; 62 | hip_servo_kd = 20; 63 | knee_servo_kp = 60; 64 | knee_servo_ki = 5; 65 | knee_servo_kd = 10; 66 | ankle_servo_kp = 20; 67 | ankle_servo_ki = 4; 68 | ankle_servo_kd = 8; 69 | deriv_filter_coeff = 100; 70 | max_torque = 20; 71 | 72 | %% Electric motor parameters 73 | motor_voltage = 24; 74 | motor_resistance = 1; 75 | motor_constant = 0.02; 76 | motor_inertia = 0; 77 | motor_damping = 0; 78 | motor_inductance = 1.2e-6; 79 | gear_ratio = 50; 80 | 81 | %% Walking controller parameters 82 | TsCtrl = 0.05; % Controller sample time (s) 83 | tWalk = 0.3; % Walking gait period (s) 84 | stepHeight = 0.02; % Step height when swinging leg (m) 85 | Kwalk = 0.5; % Walking gain for tracking XYZ foot positions 86 | 87 | %% Sample path for the robot to track 88 | if ~exist('pathType','var') 89 | pathType = 1; 90 | end 91 | if pathType == 1 % Square 92 | pathT = [0 2 15 30 45 60]'; 93 | pathX = [0 0.01 1 1 0 0]'; 94 | pathY = [0 0.01 0 1 1 0]'; 95 | elseif pathType == 2 % Circle 96 | timePts = (0:2.5:60); 97 | pathR = 0.5; 98 | pathT = [0 1 timePts+2]'; 99 | pathX = [0 0.01 pathR*cos(2*pi*timePts/60 - pi/2)]'; 100 | pathY = [0 0.01 pathR*(sin(2*pi*timePts/60 - pi/2)+1)]'; 101 | elseif pathType == 3 % Star 102 | pathT = [0 1 12 24 36 48 60]'; 103 | pathX = [0 0.01 0.75 -0.25 0.5 0.25 0]'; 104 | pathY = [0 0.01 0.5 0.5 0 0.75 0]'; 105 | end 106 | curveT = linspace(0,pathT(end),100)'; 107 | curveX = interp1(pathT,pathX,curveT); 108 | curveY = interp1(pathT,pathY,curveT); 109 | curveZ = zeros(size(curveT)); 110 | interPts = [curveX curveY curveZ]; -------------------------------------------------------------------------------- /ControlDesign/walkingRobot3DMPC.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ControlDesign/walkingRobot3DMPC.slx -------------------------------------------------------------------------------- /ControlDesign/walkingRobot3DPath.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ControlDesign/walkingRobot3DPath.slx -------------------------------------------------------------------------------- /LIPM/README.md: -------------------------------------------------------------------------------- 1 | # Walking Robots -- Linear Inverted Pendulum Model (LIPM) 2 | ### Copyright 2019 The MathWorks, Inc. 3 | 4 | Contains files for linear inverted pendulum model based control of the walking robot simulation. 5 | 6 | Requires MATLAB R2019b or later. 7 | 8 | ## PROCESS FOR CREATING TRAJECTORY 9 | By running the scripts one by one, we create the body's trajectory using the LIPM, then we convert the body's trajectory into joint trajectory. 10 | 11 | 1. `applicationLIPM.mlapp` -- Interactive application to understand how the model behaves 12 | 2. `animateLIPM.mlx` -- Create symmetric walking pattern based on the model 13 | 3. `animateLIPMLocal.mlx` -- Transform the body trajectory to feet trajectory 14 | 4. `animateInverseKinematics.mlx` -- Calculate joint trajectory from feet trajectory 15 | 5. `walkingRobotLIPM.slx` -- Run simulation based on feet trajectory 16 | 17 | ## MODEL FILES 18 | This robot has 6 degrees of freedom per leg. The kinematic structure is based on HUBO. 19 | 20 | * `walkingRobotLIPM.slx` -- Run simulation based on foot trajectory 21 | * `interactiveRobotLIPM.slx` -- Interactively control foot positions to test inverse kinematics and stability 22 | 23 | ## COMMON FILES 24 | * `getSwingFootTraj.m` -- Get swing foot trajectory based on cubic polynomial trajectory 25 | * `invKinBody2Foot.m` -- Calculate joint angles for foot position and orientation 26 | * `defaultstepinfos.mat` -- Variables for running the second script without running the first 27 | * `defaultfootinfos.mat` -- Variables for running the third script without running the second 28 | * `defaultsimulationinputs.mat` -- Variables for running the Simulink Model without running the scripts 29 | 30 | -------------------------------------------------------------------------------- /LIPM/animateInverseKinematics.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/LIPM/animateInverseKinematics.mlx -------------------------------------------------------------------------------- /LIPM/animateLIPM.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/LIPM/animateLIPM.mlx -------------------------------------------------------------------------------- /LIPM/animateLIPMLocal.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/LIPM/animateLIPMLocal.mlx -------------------------------------------------------------------------------- /LIPM/applicationLIPM.mlapp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/LIPM/applicationLIPM.mlapp -------------------------------------------------------------------------------- /LIPM/defaultfootinfos.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/LIPM/defaultfootinfos.mat -------------------------------------------------------------------------------- /LIPM/defaultsimulationinputs.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/LIPM/defaultsimulationinputs.mat -------------------------------------------------------------------------------- /LIPM/defaultstepinfos.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/LIPM/defaultstepinfos.mat -------------------------------------------------------------------------------- /LIPM/getSwingFootTraj.m: -------------------------------------------------------------------------------- 1 | function [q, qd, qdd] = getSwingFootTraj(footpos0, footpos1, swingheight, timestp0, timestpf, Ts) 2 | % Returns cubic polynomial trajectory for the robot's swing foot 3 | % 4 | % Copyright 2019 The MathWorks, Inc. 5 | 6 | % Trajectory for X and Y 7 | waypointsXY = [footpos0(1:2) footpos1(1:2)]; 8 | timestampsXY = [timestp0 timestpf]; 9 | timevecswingXY = timestampsXY(1):Ts:timestampsXY(end); 10 | [XYq, XYqd, XYqdd, ~] = cubicpolytraj(waypointsXY, timestampsXY, timevecswingXY); 11 | 12 | % Trajectory for Z 13 | waypointsZ = [footpos0(3) footpos0(3)+swingheight footpos0(3)]; 14 | timestpMid = (timestp0+timestpf)/2; % Top of swing at midpoint 15 | timestampsZ = [timestp0 timestpMid timestpf]; 16 | timevecswingZ = timestampsZ(1):Ts:timestampsZ(end); 17 | [Zq, Zqd, Zqdd, ~] = cubicpolytraj(waypointsZ, timestampsZ, timevecswingZ); 18 | 19 | % combine xy and z trajectory 20 | q = [XYq; Zq]; 21 | qd = [XYqd; Zqd]; 22 | qdd = [XYqdd; Zqdd]; 23 | 24 | end -------------------------------------------------------------------------------- /LIPM/interactiveRobotLIPM.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/LIPM/interactiveRobotLIPM.slx -------------------------------------------------------------------------------- /LIPM/invKinBody2Foot.m: -------------------------------------------------------------------------------- 1 | function th = invKinBody2Foot(tform, isLeft, varargin) 2 | % This functions expects as input: transform matrix desribing FOOT 3 | % position/orientation w.r.t. body position and orientation 4 | % the inverse kinematics is an analytic solution found in Ali 5 | % et al's 'Closed-Form Inverse Kinematics Joint Solution for Humanoid 6 | % Robots'. To solve the inverse kinematics, Ali sets the base of each leg 7 | % as the end-effector and foot as the base. So we are solving the joint 8 | % angles in reverse order. This way the last three joints (hip pitch, roll, 9 | % yaw) intersect at a point. 10 | % 11 | % Therefore in this function we 12 | % 1) perform the offset to transform body to foot -> base of each leg to foot. 13 | % 2) find the inverse of the transform to get foot to base of each leg. 14 | % 3) Find the analytic solution. 15 | % 16 | % coder.extrinsic('inputParser'); 17 | % p = inputParser; 18 | % p.addParameter('BendKneeForward', false, @islogical); 19 | % p.parse(varargin{:}); 20 | % 21 | % Copyright 2019 The MathWorks, Inc. 22 | 23 | L1 = -0.12; 24 | if isLeft 25 | L1 = -L1; 26 | end 27 | L2 = 0; 28 | L3 = 0.4; % Upper leg length 29 | L4 = 0.38; % Lower leg length 30 | L5 = 0; % Ankle to foot contact offset 31 | 32 | %% 1) Perform some offsets 33 | tform(1,4) = -tform(1,4); 34 | tform = tform - [0 0 0 L1;0 0 0 0;0 0 0 -L2;0 0 0 0]; 35 | % Extract position/orientation information 36 | R = tform(1:3,1:3); 37 | p = tform(1:3, 4); 38 | 39 | %% 2) Get inverse rotation matrix (in this case, a transpose) 40 | Rp = R'; 41 | n = Rp(:,1); 42 | s = Rp(:,2); 43 | a = Rp(:,3); 44 | p = -Rp*p; 45 | 46 | %% 3) Compute nalytic solution from the paper 47 | cos4 = ((p(1)+L5)^2 + p(2)^2 + p(3)^2 - L3^2 - L4^2)/(2*L3*L4); 48 | temp = 1 - cos4^2; 49 | if temp < 0 50 | temp = 0; 51 | disp('Waning: Unable to reach desired end-effector position/orientation'); 52 | end 53 | 54 | th4 = atan2(sqrt(temp),cos4); 55 | % NOTE: you can put -sqrt(temp) to change direction of knee bending 56 | temp = (p(1)+L5)^2+p(2)^2; 57 | if temp < 0 58 | temp = 0; 59 | disp('Warning: Unable to reach desired end-effector position/orientation'); 60 | end 61 | th5 = atan2(-p(3),sqrt(temp))-atan2(sin(th4)*L3,cos(th4)*L3+L4); 62 | th6 = atan2(p(2),-p(1)-L5); 63 | temp = 1-(sin(th6)*a(1)+cos(th6)*a(2))^2; 64 | if temp < 0 65 | temp = 0; 66 | disp('Warning: Unable to reach desired end-effector position/orientation'); 67 | end 68 | th2 = atan2(-sqrt(temp),sin(th6)*a(1)+cos(6)*a(2)); 69 | th2 = th2 + pi/2; % pi/2 offset 70 | th1 = atan2(-sin(th6)*s(1)-cos(th6)*s(2),-sin(th6)*n(1)-cos(th6)*n(2)); 71 | 72 | th345 = atan2(a(3),cos(th6)*a(1)-sin(th6)*a(2)); 73 | th345 = th345 - pi; 74 | th3 = th345 - th4 - th5; 75 | 76 | %% Pack the corresponding joint angles 77 | th = [th1 th2 th3 th4 th5 th6]; -------------------------------------------------------------------------------- /LIPM/walkingRobotLIPM.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/LIPM/walkingRobotLIPM.slx -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+hydraulic/PS_force_hydraulic_chamber_trans_comp.ssc: -------------------------------------------------------------------------------- 1 | component PS_force_hydraulic_chamber_trans_comp 2 | % Translational Hydraulic Chamber Force Compressible 3 | % This block models an ideal transducer that converts hydraulic 4 | % energy in a chamber with a translational output member (e.g., a piston 5 | % head) into a mechanical force on the output member. Fluid 6 | % compressibility is accounted for and the pressure variation due to 7 | % density fluctuations dynamically solved. 8 | % 9 | % Port A is a hydraulic conserving port associated with the chamber 10 | % inlet. Input ports pos and vel are the respective position and velocity 11 | % of the output member. Output port f is the force acting on the output 12 | % member. 13 | 14 | % Copyright 2016-2017 The MathWorks, Inc. 15 | 16 | nodes 17 | A = foundation.hydraulic.hydraulic; % A:right 18 | end 19 | 20 | inputs 21 | velocity = {0, 'm/s'}; % v:left 22 | position = {0, 'm'}; % p:left 23 | end 24 | 25 | outputs 26 | force = {0, 'N'}; % f:left 27 | end 28 | 29 | parameters 30 | area = {5e-4, 'm^2'}; % Piston area 31 | V_dead = {1e-4, 'm^3'}; % Chamber dead volume 32 | k_sh = {1.4, '1' }; % Specific heat ratio 33 | initial_pressure = {0, 'Pa' }; % Initial pressure 34 | end 35 | 36 | parameters (Access = private) 37 | p_atm = {1, 'atm' }; % Atmospheric pressure 38 | rho_g0 = {1.2, 'kg/m^3'}; % Gas density at reference pressure 39 | end 40 | 41 | variables (Access = private) 42 | pressure_chamber = {0, 'Pa' }; % Fluid pressure in the chamber 43 | volume = {1e-4, 'm^3' }; % Chamber volume 44 | q = {0, 'm^3/s'}; % Flow rate leaving the converter 45 | qV = {0, 'm^3/s'}; % Flow rate leaving the converter due to volume changes 46 | qrho = {0, 'm^3/s'}; % Flow rate leaving the converter due to density changes 47 | end 48 | 49 | function setup 50 | % Parameter range checking 51 | if area <= 0 52 | pm_error('simscape:GreaterThanZero','Piston area'); 53 | end 54 | if V_dead <= 0 55 | pm_error('simscape:GreaterThanZero','Chamber dead volume') 56 | end 57 | if k_sh < 1 58 | pm_error('simscape:GreaterThanOrEqual','Specific heat ratio','1') 59 | end 60 | if k_sh >= 2 61 | pm_error('simscape:LessThan','Specific heat ratio','2') 62 | end 63 | if initial_pressure <= {-1, 'atm'} 64 | pm_error('simscape:GreaterThanOrEqual','Initial pressure','absolute zero') 65 | end 66 | 67 | % Derived parameters 68 | rho_g0 = (initial_pressure + {1, 'atm'}) / ( {287.04, 'J/kg/K'} * {293.15, 'K'} ); 69 | 70 | % Initial conditions 71 | pressure_chamber = initial_pressure; 72 | end 73 | 74 | branches 75 | q: A.q -> *; 76 | end 77 | 78 | equations 79 | assert((A.range_error == 1) | (pressure_chamber > {-1,'atm'}), 'Pressure fell below absolute zero'); 80 | assert((A.range_error == 2) | (pressure_chamber > {-1,'atm'}), 'Pressure fell below absolute zero',Warn=true); 81 | 82 | let 83 | % The fluid mixture density at node P is calculated to know exactly 84 | % how much fluid is actually displaced. 85 | p_abs = pressure_chamber + p_atm; 86 | p_0abs = initial_pressure + p_atm; 87 | 88 | % Full fluid mixture density model 89 | rho_l0 = A.density; 90 | beta_l = A.bulk; 91 | rho_over_rho0 = (A.alpha/(1-A.alpha) * rho_g0/rho_l0 + 1)/( A.alpha/(1-A.alpha)*(p_atm/p_abs)^(1/k_sh) + exp(-pressure_chamber/beta_l) ); 92 | drho_over_rho0dp = (A.alpha/(1-A.alpha) * rho_g0/rho_l0 + 1) * (A.alpha/(1-A.alpha)/p_atm/k_sh*(p_atm/p_abs)^(1/k_sh+1) + exp(-pressure_chamber/beta_l)/beta_l)/( A.alpha/(1-A.alpha)*(p_atm/p_abs)^(1/k_sh) + exp(-pressure_chamber/beta_l) )^2; 93 | 94 | % Chamber volume 95 | vol_ref = area*position + V_dead; 96 | in 97 | % Fluid mechanical interface 98 | if vol_ref > V_dead 99 | volume == vol_ref; 100 | else 101 | volume == V_dead; 102 | end 103 | 104 | force == pressure_chamber * area; 105 | 106 | % Flow rate exiting the chamber 107 | pressure_chamber == A.p; 108 | qV == rho_over_rho0*area * velocity; 109 | qrho ==drho_over_rho0dp * pressure_chamber.der * volume; 110 | q == qV + qrho; 111 | end 112 | end 113 | end -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+hydraulic/PS_force_hydraulic_chamber_trans_comp.svg: -------------------------------------------------------------------------------- 1 | 2 | Fig file converted by fig2lat 3 | http://dktools.sourceforge.net/fig2lat.html 4 | 5 | 21 | 22 | 23 | Layer 1 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+hydraulic/PS_force_hydraulic_chamber_trans_incomp.ssc: -------------------------------------------------------------------------------- 1 | component PS_force_hydraulic_chamber_trans_incomp 2 | % Translational Hydraulic Chamber Force Incompressible 3 | % The block models an ideal transducer that converts hydraulic 4 | % energy into mechanical energy in the form of translational motion of the 5 | % converter output member. 6 | 7 | % Copyright 2016-2017 The MathWorks, Inc. 8 | 9 | nodes 10 | A = foundation.hydraulic.hydraulic; % A:right 11 | end 12 | 13 | inputs 14 | velocity = {0, 'm/s'}; % v:left 15 | position = {0, 'm'}; % p:left 16 | end 17 | 18 | outputs 19 | force = {0, 'N'}; % f:left 20 | end 21 | 22 | parameters 23 | area = { 5e-4, 'm^2' }; % Piston area 24 | end 25 | 26 | variables 27 | q = { 0, 'm^3/s' }; % Flow rate leaving the converter 28 | end 29 | 30 | function setup %#simple 31 | % Parameter range checking 32 | if area <= 0 33 | pm_error('simscape:GreaterThanZero','Piston area'); 34 | end 35 | end 36 | 37 | branches 38 | q: A.q -> *; 39 | end 40 | 41 | equations 42 | % Fluid mechanical interface 43 | force == area*A.p; 44 | 45 | % Flow rate exiting the converter 46 | q == area*velocity; 47 | end 48 | end -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+hydraulic/PS_force_hydraulic_chamber_trans_incomp.svg: -------------------------------------------------------------------------------- 1 | 2 | Fig file converted by fig2lat 3 | http://dktools.sourceforge.net/fig2lat.html 4 | 5 | 21 | 22 | 23 | Layer 1 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+hydraulic/lib.m: -------------------------------------------------------------------------------- 1 | function lib( libInfo ) 2 | % Customize library 3 | % Copyright 2005-2017 The MathWorks, Inc 4 | 5 | libInfo.Name = 'Hydraulic'; 6 | end 7 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+hydraulic/sscprj/PS_force_hydraulic_chamber_trans_comp.pmdlg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Libraries/MultiphysicsLib/Libraries/+forcesPS/+hydraulic/sscprj/PS_force_hydraulic_chamber_trans_comp.pmdlg -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+hydraulic/sscprj/PS_force_hydraulic_chamber_trans_incomp.pmdlg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Libraries/MultiphysicsLib/Libraries/+forcesPS/+hydraulic/sscprj/PS_force_hydraulic_chamber_trans_incomp.pmdlg -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+math/PS_q_axs_to_qz.ssc: -------------------------------------------------------------------------------- 1 | component PS_q_axs_to_qz 2 | % PS Calculate qz 3 | % This block calculates the rotational angle about the z axis using Angle 4 | % and Axis outputs of the Transform Sensor block 5 | 6 | % Copyright 2016-2017 The MathWorks, Inc. 7 | 8 | inputs 9 | q = {0, 'rad'}; % q:left 10 | axs = {[0 0 0]','1'} %axs:left 11 | end 12 | 13 | outputs 14 | qz = {0, 'rad'}; %qz:right 15 | end 16 | 17 | equations 18 | qz == q*sign(axs(3)); 19 | end 20 | 21 | end 22 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+math/lib.m: -------------------------------------------------------------------------------- 1 | function lib( libInfo ) 2 | % Customize library 3 | % Copyright 2005-2017 The MathWorks, Inc 4 | 5 | libInfo.Name = 'Math'; 6 | end 7 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+math/sscprj/PS_q_axs_to_qz.pmdlg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Libraries/MultiphysicsLib/Libraries/+forcesPS/+math/sscprj/PS_q_axs_to_qz.pmdlg -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/PS_force_friction_rot.ssc: -------------------------------------------------------------------------------- 1 | component PS_force_friction_rot 2 | % Rotational Friction Force 3 | % The block represents friction in the contact between moving bodies. The 4 | % friction force is simulated as a function of relative velocity and 5 | % assumed to be the sum of Stribeck, Coulomb, and viscous components. The 6 | % sum of the Coulomb and Stribeck frictions at zero velocity is often 7 | % referred to as the breakaway friction. 8 | 9 | % Copyright 2016-2017 The MathWorks, Inc. 10 | 11 | inputs 12 | w = {0, 'rad/s'}; % w:left 13 | end 14 | 15 | outputs 16 | t = {0, 'N*m'}; %t:right 17 | end 18 | 19 | parameters 20 | brkwy_trq = { 25, 'N*m' }; % Breakaway friction torque 21 | brkwy_vel = { 0.1, 'rad/s' }; % Breakaway friction velocity 22 | Col_trq = { 20, 'N*m' }; % Coulomb friction torque 23 | visc_coef = { 0.001, 'N*m*s/rad' }; % Viscous friction coefficient 24 | end 25 | 26 | parameters (Access=private) 27 | static_scale = sqrt(2*exp(1))*(brkwy_trq-Col_trq); % Scale factor for static torque 28 | static_thr = sqrt(2)*brkwy_vel; % Velocity threshold for static torque 29 | Col_thr = brkwy_vel/10; % Velocity threshold for Coulomb torque 30 | end 31 | 32 | equations 33 | % Parameter range checking 34 | assert(brkwy_trq>0) 35 | assert(brkwy_vel>0) 36 | assert(Col_trq>0) 37 | assert(Col_trq<=brkwy_trq) 38 | assert(visc_coef>=0) 39 | % Torque is a combination of viscous, static, and Coulomb losses 40 | t == visc_coef * w ... 41 | + static_scale * (w/static_thr*exp(-(w/static_thr)^2)) ... 42 | + Col_trq * tanh(w/Col_thr); 43 | end 44 | 45 | end -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/PS_force_friction_rot.svg: -------------------------------------------------------------------------------- 1 | 2 | 5 | 9 | Fig file converted by fig2lat 10 | http://dktools.sourceforge.net/fig2lat.html 11 | 12 | 33 | 34 | 40 | 45 | 49 | 53 | 57 | 58 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/PS_force_friction_trans.ssc: -------------------------------------------------------------------------------- 1 | component PS_force_friction_trans 2 | % Translational Friction Force 3 | % The block represents friction in the contact between moving bodies. The 4 | % friction force is simulated as a function of relative velocity and 5 | % assumed to be the sum of Stribeck, Coulomb, and viscous components. The 6 | % sum of the Coulomb and Stribeck frictions at zero velocity is often 7 | % referred to as the breakaway friction. 8 | 9 | % Copyright 2016-2017 The MathWorks, Inc. 10 | 11 | inputs 12 | v = {0, 'm/s'}; % v:left 13 | end 14 | 15 | outputs 16 | f = {0, 'N'}; %f:right 17 | end 18 | 19 | parameters 20 | brkwy_frc = { 25, 'N' }; % Breakaway friction force 21 | brkwy_vel = { 0.1, 'm/s' }; % Breakaway friction velocity 22 | Col_frc = { 20, 'N' }; % Coulomb friction force 23 | visc_coef = { 100, 'N*s/m' }; % Viscous friction coefficient 24 | end 25 | 26 | parameters (Access=private) 27 | static_scale = sqrt(2*exp(1))*(brkwy_frc-Col_frc); % Scale factor for static torque 28 | static_thr = sqrt(2)*brkwy_vel; % Velocity threshold for static torque 29 | Col_thr = brkwy_vel/10; % Velocity threshold for Coulomb torque 30 | end 31 | 32 | equations 33 | % Parameter range checking 34 | assert(brkwy_frc>0) 35 | assert(brkwy_vel>0) 36 | assert(Col_frc>0) 37 | assert(Col_frc<=brkwy_frc) 38 | assert(visc_coef>=0) 39 | % Force is a combination of viscous, static, and Coulomb losses 40 | f == visc_coef * v ... 41 | + static_scale * (v/static_thr*exp(-(v/static_thr)^2)) ... 42 | + Col_frc * tanh(v/Col_thr); 43 | end 44 | 45 | end -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/PS_force_friction_trans.svg: -------------------------------------------------------------------------------- 1 | 2 | 5 | 9 | Fig file converted by fig2lat 10 | http://dktools.sourceforge.net/fig2lat.html 11 | 12 | 33 | 34 | 41 | 48 | 52 | 56 | 60 | 61 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/PS_force_hardstop_rot.ssc: -------------------------------------------------------------------------------- 1 | component PS_force_hardstop_rot 2 | % Rotational Hard Stop Force 3 | % This block represents a double-sided mechanical translational hard stop 4 | % that restricts motion of a body between upper and lower bounds. The stop 5 | % is implemented as a spring and damper that comes into contact with the 6 | % slider as the gap closes. 7 | 8 | % Copyright 2016-2017 The MathWorks, Inc. 9 | 10 | inputs 11 | w = {0, 'rad/s' }; % w:left 12 | phi = {0, 'rad'}; % q:left 13 | end 14 | 15 | outputs 16 | t = {0, 'N*m'}; %t:right 17 | end 18 | 19 | parameters 20 | upper_bnd = { 0.1, 'rad' }; % Upper bound 21 | lower_bnd = { -0.1, 'rad' }; % Lower bound 22 | stiff_up = { 1e6, 'N*m/rad' }; % Stiffness (upper bound) 23 | stiff_low = { 1e6, 'N*m/rad' }; % Stiffness (lower bound) 24 | D_up = { 0.01, 'N*m*s/rad'}; % Damping (upper bound) 25 | D_low = { 0.01, 'N*m*s/rad'}; % Damping (lower bound) 26 | end 27 | 28 | equations 29 | assert(lower_bnd=0) 31 | assert(stiff_low>=0) 32 | assert(D_up>=0) 33 | assert(D_low>=0) 34 | if (phi > upper_bnd) 35 | % Slider hits upper bound 36 | t == stiff_up * (phi - upper_bnd) + D_up * w; 37 | elseif (phi < lower_bnd) 38 | % Slider hits lower bound 39 | t == stiff_low * (phi - lower_bnd) + D_low * w; 40 | else 41 | % Slider is between hard stops 42 | t == {0 'N*m'}; 43 | end 44 | end 45 | 46 | end 47 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/PS_force_hardstop_rot.svg: -------------------------------------------------------------------------------- 1 | 2 | 5 | 9 | Fig file converted by fig2lat 10 | http://dktools.sourceforge.net/fig2lat.html 11 | 12 | 26 | 27 | 33 | 40 | 47 | 51 | 52 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/PS_force_hardstop_trans.ssc: -------------------------------------------------------------------------------- 1 | component PS_force_hardstop_trans 2 | % Translational Hard Stop Force 3 | % This block represents a double-sided mechanical translational hard stop 4 | % that restricts motion of a body between upper and lower bounds. The stop 5 | % is implemented as a spring and damper that comes into contact with the 6 | % slider as the gap closes. 7 | 8 | % Copyright 2016-2017 The MathWorks, Inc. 9 | 10 | parameters 11 | upper_bnd = {0.1 , 'm' }; % Upper bound 12 | lower_bnd = {0 , 'm' }; % Lower bound 13 | stiff_up = {1e6 , 'N/m' }; % Stiffness (upper bound) 14 | stiff_low = {1e6 , 'N/m' }; % Stiffness (lower bound) 15 | D_up = {150 , 'N*s/m'}; % Damping (upper bound) 16 | D_low = {150 , 'N*s/m'}; % Damping (lower bound) 17 | end 18 | 19 | inputs 20 | v = {0, 'm/s'}; % v:left 21 | p = {0, 'm' }; % p:left 22 | end 23 | 24 | outputs 25 | f = {0, 'N'}; %f:right 26 | end 27 | 28 | function setup %#simple 29 | if lower_bnd >= upper_bnd 30 | pm_error('simscape:LessThan','Lower bound','Upper bound') 31 | end 32 | if stiff_up <= 0 33 | pm_error('simscape:GreaterThanZero','Contact stiffness at upper bound') 34 | end 35 | if stiff_low <= 0 36 | pm_error('simscape:GreaterThanZero','Contact stiffness at lower bound') 37 | end 38 | if D_up < 0 39 | pm_error('simscape:GreaterThanOrEqualToZero','Contact damping at upper bound') 40 | end 41 | if D_low < 0 42 | pm_error('simscape:GreaterThanOrEqualToZero','Contact damping at lower bound') 43 | end 44 | end 45 | 46 | equations 47 | assert(lower_bnd0) 49 | assert(stiff_low>0) 50 | assert(D_up>=0) 51 | assert(D_low>=0) 52 | if (p > upper_bnd) 53 | % Slider hits upper bound 54 | f == stiff_up * (p - upper_bnd) + D_up * v; 55 | elseif (p < lower_bnd) 56 | % Slider hits lower bound 57 | f == stiff_low * (p - lower_bnd) + D_low * v; 58 | else 59 | % Slider is between hard stops 60 | f == {0 'N'}; 61 | end 62 | end 63 | 64 | end 65 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/PS_force_hardstop_trans.svg: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 11 | 12 | 13 | 17 | 18 | 24 | 25 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/lib.m: -------------------------------------------------------------------------------- 1 | function lib( libInfo ) 2 | % Customize library 3 | % Copyright 2005-2017 The MathWorks, Inc 4 | 5 | libInfo.Name = 'Mechanical'; 6 | end 7 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/sscprj/PS_force_friction_rot.pmdlg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/sscprj/PS_force_friction_rot.pmdlg -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/sscprj/PS_force_friction_trans.pmdlg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/sscprj/PS_force_friction_trans.pmdlg -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/sscprj/PS_force_hardstop_rot.pmdlg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/sscprj/PS_force_hardstop_rot.pmdlg -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/sscprj/PS_force_hardstop_trans.pmdlg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Libraries/MultiphysicsLib/Libraries/+forcesPS/+mechanical/sscprj/PS_force_hardstop_trans.pmdlg -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/+forcesPS/lib.m: -------------------------------------------------------------------------------- 1 | function lib( libInfo ) 2 | % Customize library 3 | % Copyright 2005-2017 The MathWorks, Inc 4 | 5 | libInfo.Name = 'Simscape Forces Library'; 6 | CurrentDate = date; 7 | CurrentYear = CurrentDate(end-3:end); 8 | libInfo.Annotation = sprintf('%s\n%s','Simscape Forces Library',['Copyright 2016-' CurrentYear ' The MathWorks, Inc']); 9 | end 10 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/Multibody_Multiphysics_Lib.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Libraries/MultiphysicsLib/Libraries/Multibody_Multiphysics_Lib.slx -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/forcesPS_lib.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Libraries/MultiphysicsLib/Libraries/forcesPS_lib.slx -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/Libraries/slblocks.m: -------------------------------------------------------------------------------- 1 | function blkStruct = slblocks 2 | %SLBLOCKS Defines the block library for a specific Toolbox or Blockset. 3 | % SLBLOCKS returns information about a Blockset to Simulink. The 4 | % information returned is in the form of a BlocksetStruct with the 5 | % following fields: 6 | % 7 | % Name Name of the Blockset in the Simulink block library 8 | % Blocksets & Toolboxes subsystem. 9 | % OpenFcn MATLAB expression (function) to call when you 10 | % double-click on the block in the Blocksets & Toolboxes 11 | % subsystem. 12 | % MaskDisplay Optional field that specifies the Mask Display commands 13 | % to use for the block in the Blocksets & Toolboxes 14 | % subsystem. 15 | % Browser Array of Simulink Library Browser structures, described 16 | % below. 17 | % 18 | % The Simulink Library Browser needs to know which libraries in your 19 | % Blockset it should show, and what names to give them. To provide 20 | % this information, define an array of Browser data structures with one 21 | % array element for each library to display in the Simulink Library 22 | % Browser. Each array element has two fields: 23 | % 24 | % Library File name of the library (model file) to include in the 25 | % Library Browser. 26 | % Name Name displayed for the library in the Library Browser 27 | % window. Note that the Name is not required to be the 28 | % same as the model file name. 29 | % 30 | % Example: 31 | % 32 | % % 33 | % % Define the BlocksetStruct for the Simulink block libraries 34 | % % Only simulink_extras shows up in Blocksets & Toolboxes 35 | % % 36 | % blkStruct.Name = ['Simulink' sprintf('\n') 'Extras']; 37 | % blkStruct.OpenFcn = 'simulink_extras'; 38 | % blkStruct.MaskDisplay = sprintf('Simulink\nExtras'); 39 | % 40 | % % 41 | % % Both simulink and simulink_extras show up in the Library Browser. 42 | % % 43 | % blkStruct.Browser(1).Library = 'simulink'; 44 | % blkStruct.Browser(1).Name = 'Simulink'; 45 | % blkStruct.Browser(2).Library = 'simulink_extras'; 46 | % blkStruct.Browser(2).Name = 'Simulink Extras'; 47 | % 48 | 49 | % Copyright 1990-2017 The MathWorks, Inc. 50 | 51 | % 52 | % Name of the subsystem which will show up in the Simulink Blocksets 53 | % and Toolboxes subsystem. 54 | % 55 | blkStruct.Name = ['Simscape Multibody Multiphysics Library']; 56 | % 57 | % The function that will be called when the user double-clicks on 58 | % this icon. 59 | % 60 | blkStruct.OpenFcn = 'Multibody_Multiphysics_Lib'; 61 | 62 | % 63 | % The argument to be set as the Mask Display for the subsystem. You 64 | % may comment this line out if no specific mask is desired. 65 | % Example: blkStruct.MaskDisplay = 'plot([0:2*pi],sin([0:2*pi]));'; 66 | % No display for Simulink Extras. 67 | % 68 | blkStruct.MaskDisplay = 'Simscape Multibody Multiphysics Library'; 69 | 70 | % 71 | % Define the Browser structure array, the first element contains the 72 | % information for the Simulink block library and the second for the 73 | % Simulink Extras block library. 74 | % 75 | 76 | %blkStruct.Browser(1).Library = 'simulink'; 77 | %blkStruct.Browser(1).Name = 'Simulink'; 78 | %blkStruct.Browser(2).Library = 'Contact_Forces_Lib'; 79 | %blkStruct.Browser(2).Name = 'Simscape Multibody Contact Forces Library'; 80 | 81 | %blkStruct.Browser(1).Library = 'Contact_Forces_Lib'; 82 | %blkStruct.Browser(1).Name = 'Simscape Multibody Contact Forces Library'; 83 | 84 | Browser(1).Library = 'Multibody_Multiphysics_Lib'; 85 | Browser(1).Name = 'Simscape Multibody Multiphysics Library'; 86 | 87 | %Browser(1).IsTopLevel = 0; 88 | %Browser(1).IsFlat = 1;% Is this library "flat" (i.e. no subsystems)? 89 | 90 | %Browser(2).Library = 'TNO_dtlib_2'; 91 | %Browser(2).Name = 'Simulink'; 92 | %Browser(2).IsFlat = 0;% Is this library "flat" (i.e. no subsystems)? 93 | 94 | blkStruct.Browser = Browser; 95 | clear Browser; 96 | 97 | 98 | % Define information for model updater 99 | blkStruct.ModelUpdaterMethods.fhDetermineBrokenLinks = @UpdateSimulinkBrokenLinksMappingHelper; 100 | blkStruct.ModelUpdaterMethods.fhSeparatedChecks = @UpdateSimulinkBlocksHelper; 101 | 102 | % End of slblocks 103 | 104 | 105 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/README.txt: -------------------------------------------------------------------------------- 1 | Simscape Multibody Multiphysics Library. 2 | 3 | Run startup_sm_ssci.m to get started 4 | 5 | This file contains example models showing how to extend Simscape Multibody models 6 | by adding physical effects spanning multiple physical domains modeled in Simscape. 7 | 8 | Connecting the models using Simscape Physical Signals ensures a lossless transfer 9 | of power between physical networks. This submission contains a library that contains 10 | general interface blocks (rotational, translational), and example models showing 11 | how to use them to model multidomain physical systems. 12 | 13 | You need to ensure that your use of these interfaces is physically valid. Connecting 14 | a 3D mechanical model to a 1D physical systems requires that you follow a few basic 15 | rules: 16 | 17 | 1. Never add inertia directly to the node on the Simscape side of the interface. 18 | 19 | All masses in Simscape models live in an implicit inertial reference frame. A Simscape mechanical 20 | circuit interfaced to a Simscape Multibody machine in general moves in an accelerated frame. A simulation 21 | with such a circuit does not include the pseudoforces acting on the Simscape mass and inertia elements 22 | as experienced in such a noninertial frame and thus violates Newton's second law of mechanics. 23 | 24 | 2. If you must model inertia in the Simscape network, connect it to the interface element 25 | via a spring and damper connected in parallel. Be aware that a Simscape circuit does not model 26 | the motion of such bodies along or about axes orthogonal to the coupled primitive axis chosen 27 | in the interfaced Joint. 28 | 29 | 3. Quantities sensed in Simscape (like translation at a node) may be offset from comparable quantities 30 | measured in Simscape Multibody. This is because the initial position of the Simscape Multibody joint, 31 | which is determined during the assembly process, is not automatically conveyed to the Simscape network. 32 | You must either use MATLAB variables to synchronize the setting of the initial position or feed 33 | the position from Simscape Multibody to the Simscape network. The examples in this submission 34 | show how to do that. 35 | 36 | Copyright 2013-2017 The MathWorks, Inc. 37 | 38 | ######### Release History ######### 39 | v 2.3 (R2017a) July 2017 Fixed mistake in library (Interfaces/Translational Simscape Multibody). 40 | Changed checkbox from torque to force. 41 | Added example sm_ssci_01_slider_crank.slx 42 | 43 | v 2.2 (R2017a) May 2017 Initial release (version number set to match File Exchange) 44 | Includes general 3D-1D interface blocks as well as abstract multiphysics 45 | blocks connecting hydraulic, electrical, and mechanical effects to 46 | multibody systems. 5 basic examples and one CAD workflow example. 47 | 48 | -------------------------------------------------------------------------------- /Libraries/MultiphysicsLib/startup_sm_ssci.m: -------------------------------------------------------------------------------- 1 | % Copyright 2016-2017 The MathWorks, Inc. 2 | 3 | SCI_HomeDir = pwd; 4 | addpath(pwd) 5 | addpath(genpath(pwd)) 6 | 7 | cd Libraries 8 | if((exist('+forcesPS')==7) && ~exist('forcesPS_Lib')) 9 | ssc_build forcesPS 10 | end 11 | cd .. 12 | 13 | if exist('Multibody_Multiphysics_Demo_Script.html') 14 | web('Multibody_Multiphysics_Demo_Script.html'); 15 | end -------------------------------------------------------------------------------- /Libraries/walkingRobotUtils.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Libraries/walkingRobotUtils.slx -------------------------------------------------------------------------------- /ModelingSimulation/Intermediate/initConditionPicker.mlapp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ModelingSimulation/Intermediate/initConditionPicker.mlapp -------------------------------------------------------------------------------- /ModelingSimulation/Intermediate/jointAngs.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ModelingSimulation/Intermediate/jointAngs.mat -------------------------------------------------------------------------------- /ModelingSimulation/Intermediate/walkingRobot_step1_mechanics.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ModelingSimulation/Intermediate/walkingRobot_step1_mechanics.slx -------------------------------------------------------------------------------- /ModelingSimulation/Intermediate/walkingRobot_step2_motion.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ModelingSimulation/Intermediate/walkingRobot_step2_motion.slx -------------------------------------------------------------------------------- /ModelingSimulation/Intermediate/walkingRobot_step3_contact.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ModelingSimulation/Intermediate/walkingRobot_step3_contact.slx -------------------------------------------------------------------------------- /ModelingSimulation/Intermediate/walkingRobot_step4_library.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ModelingSimulation/Intermediate/walkingRobot_step4_library.slx -------------------------------------------------------------------------------- /ModelingSimulation/README.md: -------------------------------------------------------------------------------- 1 | # Walking Robots -- Modeling and Simulation 2 | ### Copyright 2017-2019 The MathWorks, Inc. 3 | 4 | Contains files for modeling and simulation of the walking robot using Simscape Multibody. 5 | 6 | Requires MATLAB R2019b or later. 7 | 8 | ## MAIN FOLDER 9 | * `walkingRobot.slx` -- Main model 10 | * `robotParameters.m` -- Parameter file automatically loaded with walking robot simulation 11 | * `compareActuatorTypes.m` -- Compares simulation results for a trajectory across different actuator types 12 | 13 | ## INTERMEDIATE FOLDER 14 | Contains models with intermediate steps for setting up the walking robot simulation. -------------------------------------------------------------------------------- /ModelingSimulation/compareActuatorTypes.m: -------------------------------------------------------------------------------- 1 | % Simulates the walking robot model with the 3 actuator variants and 2 | % compares results 3 | % Copyright 2017-2019 The MathWorks, Inc. 4 | 5 | %% Setup 6 | clc; close all; 7 | mdlName = 'walkingRobot'; 8 | bdclose(mdlName) 9 | open_system(mdlName) 10 | 11 | %% Simulate 12 | actuatorType = 1; 13 | tic; simout_motion = sim(mdlName,'StopTime','10'); 14 | disp(['Compiled and ran Motion actuated simulation in ' num2str(toc) ' seconds']); 15 | actuatorType = 2; 16 | tic; simout_torque = sim(mdlName,'StopTime','10'); 17 | disp(['Compiled and ran Torque actuated simulation in ' num2str(toc) ' seconds']); 18 | actuatorType = 3; 19 | tic; simout_motor = sim(mdlName,'StopTime','10'); 20 | disp(['Compiled and ran Motor actuated simulation in ' num2str(toc) ' seconds']); 21 | 22 | %% Torso Position Plots 23 | figure(1) 24 | subplot(3,1,1) 25 | hold on 26 | plot(get(simout_motion.simout,'measBody').Values.X.Time,get(simout_motion.simout,'measBody').Values.X.Data,'b-'); 27 | plot(get(simout_torque.simout,'measBody').Values.X.Time,get(simout_torque.simout,'measBody').Values.X.Data,'r-'); 28 | plot(get(simout_motor.simout,'measBody').Values.X.Time, get(simout_motor.simout,'measBody').Values.X.Data, 'k-'); 29 | title('Robot Motion') 30 | legend('Motion','Torque','Motor'); 31 | title('Simulation Output Comparisons'); 32 | xlabel('Time [s]'); 33 | ylabel('Torso X Position [m]'); 34 | subplot(3,1,2) 35 | hold on 36 | plot(get(simout_motion.simout,'measBody').Values.Y.Time,get(simout_motion.simout,'measBody').Values.Y.Data,'b-'); 37 | plot(get(simout_torque.simout,'measBody').Values.Y.Time,get(simout_torque.simout,'measBody').Values.Y.Data,'r-'); 38 | plot(get(simout_motor.simout,'measBody').Values.Y.Time, get(simout_motor.simout,'measBody').Values.Y.Data, 'k-'); 39 | title('Robot Motion') 40 | legend('Motion','Torque','Motor'); 41 | xlabel('Time [s]'); 42 | ylabel('Torso Y Position [m]'); 43 | subplot(3,1,3) 44 | hold on 45 | plot(get(simout_motion.simout,'measBody').Values.Z.Time,get(simout_motion.simout,'measBody').Values.Z.Data,'b-'); 46 | plot(get(simout_torque.simout,'measBody').Values.Z.Time,get(simout_torque.simout,'measBody').Values.Z.Data,'r-'); 47 | plot(get(simout_motor.simout,'measBody').Values.Z.Time, get(simout_motor.simout,'measBody').Values.Z.Data, 'k-'); 48 | title('Robot Motion') 49 | legend('Motion','Torque','Motor'); 50 | xlabel('Time [s]'); 51 | ylabel('Torso Z Position [m]'); 52 | 53 | %% Joint Plots 54 | jointNames = {'ankleroll','anklepitch','knee','hippitch','hiproll','hipyaw'}; 55 | for idx = 1:6 56 | figure(idx+1) 57 | jName = jointNames{idx}; 58 | 59 | % Joint angle 60 | subplot(2,1,1) 61 | hold on 62 | modifier = [jName '_angle']; 63 | plot(get(simout_motion.simout,'measR').Values.(modifier).Time,get(simout_motion.simout,'measR').Values.(modifier).Data,'b-'); 64 | plot(get(simout_torque.simout,'measR').Values.(modifier).Time,get(simout_torque.simout,'measR').Values.(modifier).Data,'r-'); 65 | plot(get(simout_motor.simout,'measR').Values.(modifier).Time, get(simout_motor.simout,'measR').Values.(modifier).Data, 'k-'); 66 | legend('Motion','Torque','Motor'); 67 | xlabel('Time [s]'); 68 | ylabel('Joint Angle [rad]'); 69 | title(jointNames{idx}) 70 | 71 | % Joint torque 72 | subplot(2,1,2) 73 | hold on 74 | modifier = [jName '_torque']; 75 | plot(get(simout_motion.simout,'measR').Values.(modifier).Time,get(simout_motion.simout,'measR').Values.(modifier).Data,'b-'); 76 | plot(get(simout_torque.simout,'measR').Values.(modifier).Time,get(simout_torque.simout,'measR').Values.(modifier).Data,'r-'); 77 | plot(get(simout_motor.simout,'measR').Values.(modifier).Time, get(simout_motor.simout,'measR').Values.(modifier).Data, 'k-'); 78 | legend('Motion','Torque','Motor'); 79 | xlabel('Time [s]'); 80 | ylabel('Joint Torque [N*m]'); 81 | ylim(max_torque*[-1, 1]) 82 | end 83 | 84 | 85 | %% Cleanup 86 | bdclose(mdlName) -------------------------------------------------------------------------------- /ModelingSimulation/robotParameters.m: -------------------------------------------------------------------------------- 1 | % Walking Robot Parameters 2 | % Copyright 2017-2019 The MathWorks, Inc. 3 | 4 | %% Actuator type 5 | actuatorType = 1; % 1:motion controlled, 2:torque controlled, 3:motor controlled 6 | 7 | %% Contact and friction parameters 8 | contact_stiffness = 400/0.001; % Approximated at weight (N) / desired displacement (m) 9 | contact_damping = contact_stiffness/10; % Tuned based on contact stiffness value 10 | mu_s = 0.9; % Static friction coefficient: Around that of rubber-asphalt 11 | mu_k = 0.8; % Kinetic friction coefficient: Lower than the static coefficient 12 | mu_vth = 0.1; % Friction velocity threshold (m/s) 13 | 14 | height_plane = 0.025; 15 | plane_z = height_plane; 16 | plane_x = 3; 17 | plane_y = 50; 18 | contact_point_radius = 0.0001; %m 19 | 20 | %% Robot mechanical Parameters (m) 21 | density = 1000; 22 | 23 | leg_width = 0.08; 24 | lower_leg_length = 0.38; 25 | upper_leg_length = 0.40; 26 | 27 | foot_x = 0.17; 28 | foot_y = 0.17; 29 | foot_z = 0.02; 30 | 31 | torso_width = 0.24; 32 | torso_length = 0.20; 33 | torso_height = 0.35; 34 | 35 | torso_offset_height = 0; 36 | torso_offset_length = 0; 37 | 38 | world_damping = 0; % Translational damping for 6-DOF joint [N/m] 39 | world_rot_damping = 0; % Rotational damping for 6-DOF joint [N*m/(rad/s)] 40 | 41 | %% Initial conditions 42 | % Height of the 6-DOF joint between the ground and robot torso 43 | init_height = lower_leg_length + upper_leg_length + torso_offset_height ... 44 | + foot_z/2 + plane_z/2; 45 | % Joint angles [hip_yaw, hip_roll, hip_pitch, knee, ankle_pitch, ankle_roll] 46 | init_angs_R = zeros(6,1); 47 | init_angs_L = zeros(6,1); 48 | 49 | %% Robot joint parameters 50 | joint_damping = 1; 51 | motion_time_constant = 0.001; 52 | 53 | %% Joint controller parameters 54 | hip_servo_kp = 12500; 55 | hip_servo_ki = 3500; 56 | hip_servo_kd = 100; 57 | 58 | knee_servo_kp = 10000; 59 | knee_servo_ki = 2750; 60 | knee_servo_kd = 75; 61 | 62 | ankle_servo_kp = 7500; 63 | ankle_servo_ki = 2000; 64 | ankle_servo_kd = 50; 65 | 66 | deriv_filter_coeff = 1000; 67 | max_torque = 80; 68 | 69 | %% Electric motor parameters 70 | motor_voltage = 24; 71 | motor_resistance = 8; 72 | motor_constant = 0.16; 73 | motor_inertia = 1e-5; 74 | motor_damping = 1e-5; 75 | motor_inductance = 250e-6; 76 | gear_ratio = 100; 77 | -------------------------------------------------------------------------------- /ModelingSimulation/walkingRobot.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ModelingSimulation/walkingRobot.slx -------------------------------------------------------------------------------- /Optimization/README.md: -------------------------------------------------------------------------------- 1 | # Walking Robots -- Trajectory Optimization 2 | ### Copyright 2017-2019 The MathWorks, Inc. 3 | 4 | Requires MATLAB R2019b or later. 5 | 6 | `optimizeRobotMotion.mlx` is the main optimization script, which has several options listed below 7 | 8 | a. `parallelFlag` [`true`|`false`] 9 | Use Parallel Computing Toolbox or MATLAB Distributed Computing Server to speed up optimization. 10 | This requires that you have already set up a default parallel computing cluster. 11 | 12 | b. `accelFlag` [`true`|`false`] 13 | Run the simulations in Accelerator mode. This will turn off visualization with the Mechanics Explorer, but will run faster. 14 | 15 | c. `actuatorType` [`1`|`2`|`3`] 16 | Different fidelity actuators for simulation. Suggest running optimization with `actuatorType = 1` for speed. 17 | * `1`: Ideal motion actuation 18 | * `2`: Torque actuation with PID control 19 | * `3`: Electric motor actuation with average-model PWM, H-Bridge, and PID control 20 | 21 | Other files 22 | * `walkingRobotOptim.slx` -- Simulink model configured for optimization 23 | * `simulateWalkingRobot.m` -- Runs a single iteration of the optimization by running the above model with certain parameters, collecting results, and calculating a fitness function 24 | * `doSpeedupTasks.m` -- Configures the optimization problem to use simulation speedup capabilities like Fast Restart, Accelerator mode, and parallel simulation 25 | * `SavedResults` folder -- Contains presaved optimization results 26 | * `createSmoothTrajectory.m` -- Function that generates a cubic polynomial interpolating trajectory from gait waypoints and times 27 | 28 | **NOTE:** Once your optimization terminates or you load presaved results, you can view the results using the `compareSimulationTypes.m` script in the `ModelingSimulation` folder. -------------------------------------------------------------------------------- /Optimization/SavedResults/optimizedData_01Nov19_1143.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Optimization/SavedResults/optimizedData_01Nov19_1143.mat -------------------------------------------------------------------------------- /Optimization/SavedResults/optimizedData_03Nov19_1140.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Optimization/SavedResults/optimizedData_03Nov19_1140.mat -------------------------------------------------------------------------------- /Optimization/SavedResults/optimizedData_31Oct19_0741.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Optimization/SavedResults/optimizedData_31Oct19_0741.mat -------------------------------------------------------------------------------- /Optimization/createSmoothTrajectory.m: -------------------------------------------------------------------------------- 1 | function [q,hip_der,knee_der,ankle_der] = createSmoothTrajectory(hip,knee,ankle,period,evalTimes) 2 | % Generates cubic spline trajectory parameters given waypoints 3 | % for 3 joints (ankle, knee, hip) and total gait period 4 | % Copyright 2017-2019 The MathWorks, Inc. 5 | 6 | % Create necessary values for calculations 7 | numPoints = numel(hip); 8 | traj_times = linspace(0,period,numPoints); 9 | 10 | % Calculate derivatives 11 | dt = period/(numPoints-1); 12 | hip_der = [0, 0.5*( diff(hip(1:end-1)) + diff(hip(2:end)) )/dt, 0]; 13 | knee_der = [0, 0.5*( diff(knee(1:end-1)) + diff(knee(2:end)) )/dt, 0]; 14 | ankle_der = [0, 0.5*( diff(ankle(1:end-1)) + diff(ankle(2:end)) )/dt, 0]; 15 | 16 | % Set initial conditions 17 | % Evaluate the trajectory at the start and halfway points for right and 18 | % left legs, respectively 19 | q = cubicpolytraj([hip;knee;ankle],traj_times,evalTimes,... 20 | 'VelocityBoundaryCondition',[hip_der;knee_der;ankle_der]); 21 | -------------------------------------------------------------------------------- /Optimization/doSpeedupTasks.m: -------------------------------------------------------------------------------- 1 | % Sets up optimization environment for acceleration and parallel computing 2 | % Copyright 2017-2019 The MathWorks, Inc. 3 | 4 | if parallelFlag 5 | 6 | % Create a parallel pool if one is not active 7 | if isempty(gcp('nocreate')) 8 | parpool; % Uses default saved profile 9 | end 10 | p = gcp; 11 | 12 | % Prevent parallel simulations from accessing Simulation Data Inspector 13 | Simulink.sdi.enablePCTSupport('none'); 14 | 15 | % Add paths and dependent files to run simulations in parallel 16 | rootDir = fullfile(fileparts(mfilename('fullpath')),'..'); 17 | addAttachedFiles(p,fullfile(rootDir,{'ModelingSimulation','Optimization','Libraries'})); 18 | parfevalOnAll(@addpath,0,fullfile(rootDir,'Optimization'), ... 19 | genpath(fullfile(rootDir,'ModelingSimulation')), ... 20 | genpath(fullfile(rootDir,'Libraries'))); 21 | parfevalOnAll(@load_system,0,mdlName); 22 | 23 | % If the acceleration flag is true, set the simulation mode to 24 | % accelerator 25 | if accelFlag 26 | parfevalOnAll(@set_param,0,mdlName,'SimulationMode','accelerator'); 27 | parfevalOnAll(@set_param,0,mdlName,'SimMechanicsOpenEditorOnUpdate','off'); 28 | end 29 | 30 | % Change each worker to unique folder so cache files do not conflict 31 | cd(fullfile(rootDir,'Optimization')); 32 | if exist('temp','dir') 33 | rmdir('temp','s'); 34 | end 35 | mkdir('temp'); 36 | spmd 37 | tempFolder = fullfile(rootDir,'Optimization','temp'); 38 | cd(tempFolder); 39 | folderName = tempname(tempFolder); 40 | mkdir(folderName); 41 | cd(folderName) 42 | end 43 | 44 | else 45 | % If the acceleration flag is true, set the simulation mode to 46 | % accelerator 47 | if accelFlag 48 | load_system(mdlName); 49 | set_param(mdlName,'SimulationMode','accelerator'); 50 | set_param(mdlName,'SimMechanicsOpenEditorOnUpdate','off'); 51 | end 52 | end -------------------------------------------------------------------------------- /Optimization/optimizeRobotMotion.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Optimization/optimizeRobotMotion.mlx -------------------------------------------------------------------------------- /Optimization/simulateWalkingRobot.m: -------------------------------------------------------------------------------- 1 | function penalty = simulateWalkingRobot(params,mdlName,scaleFactor,gait_period,actuatorType) 2 | % Cost function for robot walking optimization 3 | % Copyright 2017-2019 The MathWorks, Inc. 4 | 5 | % Load parameters into function workspace 6 | robotParameters; 7 | % Trajectory sample time 8 | tsTraj = 0.01; 9 | % Simulate air drag for stability 10 | world_damping = 20; % Translational damping 11 | world_rot_damping = 10; % Rotational damping 12 | 13 | % Apply variable scaling 14 | params = scaleFactor*params; 15 | 16 | % Extract simulation inputs from parameters 17 | N = numel(params)/3; 18 | hip_motion = deg2rad([params(1:N), params(1)]); 19 | knee_motion = deg2rad([params(N+1:2*N), params(N+1)]); 20 | ankle_motion = deg2rad([params(2*N+1:3*N), params(2*N+1)]); 21 | traj_times = linspace(0,gait_period,N+1); 22 | 23 | % Evaluate the trajectory at the start and halfway points for right and 24 | % left legs, respectively, to get initial conditions and trajectory 25 | % waypoint derivatives 26 | [q,hip_der,knee_der,ankle_der] = createSmoothTrajectory( ... 27 | hip_motion,knee_motion,ankle_motion,gait_period,[0 gait_period/2]); 28 | % Package up the initial conditions, keeping the yaw/roll joints fixed 29 | init_angs_R = [0 0 -q(1,1) -q(2,1) -q(3,1) 0]; 30 | init_angs_L = [0 0 -q(1,2) -q(2,2) -q(3,2) 0]; 31 | 32 | % Simulate the model 33 | simout = sim(mdlName,'StopTime','10','SrcWorkspace','current','FastRestart','on'); 34 | 35 | % Unpack logged data 36 | measBody = get(simout.simout,'measBody').Values; 37 | xMax = max(abs(measBody.X.Data)); 38 | yEnd = measBody.Y.Data(end); 39 | tEnd = simout.tout(end); 40 | 41 | % Calculate penalty from logged data 42 | 43 | % Longitudinal (Y) distance traveled without falling 44 | % (ending simulation early) increases reward 45 | positiveReward = sign(yEnd)*yEnd^2 * tEnd; 46 | 47 | % Lateral (Y) displacement and trajectory aggressiveness 48 | % (number of times the derivative flips signs) decreases reward 49 | % NOTE: Set lower limits to prevent divisions by zero 50 | aggressiveness = 0; 51 | diffs = [diff(hip_motion) diff(knee_motion) diff(ankle_motion)]; 52 | for idx = 1:numel(diffs)-1 53 | if (sign(diffs(idx)/diffs(idx+1))<0) && mod(idx,N) 54 | aggressiveness = aggressiveness + 1; 55 | end 56 | end 57 | negativeReward = max(xMax,0.1) * max(aggressiveness,1); 58 | 59 | % Negative sign needed since the optimization minimizes cost function 60 | penalty = -positiveReward/negativeReward; 61 | 62 | end 63 | 64 | -------------------------------------------------------------------------------- /Optimization/walkingRobotOptim.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/Optimization/walkingRobotOptim.slx -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Walking Robot Examples 2 | Copyright 2017-2019 The MathWorks, Inc. 3 | 4 | ## Introduction 5 | This repository contains example files for the following [MATLAB and Simulink Robotics Arena](https://www.mathworks.com/academia/student-competitions/roboticsarena.html) videos on walking robots. 6 | 7 | * [Basics of walking robots](https://www.mathworks.com/videos/model-based-control-of-humanoid-walking-1574399243682.html) 8 | * [Modeling and simulation](https://www.mathworks.com/videos/modeling-and-simulation-of-walking-robots-1576560207573.html) 9 | * [Trajectory optimization](https://www.mathworks.com/videos/matlab-and-simulink-robotics-arena-walking-robots-part-3-trajectory-optimization-1506440520726.html) 10 | * [Walking pattern generation](https://www.mathworks.com/videos/matlab-and-simulink-robotics-arena-walking-robots-pattern-generation-1546434170253.html) 11 | * [Deep reinforcement learning](https://www.mathworks.com/videos/deep-reinforcement-learning-for-walking-robots--1551449152203.html) 12 | 13 | You can also learn more about this example from our blog posts on 14 | [modeling and simulation](https://blogs.mathworks.com/racing-lounge/2017/10/11/walking-robot-modeling-and-simulation) 15 | and [control](https://blogs.mathworks.com/racing-lounge/2019/04/24/walking-robot-control/). 16 | 17 | For any questions, email us at roboticsarena@mathworks.com. 18 | 19 | --- 20 | 21 | ## Setup 22 | Run `startupWalkingRobot.m` to get the MATLAB path ready. 23 | 24 | Below are the main folders containing various walking robot examples: 25 | 26 | 1. `LIPM` -- Shows how to generate a walking pattern using the 27 | linear inverted pendulum model (LIPM), which is one of the foundational 28 | models for humanoid walking control. 29 | 30 | 2. `ModelingSimulation` -- Shows how to build the simulation of the walking 31 | robot, including contact forces, various actuator models, and importing from CAD. 32 | 33 | 3. `Optimization` -- Shows how to use genetic algorithms to optimize joint angle 34 | trajectories for stability and speed. 35 | 36 | 4. `ControlDesign` -- Shows how to create closed-loop walking controllers 37 | using common techniques like Zero Moment Point (ZMP) manipulation and 38 | Model Predictive Control (MPC) for pattern generation. 39 | 40 | 5. `ReinforcementLearning` -- Shows how to set up and train a Deep Deterministic 41 | Policy Gradient (DDPG) reinforcement learning agent for learning how to walk. 42 | 43 | Each of these folders has its own separate README with more information. 44 | 45 | --- 46 | 47 | ## Multiphysics Library 48 | For convenience, a local copy of the Simscape Multibody Multiphysics Library 49 | has been included with this submission. If you would like to install the 50 | latest version of these libraries, you can find them from the Add-On Explorer, 51 | or on the File Exchange at https://www.mathworks.com/matlabcentral/fileexchange/37636-simscape-multibody-multiphysics-library 52 | -------------------------------------------------------------------------------- /ReinforcementLearning/README.md: -------------------------------------------------------------------------------- 1 | # Walking Robots -- Reinforcement Learning 2 | ### Copyright 2019 The MathWorks, Inc. 3 | 4 | Contains files for reinforcement learning of the walking robot simulation. 5 | 6 | Requires MATLAB R2019b or later. 7 | 8 | ## 2D (6-DOF) MODEL FILES 9 | This robot has 3 degrees of freedom per leg, and can only move legs along the longitudinal direction. 10 | 11 | * `walkingRobotRL2D.slx` -- Reinforcement learning capable model 12 | * `createWalkingAgent2D.m` -- Reinforcement agent creation and training script 13 | 14 | ## 3D (10-DOF) MODEL FILES 15 | This robot has 5 degrees of freedom per leg, which allows it to take lateral steps as well. 16 | 17 | * `walkingRobotRL3D.slx` -- Reinforcement learning capable model 18 | * `createWalkingAgent3D.m` -- Reinforcement agent creation and training script 19 | 20 | ## COMMON FILES 21 | * `createDDPGNetworks.m` -- Creates actor and critic neural networks and reinforcement learning representations 22 | * `createDDPGOptions.m` -- Creates training options for reinforcement learning 23 | * `robotParametersRL.m` -- Parameter file automatically loaded on model startup 24 | * `walkerResetFcn.m` -- Utility function to set initial conditions randomly for exploration 25 | * `walkerInvKin.m` -- Inverse kinematics utility function to help set initial conditions 26 | * `plotTrainingResults.m` -- Plots the reinforcement learning training results from a given saved MAT-file 27 | * `savedAgents` folder -- Contains presaved agents and training results 28 | 29 | NOTE: If you are running one of the models without first training an agent, 30 | you should load one of the presaved agents from the `savedAgents` folder. -------------------------------------------------------------------------------- /ReinforcementLearning/createDDPGNetworks.m: -------------------------------------------------------------------------------- 1 | % Walking Robot -- DDPG Neural Network Setup Script (2D Walker) 2 | % Copyright 2019 The MathWorks, Inc. 3 | 4 | % Network structure inspired by original 2015 DDPG paper 5 | % "Continuous Control with Deep Reinforcement Learning", Lillicrap et al. 6 | % https://arxiv.org/pdf/1509.02971.pdf 7 | 8 | %% CRITIC 9 | % Create the critic network layers 10 | criticLayerSizes = [400 300]; 11 | statePath = [ 12 | imageInputLayer([numObs 1 1],'Normalization','none','Name', 'observation') 13 | fullyConnectedLayer(criticLayerSizes(1), 'Name', 'CriticStateFC1', ... 14 | 'Weights',2/sqrt(numObs)*(rand(criticLayerSizes(1),numObs)-0.5), ... 15 | 'Bias',2/sqrt(numObs)*(rand(criticLayerSizes(1),1)-0.5)) 16 | reluLayer('Name','CriticStateRelu1') 17 | fullyConnectedLayer(criticLayerSizes(2), 'Name', 'CriticStateFC2', ... 18 | 'Weights',2/sqrt(criticLayerSizes(1))*(rand(criticLayerSizes(2),criticLayerSizes(1))-0.5), ... 19 | 'Bias',2/sqrt(criticLayerSizes(1))*(rand(criticLayerSizes(2),1)-0.5)) 20 | ]; 21 | actionPath = [ 22 | imageInputLayer([numAct 1 1],'Normalization','none', 'Name', 'action') 23 | fullyConnectedLayer(criticLayerSizes(2), 'Name', 'CriticActionFC1', ... 24 | 'Weights',2/sqrt(numAct)*(rand(criticLayerSizes(2),numAct)-0.5), ... 25 | 'Bias',2/sqrt(numAct)*(rand(criticLayerSizes(2),1)-0.5)) 26 | ]; 27 | commonPath = [ 28 | additionLayer(2,'Name','add') 29 | reluLayer('Name','CriticCommonRelu1') 30 | fullyConnectedLayer(1, 'Name', 'CriticOutput',... 31 | 'Weights',2*5e-3*(rand(1,criticLayerSizes(2))-0.5), ... 32 | 'Bias',2*5e-3*(rand(1,1)-0.5)) 33 | ]; 34 | 35 | % Connect the layer graph 36 | criticNetwork = layerGraph(statePath); 37 | criticNetwork = addLayers(criticNetwork, actionPath); 38 | criticNetwork = addLayers(criticNetwork, commonPath); 39 | criticNetwork = connectLayers(criticNetwork,'CriticStateFC2','add/in1'); 40 | criticNetwork = connectLayers(criticNetwork,'CriticActionFC1','add/in2'); 41 | 42 | % Create critic representation 43 | criticOptions = rlRepresentationOptions('Optimizer','adam','LearnRate',1e-3, ... 44 | 'GradientThreshold',1,'L2RegularizationFactor',2e-4); 45 | if useGPU 46 | criticOptions.UseDevice = 'gpu'; 47 | end 48 | critic = rlRepresentation(criticNetwork,criticOptions, ... 49 | 'Observation',{'observation'},env.getObservationInfo, ... 50 | 'Action',{'action'},env.getActionInfo); 51 | 52 | %% ACTOR 53 | % Create the actor network layers 54 | actorLayerSizes = [400 300]; 55 | actorNetwork = [ 56 | imageInputLayer([numObs 1 1],'Normalization','none','Name','observation') 57 | fullyConnectedLayer(actorLayerSizes(1), 'Name', 'ActorFC1', ... 58 | 'Weights',2/sqrt(numObs)*(rand(actorLayerSizes(1),numObs)-0.5), ... 59 | 'Bias',2/sqrt(numObs)*(rand(actorLayerSizes(1),1)-0.5)) 60 | reluLayer('Name', 'ActorRelu1') 61 | fullyConnectedLayer(actorLayerSizes(2), 'Name', 'ActorFC2', ... 62 | 'Weights',2/sqrt(actorLayerSizes(1))*(rand(actorLayerSizes(2),actorLayerSizes(1))-0.5), ... 63 | 'Bias',2/sqrt(actorLayerSizes(1))*(rand(actorLayerSizes(2),1)-0.5)) 64 | reluLayer('Name', 'ActorRelu2') 65 | fullyConnectedLayer(numAct, 'Name', 'ActorFC3', ... 66 | 'Weights',2*5e-3*(rand(numAct,actorLayerSizes(2))-0.5), ... 67 | 'Bias',2*5e-3*(rand(numAct,1)-0.5)) 68 | tanhLayer('Name','ActorTanh1') 69 | ]; 70 | 71 | % Create actor representation 72 | actorOptions = rlRepresentationOptions('Optimizer','adam','LearnRate',1e-4, ... 73 | 'GradientThreshold',1,'L2RegularizationFactor',1e-5); 74 | if useGPU 75 | actorOptions.UseDevice = 'gpu'; 76 | end 77 | actor = rlRepresentation(actorNetwork,actorOptions, ... 78 | 'Observation',{'observation'},env.getObservationInfo, ... 79 | 'Action',{'ActorTanh1'},env.getActionInfo); 80 | 81 | %% VISUALIZING NETWORKS 82 | % Once you have created the networks, you can visualize and modify them 83 | % interactively with the Deep Network Designer app. 84 | % 85 | % >> deepNetworkDesigner -------------------------------------------------------------------------------- /ReinforcementLearning/createDDPGOptions.m: -------------------------------------------------------------------------------- 1 | % Create DDPG agent and training options for walking robot example 2 | % 3 | % Copyright 2019 The MathWorks, Inc. 4 | 5 | %% DDPG Agent Options 6 | agentOptions = rlDDPGAgentOptions; 7 | agentOptions.SampleTime = Ts; 8 | agentOptions.DiscountFactor = 0.99; 9 | agentOptions.MiniBatchSize = 128; 10 | agentOptions.ExperienceBufferLength = 5e5; 11 | agentOptions.TargetSmoothFactor = 1e-3; 12 | agentOptions.NoiseOptions.MeanAttractionConstant = 5; 13 | agentOptions.NoiseOptions.Variance = 0.5; 14 | agentOptions.NoiseOptions.VarianceDecayRate = 1e-5; 15 | 16 | %% Training Options 17 | trainingOptions = rlTrainingOptions; 18 | trainingOptions.MaxEpisodes = 5000; 19 | trainingOptions.MaxStepsPerEpisode = Tf/Ts; 20 | trainingOptions.ScoreAveragingWindowLength = 100; 21 | trainingOptions.StopTrainingCriteria = 'AverageReward'; 22 | trainingOptions.StopTrainingValue = 110; 23 | trainingOptions.SaveAgentCriteria = 'EpisodeReward'; 24 | trainingOptions.SaveAgentValue = 150; 25 | trainingOptions.Plots = 'training-progress'; 26 | trainingOptions.Verbose = true; 27 | if useParallel 28 | trainingOptions.Parallelization = 'async'; 29 | trainingOptions.ParallelizationOptions.StepsUntilDataIsSent = 32; 30 | end -------------------------------------------------------------------------------- /ReinforcementLearning/createWalkingAgent2D.m: -------------------------------------------------------------------------------- 1 | % Walking Robot -- DDPG Agent Training Script (2D) 2 | % Copyright 2019 The MathWorks, Inc. 3 | 4 | %% SET UP ENVIRONMENT 5 | % Speedup options 6 | useFastRestart = true; 7 | useGPU = true; 8 | useParallel = true; 9 | 10 | % Create the observation info 11 | numObs = 31; 12 | observationInfo = rlNumericSpec([numObs 1]); 13 | observationInfo.Name = 'observations'; 14 | 15 | % create the action info 16 | numAct = 6; 17 | actionInfo = rlNumericSpec([numAct 1],'LowerLimit',-1,'UpperLimit', 1); 18 | actionInfo.Name = 'foot_torque'; 19 | 20 | % Environment 21 | mdl = 'walkingRobotRL2D'; 22 | load_system(mdl); 23 | blk = [mdl,'/RL Agent']; 24 | env = rlSimulinkEnv(mdl,blk,observationInfo,actionInfo); 25 | env.ResetFcn = @(in)walkerResetFcn(in,upper_leg_length/100,lower_leg_length/100,h/100,'2D'); 26 | if ~useFastRestart 27 | env.UseFastRestart = 'off'; 28 | end 29 | 30 | %% CREATE NEURAL NETWORKS 31 | createDDPGNetworks; 32 | 33 | %% CREATE AND TRAIN AGENT 34 | createDDPGOptions; 35 | agent = rlDDPGAgent(actor,critic,agentOptions); 36 | trainingResults = train(agent,env,trainingOptions) 37 | 38 | %% SAVE AGENT 39 | reset(agent); % Clears the experience buffer 40 | curDir = pwd; 41 | saveDir = 'savedAgents'; 42 | cd(saveDir) 43 | save(['trainedAgent_2D_' datestr(now,'mm_DD_YYYY_HHMM')],'agent'); 44 | save(['trainingResults_2D_' datestr(now,'mm_DD_YYYY_HHMM')],'trainingResults'); 45 | cd(curDir) -------------------------------------------------------------------------------- /ReinforcementLearning/createWalkingAgent3D.m: -------------------------------------------------------------------------------- 1 | % Walking Robot -- DDPG Agent Training Script (3D) 2 | % Copyright 2019 The MathWorks, Inc. 3 | 4 | %% SET UP ENVIRONMENT 5 | % Speedup options 6 | useFastRestart = true; 7 | useGPU = true; 8 | useParallel = true; 9 | 10 | % Create the observation info 11 | numObs = 43; 12 | observationInfo = rlNumericSpec([numObs 1]); 13 | observationInfo.Name = 'observations'; 14 | 15 | % create the action info 16 | numAct = 10; 17 | actionInfo = rlNumericSpec([numAct 1],'LowerLimit',-1,'UpperLimit', 1); 18 | actionInfo.Name = 'foot_torque'; 19 | 20 | % Environment 21 | mdl = 'walkingRobotRL3D'; 22 | load_system(mdl); 23 | blk = [mdl,'/RL Agent']; 24 | env = rlSimulinkEnv(mdl,blk,observationInfo,actionInfo); 25 | env.ResetFcn = @(in)walkerResetFcn(in,upper_leg_length/100,lower_leg_length/100,h/100,'3D'); 26 | if ~useFastRestart 27 | env.UseFastRestart = 'off'; 28 | end 29 | 30 | %% CREATE NEURAL NETWORKS 31 | createDDPGNetworks; 32 | 33 | %% CREATE AND TRAIN AGENT 34 | createDDPGOptions; 35 | agent = rlDDPGAgent(actor,critic,agentOptions); 36 | trainingResults = train(agent,env,trainingOptions) 37 | 38 | %% SAVE AGENT 39 | reset(agent); % Clears the experience buffer 40 | curDir = pwd; 41 | saveDir = 'savedAgents'; 42 | cd(saveDir) 43 | save(['trainedAgent_3D_' datestr(now,'mm_DD_YYYY_HHMM')],'agent'); 44 | save(['trainingResults_3D_' datestr(now,'mm_DD_YYYY_HHMM')],'trainingResults'); 45 | cd(curDir) -------------------------------------------------------------------------------- /ReinforcementLearning/plotTrainingResults.m: -------------------------------------------------------------------------------- 1 | % Plot reinforcement learning training results given filename 2 | % Copyright 2019 The MathWorks, Inc. 3 | function plotTrainingResults 4 | 5 | %% Interactively load file 6 | close all 7 | filename = uigetfile; 8 | load(filename) 9 | 10 | %% Episode reward plot 11 | figure(1), hold on 12 | plot(trainingResults.EpisodeReward,'bo-'); 13 | plot(trainingResults.AverageReward,'r*-'); 14 | plot(trainingResults.EpisodeQ0,'gx--'); 15 | legend('Episode Reward','Average Reward','Initial Episode Value (Q0)','Location','NorthWest'); 16 | title('Episode Training Plot') 17 | xlabel('Episode Number') 18 | 19 | %% Average steps over time 20 | figure(2), hold on 21 | plot(trainingResults.EpisodeSteps,'bo-'); 22 | plot(trainingResults.TotalAgentSteps./(1:numel(trainingResults.EpisodeSteps))','r*-'); 23 | legend('Episode Steps','Average Steps','Location','NorthWest'); 24 | title('Average Steps Plot') 25 | xlabel('Episode Number') 26 | ylabel('Average Steps') 27 | 28 | %% Episode reward plot vs. steps 29 | figure(3), hold on 30 | plot(trainingResults.TotalAgentSteps,trainingResults.EpisodeReward,'bo-'); 31 | plot(trainingResults.TotalAgentSteps,trainingResults.AverageReward,'r*-'); 32 | plot(trainingResults.TotalAgentSteps,trainingResults.EpisodeQ0,'gx--'); 33 | legend('Episode Reward','Average Reward','Initial Episode Value (Q0)','Location','NorthWest'); 34 | title('Step Training Plot') 35 | xlabel('Total Steps') -------------------------------------------------------------------------------- /ReinforcementLearning/robotParametersRL.m: -------------------------------------------------------------------------------- 1 | % Walking Robot Parameters -- Reinforcement Learning 2 | % Copyright 2019 The MathWorks, Inc. 3 | 4 | %% Model parameters 5 | % Mechanical 6 | density = 500; 7 | foot_density = 1000; 8 | if ~exist('actuatorType','var') 9 | actuatorType = 1; 10 | end 11 | world_damping = 1e-3; 12 | world_rot_damping = 5e-2; 13 | 14 | % Contact/friction parameters 15 | contact_stiffness = 500; 16 | contact_damping = 50; 17 | mu_k = 0.7; 18 | mu_s = 0.9; 19 | mu_vth = 0.01; 20 | height_plane = 0.025; 21 | plane_x = 25; 22 | plane_y = 10; 23 | contact_point_radius = 1e-4; 24 | 25 | % Foot dimensions 26 | foot_x = 5; 27 | foot_y = 4; 28 | foot_z = 1; 29 | foot_offset = [-1 0 0]; 30 | 31 | % Leg dimensions 32 | leg_radius = 0.75; 33 | lower_leg_length = 10; 34 | upper_leg_length = 10; 35 | 36 | % Torso dimensions 37 | torso_y = 8; 38 | torso_x = 5; 39 | torso_z = 8; 40 | torso_offset_z = -2; 41 | torso_offset_x = -1; 42 | mass = (0.01^3)*torso_y*torso_x*torso_z*density; 43 | g = 9.80665; 44 | 45 | % Joint parameters 46 | joint_damping = 1; 47 | joint_stiffness = 0; 48 | motion_time_constant = 0.01; 49 | joint_limit_stiffness = 1e4; 50 | joint_limit_damping = 10; 51 | 52 | %% Reinforcement Learning (RL) parameters 53 | Ts = 0.025; % Agent sample time 54 | Tf = 10; % Simulation end time 55 | 56 | % Scaling factor for RL action [-1 1] 57 | max_torque = 3; 58 | 59 | % Initial conditions 60 | h = 18; % Hip height [cm] 61 | init_height = foot_z + h + ... 62 | torso_z/2 + torso_offset_z + height_plane/2; 63 | vx0 = 0; % Initial X linear velocity [m/s] 64 | vy0 = 0; % Initial Y linear velocity [m/s] 65 | wx0 = 0; % Initial X angular velocity [rad/s] 66 | wy0 = 0; % Initial Y angular velocity [rad/s] 67 | % Initial foot positions [m] 68 | leftinit = [0;0;-h/100]; 69 | rightinit = [0;0;-h/100]; 70 | 71 | % Calculate initial joint angles 72 | init_angs_L = walkerInvKin(leftinit, upper_leg_length/100, lower_leg_length/100,'3D'); 73 | init_angs_R = walkerInvKin(rightinit, upper_leg_length/100, lower_leg_length/100,'3D'); -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainedAgent_2D_1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainedAgent_2D_1.mat -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainedAgent_2D_2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainedAgent_2D_2.mat -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainedAgent_2D_3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainedAgent_2D_3.mat -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainedAgent_2D_4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainedAgent_2D_4.mat -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainedAgent_3D_1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainedAgent_3D_1.mat -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainedAgent_3D_2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainedAgent_3D_2.mat -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainedAgent_3D_3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainedAgent_3D_3.mat -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainedAgent_3D_4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainedAgent_3D_4.mat -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainedAgent_3D_5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainedAgent_3D_5.mat -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainedAgent_3D_6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainedAgent_3D_6.mat -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainingResults_2D_02_22_2019_1713.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainingResults_2D_02_22_2019_1713.mat -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainingResults_2D_02_27_2019_1008.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainingResults_2D_02_27_2019_1008.mat -------------------------------------------------------------------------------- /ReinforcementLearning/savedAgents/trainingResults_3D_11_04_2019_0102.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/savedAgents/trainingResults_3D_11_04_2019_0102.mat -------------------------------------------------------------------------------- /ReinforcementLearning/walkerInvKin.m: -------------------------------------------------------------------------------- 1 | function angs = walkerInvKin(pos, upper_leg_length, lower_leg_length,dim) 2 | % 3D Inverse Kinematics to set the robot leg initial conditions 3 | % The "dim" argument can be set to "2D" or "3D" for the different cases. 4 | % 5 | % Copyright 2019 The MathWorks, Inc. 6 | 7 | % Unpack 8 | x = pos(1); 9 | if isequal(dim,'3D') 10 | y = pos(2); 11 | else 12 | y = 0; 13 | end 14 | z = pos(3); 15 | 16 | % Compute the hip angle 17 | hipRoll = -atan2(y,-z); 18 | 19 | % Call symbolic generated function for 2D leg 20 | zMod = z/cos(hipRoll); 21 | 22 | maxMag = (upper_leg_length + lower_leg_length)*0.99; % Do not allow fully extended 23 | mag = sqrt(x^2 + zMod^2); 24 | if sqrt(x^2 + zMod^2) > maxMag 25 | x = x*maxMag/mag; 26 | zMod = zMod*maxMag/mag; 27 | end 28 | 29 | % Call 2D Inverse Kinematics 30 | theta = legInvKin(upper_leg_length,lower_leg_length,-x,zMod); 31 | 32 | % Address multiple outputs 33 | if size(theta,1) == 2 34 | if theta(1,2) < 0 35 | hipPitch = theta(2,1); 36 | kneePitch = theta(2,2); 37 | else 38 | hipPitch = theta(1,1); 39 | kneePitch = theta(1,2); 40 | end 41 | else 42 | hipPitch = theta(1); 43 | kneePitch = theta(2); 44 | end 45 | 46 | % Set ankle angles 47 | anklePitch = -kneePitch - hipPitch; 48 | ankleRoll = -hipRoll; 49 | 50 | % Pack 51 | angs = [ankleRoll; anklePitch; kneePitch; hipRoll; hipPitch]; 52 | 53 | end -------------------------------------------------------------------------------- /ReinforcementLearning/walkerResetFcn.m: -------------------------------------------------------------------------------- 1 | % Helper function to reset walking robot simulation with different initial conditions 2 | % 3 | % Copyright 2019 The MathWorks, Inc. 4 | 5 | function in = walkerResetFcn(in,upper_leg_length,lower_leg_length,init_height,dim) 6 | 7 | % Randomization Parameters 8 | % Note: Y-direction parameters are only used for the 3D walker model 9 | max_displacement_x = 0.05; 10 | max_speed_x = 0.05; 11 | max_displacement_y = 0.025; 12 | max_speed_y = 0.025; 13 | 14 | % Chance of randomizing initial conditions 15 | if rand < 0.5 16 | if rand < 0.5 17 | in = in.setVariable('vx0',2*max_speed_x *(rand-0.5)); 18 | in = in.setVariable('vy0',2*max_speed_y *(rand-0.5)); 19 | else 20 | in = in.setVariable('vx0',0); 21 | in = in.setVariable('vy0',0); 22 | end 23 | leftinit = [(rand(2,1)-[0.5;0]).*[2*max_displacement_x; max_displacement_y]; -init_height]; 24 | rightinit = [-leftinit(1); -rand*max_displacement_y ; -init_height]; % Ensure feet are symmetrically positioned for stability 25 | 26 | % Chance of starting from zero initial conditions 27 | else 28 | in = in.setVariable('vx0',0); 29 | in = in.setVariable('vy0',0); 30 | leftinit = [0;0;-init_height]; 31 | rightinit = [0;0;-init_height]; 32 | end 33 | 34 | % Solve Inverse Kinematics for both left and right foot positions 35 | init_angs_L = walkerInvKin(leftinit, upper_leg_length, lower_leg_length, dim); 36 | in = in.setVariable('leftinit',leftinit); 37 | in = in.setVariable('init_angs_L',init_angs_L); 38 | 39 | init_angs_R = walkerInvKin(rightinit, upper_leg_length, lower_leg_length, dim); 40 | in = in.setVariable('rightinit',rightinit); 41 | in = in.setVariable('init_angs_R',init_angs_R); 42 | 43 | end -------------------------------------------------------------------------------- /ReinforcementLearning/walkingRobotRL2D.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/walkingRobotRL2D.slx -------------------------------------------------------------------------------- /ReinforcementLearning/walkingRobotRL3D.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/msra-walking-robot/b2e17faeb8ed569d7e1e7b456dbf20715a68fc32/ReinforcementLearning/walkingRobotRL3D.slx -------------------------------------------------------------------------------- /startupWalkingRobot.m: -------------------------------------------------------------------------------- 1 | % Walking Robot Startup Script 2 | % 3 | % Copyright 2017-2019 The MathWorks, Inc. 4 | 5 | %% Clear everything 6 | clc 7 | clear 8 | close all 9 | 10 | %% Add folders to the path 11 | addpath(genpath('LIPM'), ... % Linear inverted pendulum model (LIPM) files 12 | genpath('ModelingSimulation'), ... % Modeling and simulation files 13 | genpath('Optimization'), ... % Optimization files 14 | genpath('ControlDesign'), ... % Control design files 15 | genpath('ReinforcementLearning'), ... % Reinforcement learning files 16 | genpath('Libraries')); % Other dependencies 17 | 18 | %% Load basic robot parameters from modeling and simulation example 19 | robotParameters 20 | 21 | %% Open the README file 22 | edit README.md --------------------------------------------------------------------------------