├── .gitignore ├── Acrobot ├── Derive_acrobot.m ├── MAIN_passiveSimulate.m ├── acrobotDynamics.m ├── acrobotEnergy.m ├── acrobotKinematics.m ├── animate.m ├── autoGen_acrobotDynamics.m ├── autoGen_acrobotEnergy.m ├── autoGen_acrobotKinematics.m ├── drawAcrobot.m └── plotAcrobot.m ├── Continuous_Finite_LQR ├── Derive_EoM.m ├── MAIN_linearPlant.m ├── MAIN_trajectory.m ├── README.md ├── dynamics.m ├── eventFunc.m ├── finiteLqr.m ├── getContour.m ├── getLinSys.m ├── getLinTraj.m ├── randDisk.m ├── reachableDynamics.m ├── reachableDynamicsRel.m ├── reachableForward.m ├── reachableForwardRel.m ├── rhs.m ├── stabilizingController.m └── trajectoryLqr.m ├── CppSimulator └── BouncingBall_1D │ ├── BouncingBall_1D │ ├── README.txt │ ├── data.csv │ ├── makefile │ ├── plotData.m │ ├── plotData.py │ └── src │ ├── Ball.cpp │ ├── Ball.h │ └── BouncingBall_1D.cpp ├── DoublePendulum ├── Animate_Double_Pendulum.m ├── Cross.m ├── DeriveDynamicsDP.m ├── Double_Pendulum_Dynamics.m ├── Double_Pendulum_Kinematics.m ├── Evaluate_Dynamics.cpp ├── Flatten_Cpp_Params.m ├── Flatten_Param_Struct.m ├── GetParameters.h ├── GetParameters_dynamics.h ├── Kinematics_String_Replace.m ├── MAIN.m ├── ParamStructData.m ├── ParseTmpFile.m ├── Plot_Double_Pendulum.m ├── README.md ├── README.txt ├── Set_Parameters.m ├── WriteDynamicsFile_Cpp.m ├── WriteDynamicsFile_Matlab.m ├── WriteKinematicsFile_Matlab.m ├── Write_Parameter_File.m └── dynamics.cpp ├── DoublePendulumWalker ├── Double-Pendulum-WalkerIPOPTinfo.txt ├── Double_Pendulum_Dynamics.m ├── Double_Pendulum_HeelStrike.m ├── Double_Pendulum_Kinematics.m ├── MAIN.m ├── README.md ├── README.txt ├── SmoothAbs.m ├── WrapAngle.m ├── plotOutput.m ├── walkerContinuous.m └── walkerEndpoint.m ├── FancyDoublePendulum ├── Cartesian │ ├── Derive_Equations_of_Motion │ │ ├── Derive_EoM.m │ │ ├── Write_ActuatorPower.m │ │ ├── Write_ContinuousDynamics.m │ │ ├── Write_Convert.m │ │ ├── Write_Energy.m │ │ ├── Write_Kinematics.m │ │ └── Write_PhaseMap.m │ ├── Falling_Simulation │ │ ├── Controller_PD.m │ │ ├── MAIN_Falling_Simulation.m │ │ ├── plotEnergy.m │ │ ├── plotFrame.m │ │ ├── rhs.m │ │ └── setupController.m │ ├── README.md │ ├── README.txt │ ├── Shared │ │ ├── Make_Struct.m │ │ ├── SmoothAbs.m │ │ ├── SmoothAbsFancy.m │ │ ├── animation.m │ │ ├── getPlotInfo.m │ │ ├── plotStates.m │ │ └── save2pdf.m │ └── computerGeneratedCode │ │ ├── actuatorPower.m │ │ ├── convert.m │ │ ├── dynamics_doubleStance.m │ │ ├── dynamics_flight.m │ │ ├── dynamics_singleStanceOne.m │ │ ├── dynamics_singleStanceTwo.m │ │ ├── energy.m │ │ ├── kinematics.m │ │ └── phaseMap.m ├── Polar │ ├── Derive_Equations_of_Motion │ │ ├── Derive_EoM.m │ │ ├── Write_ActuatorPower.m │ │ ├── Write_ContinuousDynamics.m │ │ ├── Write_Convert.m │ │ ├── Write_Kinematics.m │ │ └── Write_PhaseMap.m │ ├── Falling_Simulation │ │ ├── Controller_PD.m │ │ ├── MAIN_Falling_Simulation.m │ │ ├── plotEnergy.m │ │ ├── plotFrame.m │ │ ├── rhs.m │ │ └── setupController.m │ ├── README.md │ ├── README.txt │ ├── Shared │ │ ├── Make_Struct.m │ │ ├── SmoothAbs.m │ │ ├── SmoothAbsFancy.m │ │ ├── animation.m │ │ ├── getPlotInfo.m │ │ ├── plotStates.m │ │ └── save2pdf.m │ └── computerGeneratedCode │ │ ├── actuatorPower.m │ │ ├── convert.m │ │ ├── dynamics_doubleStance.m │ │ ├── dynamics_flight.m │ │ ├── dynamics_singleStanceOne.m │ │ ├── dynamics_singleStanceTwo.m │ │ ├── kinematics.m │ │ └── phaseMap.m └── README.md ├── LagrangeMechanics ├── README.md ├── doublePendulum │ ├── EoM_Double_Pendulum.m │ ├── MAIN_doublePendulum.m │ ├── README.md │ ├── doublePendulumAnimate.m │ ├── doublePendulumDynamics.m │ ├── doublePendulumEnergy.m │ ├── doublePendulumPosition.m │ ├── writeDoublePendulumDynamics.m │ ├── writeDoublePendulumEnergy.m │ └── writeDoublePendulumPosition.m ├── doublePendulumForced │ ├── EoM_Double_Pendulum.m │ ├── MAIN_doublePendulum.m │ ├── README.md │ ├── doublePendulumAnimate.m │ ├── doublePendulumDynamics.m │ ├── doublePendulumEnergy.m │ ├── doublePendulumPosition.m │ ├── writeDoublePendulumDynamics.m │ ├── writeDoublePendulumEnergy.m │ └── writeDoublePendulumPosition.m ├── nLinkPendulum │ ├── EoM_nLink_pendulum.m │ ├── MAIN_nLink_pendulum.m │ ├── README.md │ ├── dynamics_10_link.m │ ├── dynamics_10_link.prj │ ├── dynamics_3_link.m │ ├── dynamics_5_link.m │ ├── dynamics_6_link.m │ ├── dynamics_7_link.m │ ├── energy_10_link.m │ ├── energy_3_link.m │ ├── energy_5_link.m │ ├── energy_6_link.m │ ├── energy_7_link.m │ ├── nLinkAnimate.m │ ├── position_10_link.m │ ├── position_3_link.m │ ├── position_5_link.m │ ├── position_6_link.m │ ├── position_7_link.m │ ├── testFile │ ├── testFile.m │ ├── testFileCpp │ ├── writeDynamics.m │ ├── writeEnergy.m │ └── writePosition.m ├── polarPointMass │ ├── Derive_EoM.m │ ├── MAIN.m │ ├── README.md │ ├── controller.m │ ├── dynamics.m │ └── write_dynamics.m ├── rollingCartPole │ ├── EoM_Rolling_Cart_Pole.m │ ├── rollingCartPoleDynamics.m │ └── writeRollingCartPoleDynamics.m ├── simpleHarmonicOscillator │ ├── EoM_simpleHarmonicOscillator.m │ ├── MAIN_singleHarmonicOscillator.m │ ├── README.md │ └── simpleHarmonicOscillatorDynamics.m ├── simpleRocket │ ├── EoM_Derivation.m │ ├── MAIN.m │ ├── README.md │ ├── controller.m │ ├── rhs.m │ ├── rocketDynamics.m │ └── writeDynamics.m ├── singlePendulum │ ├── EoM_Single_Pendulum.m │ ├── MAIN_singlePendulum.m │ ├── README.md │ ├── singlePendulumDynamics.m │ ├── singlePendulumEnergy.m │ └── singlePendulumRhs.m └── springCartPole │ ├── EoM_Spring_Cart_Pole.m │ ├── springCartPoleDynamics.m │ └── writeSpringCartPoleDynamics.m ├── MDP_Pendulum ├── MAIN_solvePendulum.m ├── MDP_Pendulum.m ├── README.md ├── TEST_barycentric.m ├── barycentricInterpolate.m ├── barycentricInterpolate.prj ├── barycentricWeights.m ├── barycentricWeights.prj ├── drawBoundingBox.m ├── pendulumController.m ├── pendulumDynamics.m ├── pendulumSystem.m ├── rk4.m └── smoothEdgePenalty.m ├── MatlabAnimationTutorial ├── 1_simple_plot │ ├── MAIN.m │ └── cartPoleDynamics.m ├── 2_simple_drawing │ ├── MAIN.m │ ├── cartPoleDynamics.m │ ├── cartPolePosition.m │ └── plotPendulumCart.m ├── 3_multiple_drawing │ ├── MAIN.m │ ├── cartPoleDynamics.m │ ├── cartPolePosition.m │ ├── drawCartPole.m │ └── plotCartPole.m ├── 4_simple_animation │ ├── MAIN.m │ ├── cartPoleDynamics.m │ ├── cartPolePosition.m │ ├── drawCartPole.m │ └── plotCartPole.m ├── 5_pretty_animation │ ├── MAIN.m │ ├── cartPoleDynamics.m │ ├── cartPolePosition.m │ ├── drawCartPole.m │ ├── getStarVerticies.m │ └── plotCartPole.m ├── 6_create_gif │ ├── .gitignore │ ├── MAIN.m │ ├── cartPoleDynamics.m │ ├── cartPolePosition.m │ ├── drawCartPole.m │ ├── getStarVerticies.m │ └── plotCartPole.m ├── 7_create_mp4 │ ├── .gitignore │ ├── MAIN.m │ ├── cartPoleDynamics.m │ ├── cartPolePosition.m │ ├── drawCartPole.m │ ├── getStarVerticies.m │ └── plotCartPole.m ├── 8_animation_callBacks │ ├── MAIN.m │ ├── animate.m │ ├── cartPoleDynamics.m │ ├── cartPolePosition.m │ ├── drawCartPole.m │ ├── getStarVerticies.m │ └── plotCartPole.m └── Derive_CartPole │ ├── Derive_cartPole.m │ ├── TEST_cartPoleDynamics.m │ ├── TEST_humanReadableDynamics.m │ ├── autoGen_cartPoleDynamics.m │ ├── autoGen_cartPoleEnergy.m │ ├── autoGen_cartPoleKinematics.m │ ├── cartPoleDynamics.m │ ├── cartPoleDynamicsHumanReadable.m │ ├── cartPoleEnergy.m │ ├── cartPoleKinematics.m │ ├── drawCartPoleTraj.m │ ├── plotCartPole.m │ └── plotPendulumCart.m ├── ParticleSwarmOptimization ├── MAIN.m ├── particleSwarmOptimization.m └── plotIterData.m ├── PendulumCart_SwingUp ├── Chebyshev_Grid_Soln │ ├── Initial_State.m │ ├── MAIN.m │ ├── NonLinCon.m │ ├── Objective_Function.m │ ├── Pendulum_Cart_Dynamics_Forced.m │ ├── Set_Bounds.m │ ├── Set_Parameters.m │ ├── chebyshevDerivative.m │ ├── chebyshevDifferentiationMatrix.m │ ├── chebyshevIntegral.m │ ├── chebyshevInterpolate.m │ ├── chebyshevPoints.m │ ├── convertVecStruct.m │ ├── makeStruct.m │ ├── oldSoln.mat │ ├── smoothAbs.m │ ├── snoptObjective.m │ ├── wrapper_FMINCON.m │ └── wrapper_SNOPT.m ├── GPOPS_2_Soln │ ├── .gitignore │ ├── MAIN.m │ ├── Pendulum_Cart_Dynamics_Forced.m │ ├── oldSoln.mat │ ├── pendulumCart_Continuous.m │ └── pendulumCart_Endpoint.m ├── Piecewise_Linear_Soln │ ├── Animate.m │ ├── Initial_State.m │ ├── MAIN.m │ ├── NonLinCon.m │ ├── Objective_Function.m │ ├── Pendulum_Cart_Dynamics_Forced.m │ ├── Set_Bounds.m │ ├── Set_Parameters.m │ ├── animatePendulumCart.m │ ├── convertVecStruct.m │ ├── makeStruct.m │ ├── oldSoln.mat │ ├── plotPendulumCart.m │ ├── smoothAbs.m │ ├── snoptObjective.m │ ├── stopActionPendulumCart.m │ ├── wrapper_FMINCON.m │ └── wrapper_SNOPT.m ├── README.md └── README.txt ├── README.md ├── RobotArmTrajctory ├── MAIN.m ├── MAIN_multiSegment.m ├── MAIN_robotArmTask.m ├── README.md ├── chebyshevDerivative.m ├── chebyshevDifferentiationMatrix.m ├── chebyshevInterpolate.m ├── chebyshevPoints.m ├── chebyshevSegment.m ├── getDefaultPlotColors.m ├── smoothJointTrajectory.m └── structBlkDiag.m ├── SimpleTutorials ├── pd_controller.m └── pendulum.m ├── ThreeMassPeriodicOrbits ├── bvp4c │ ├── MAIN_bvp4c.m │ ├── boundaryConditions.m │ └── dynamics.m └── rk4ms │ ├── MAIN_bvp4c.m │ ├── boundaryConditions.m │ └── dynamics.m ├── TrajectoryOptimization ├── Example_1_Cannon │ ├── MAIN_cannon.m │ ├── README.md │ ├── TEST_cannonDynamics.m │ ├── TEST_cannonFeasibility.m │ ├── cannon.svg │ ├── cannonDynamics.m │ ├── cannon_collocation.m │ ├── cannon_directCollocation.gif │ ├── cannon_directCollocation.svg │ ├── cannon_gpops.m │ ├── cannon_multipleShooting.gif │ ├── cannon_multipleShooting.m │ ├── cannon_multipleShooting.svg │ ├── cannon_singleShooting.gif │ ├── cannon_singleShooting.m │ ├── cannon_singleShooting.svg │ ├── diagnostics_collocation.m │ ├── diagnostics_multipleShooting.m │ ├── diagnostics_singleShooting.m │ ├── drawTree.m │ ├── groundEvent.m │ ├── objective.m │ ├── plotBackground.m │ ├── plotSoln.m │ ├── rainbow.m │ ├── rk4.m │ ├── rk4_cannon.m │ └── simulateCannon.m ├── Example_2_CartPole │ ├── Derive_EoM.m │ ├── MAIN.m │ ├── TEST_cartPoleDynamics.m │ ├── animate.m │ ├── autoGen_cartPoleDynamics.m │ ├── autoGen_cartPoleInvDyn.m │ ├── autoGen_cartPoleKinematics.m │ ├── buildConstraints.m │ ├── cartPoleDynamics.m │ ├── cartPoleEqns.svg │ ├── chebyshevScalePoints.m │ ├── computeBounds.m │ ├── computeGuess.m │ ├── costFunction.m │ ├── dirTrans_Euler.m │ ├── dirTrans_Trapz.m │ ├── directCollocation1.m │ ├── directCollocation2.m │ ├── drawCartPole.m │ ├── getIndex.m │ ├── multipleShooting1.m │ ├── multipleShooting2.m │ ├── multipleShooting4.m │ ├── orthogonalCollocation.m │ ├── packDecVar.m │ ├── plotTraj.m │ └── unPackDecVar.m ├── README.md └── RiverCrossing │ ├── DirectCollocation_HermiteSimpson │ ├── MAIN.m │ ├── directCollocation_HermiteSimpson.m │ └── riverBoatDynamics.m │ ├── DirectCollocation_Trapazoid │ ├── MAIN.m │ ├── directCollocation_trapazoid.m │ └── riverBoatDynamics.m │ ├── MultipleShooting_Euler │ ├── MAIN.m │ ├── multipleShooting_Euler.m │ └── riverBoatDynamics.m │ └── README.md ├── Trajectory_Optimization_Hammer_Example ├── Convert_State.m ├── Get_Bounds.m ├── Get_Parameters.m ├── HammerDynamics.m ├── HammerImpact.m ├── Initial_State.m ├── MAIN.m ├── NonLinCon.m ├── Objective_Function.m ├── PhysicsIntegration.m ├── Plotting_Script.m ├── README.md ├── README.txt ├── README.txt~ ├── Save_Figure_Script.m ├── SmoothAbs.m ├── SmoothBnd.m ├── SmoothRamp.m ├── TexFigureCode.txt └── save2pdf.m ├── bezierCurves ├── DEMO_bezierCurve.m ├── DEMO_bezierFunction.m ├── DEMO_fitBezierCurve.m ├── DEMO_rationalBezierCurve.m ├── DEMO_rationalBezierFunction.m ├── README.txt ├── bezierCurve.m ├── fitBezierCurve.m └── rationalBezierCurve.m ├── bouncingBall ├── Animate.m ├── Ball_Dynamics.m ├── EventFunction.m ├── MAIN.m ├── PlotEnergy.m ├── PlotStates.m ├── README.md ├── README.txt ├── Set_Parameters.m ├── groundHeight.m └── impactMap.m ├── chebyshevPolynomials ├── DEMO_10_data_fitting.m ├── DEMO_1_compare.m ├── DEMO_2_derivatives.m ├── DEMO_3_nodeDerivatives.m ├── DEMO_4_Order_vs_Accuracy.m ├── DEMO_5_Multivariate.m ├── DEMO_6_ODE_solve.m ├── DEMO_7_Order_vs_Accuracy_ODE_solve.m ├── DEMO_8_integral.m ├── DEMO_9_chebEval.m ├── Description.html ├── README.md ├── README.txt ├── chebEval.m ├── chebFitLs.m ├── chebyshevDerivative.m ├── chebyshevDifferentiationMatrix.m ├── chebyshevFit.m ├── chebyshevIntegral.m ├── chebyshevInterpolate.m ├── chebyshevMexProject.prj ├── chebyshevODEsolve.m ├── chebyshevPoints.m ├── simpleHarmonicOscillator.m └── testFunction.m ├── cmaes_controlDesign ├── MAIN.m ├── README.md ├── control.m ├── dynamics.m ├── objective.m └── simulate.m ├── dynamicsFuncGradients ├── Derive_Equations.m ├── MAIN.m ├── autoGen_dynamicsAnalytic.m ├── autoGen_dynamicsNumeric.m ├── computeGradientOfBackSlash.m ├── dynamicsAnalytic.m ├── dynamicsNumeric.m └── symbolicGradients.m ├── feedbackLinearization ├── Derive_acrobot.m ├── MAIN_passiveSimulate.m ├── MAIN_runController.m ├── acrobotDynamics.m ├── acrobotEnergy.m ├── acrobotKinematics.m ├── animate.m ├── autoGen_acrobotDynamics.m ├── autoGen_acrobotEnergy.m ├── autoGen_acrobotKinematics.m ├── buildRefTraj.m ├── controlMap.m ├── controller.m ├── drawAcrobot.m └── plotAcrobot.m ├── mex_springPendulum ├── Cpp_Integrator.cpp ├── Cpp_Integrator.mexa64 ├── MAIN.m ├── README.md ├── README.txt ├── dynamics.cpp └── rungeKutta.cpp ├── planarOrbitFallingWeight ├── Derive_EoM.m ├── MAIN.m ├── autoGen_dynamics.m ├── dynamics.m └── problemStatement.svg ├── pwPoly ├── DEMO_fitPoly5.m ├── DEMO_fitPoly5p.m ├── DEMO_fitSpline.m ├── DEMO_pwPoly2.m ├── DEMO_pwPoly4.m ├── DEMO_pwPoly5.m ├── Derive_pwPoly2.m ├── Derive_pwPoly4.m ├── Derive_pwPoly5.m ├── README.md ├── autoGen_pwPoly5.m ├── fitPoly5.m ├── fitPoly5p.m ├── fitSpline.m ├── pwPoly2.m ├── pwPoly4.m └── pwPoly5.m ├── smoothing ├── exponentialSmoothing │ ├── README.md │ ├── README.txt │ ├── SmoothAbs.m │ ├── SmoothAbsFancy.m │ ├── SmoothBnd.m │ ├── SmoothMax.m │ ├── SmoothRamp.m │ ├── TEST_SmoothAbs.m │ ├── TEST_SmoothAbsFancy.m │ ├── TEST_SmoothBnd.m │ ├── TEST_SmoothMax.m │ └── TEST_SmoothRamp.m └── polynomialSmoothing │ ├── README.md │ ├── README.txt │ ├── SolveCoeff.m │ ├── TEST_smoothAbs.m │ ├── TEST_smoothBnd.m │ ├── TEST_smoothRamp.m │ ├── smoothAbs.m │ ├── smoothBnd.m │ ├── smoothRamp.m │ ├── writeSmoothAbs.m │ └── writeSmoothRamp.m ├── toppling_stick ├── EoM_flight.m ├── EoM_hinge.m ├── EoM_slide.m ├── FIGURE_Critical_Angle.m ├── FIGURE_PointMassTopple.m ├── FIGURE_phaseDiagram.m ├── FIGURE_sliding_distance.m ├── MAIN_fsm.m ├── README.md ├── README.md~ ├── SlideDistanceLimit.m ├── ToppleFromRest.m ├── animation.m ├── dynamics_flight.m ├── dynamics_hinge.m ├── dynamics_slideNeg.m ├── dynamics_slidePos.m ├── events_flight.m ├── events_hinge.m ├── events_slideNeg.m ├── events_slidePos.m ├── fsm.m ├── getBoundaries.m ├── getSlipDist.m ├── plotData.m ├── runSimulation.m ├── runSlipExperiment.m ├── save2pdf.m ├── selectPhase.m ├── simulate_flight.m ├── simulate_hinge.m ├── simulate_slideNeg.m ├── simulate_slidePos.m ├── stitchData.m ├── topple_angularRate.m ├── topple_contactHorizontal.m ├── topple_contactVertical.m ├── topple_criticalMu.m ├── writeSlide.m └── writeTopple.m └── tractorTrailer ├── A_Matrix.m ├── Actuator_Model.m ├── Animate_System.m ├── B_Matrix.m ├── Controller.m ├── Create_Trajectory.m ├── Create_Video.m ├── Derive_EoM.m ├── Dot.m ├── Dynamics.m ├── Estimator.m ├── MAIN.m ├── PLOT_Estimator.m ├── PLOT_Inputs.m ├── PLOT_Map.m ├── PLOT_Stop_Action.m ├── README.md ├── README.txt ├── Select_Goal_Point.m ├── Sensors.m ├── Set_Gain_Matrix.m ├── Set_Parameters.m ├── Simulator.m └── Trajectory_Examples.m /.gitignore: -------------------------------------------------------------------------------- 1 | # TexMaker autogen files: 2 | *.aux 3 | *.bbl 4 | *.blg 5 | *.log 6 | *.synctex.gz 7 | *.tks 8 | *.*~ 9 | 10 | # Matlab automatically generated files: 11 | codegen/ 12 | *.mex* 13 | **/*.*~ 14 | **/*.asv 15 | 16 | #GPOPS2 automatically generated info files: 17 | *IPOPTinfo.txt 18 | *SNOPTinfo.txt 19 | 20 | -------------------------------------------------------------------------------- /Acrobot/acrobotDynamics.m: -------------------------------------------------------------------------------- 1 | function dz = acrobotDynamics(z,u,p) 2 | % dz = acrobotDynamics(z,u,p) 3 | % 4 | % This function computes the dynamics of the acrobot: double pendulum, two 5 | % point masses, torque motor between links, no friction. 6 | % 7 | % INPUTS: 8 | % z = [4,1] = state vector 9 | % u = [1,1] = input torque 10 | % p = parameter struct: 11 | % .m1 = elbow mass 12 | % .m2 = wrist mass 13 | % .g = gravitational acceleration 14 | % .l1 = length shoulder to elbow 15 | % .l2 = length elbow to wrist 16 | % 17 | % OUTPUTS: 18 | % dz = [4,1] = dz/dt = time derivative of the state 19 | % 20 | % NOTES: 21 | % 22 | % states: 23 | % 1 = q1 = first link angle 24 | % 2 = q2 = second link angle 25 | % 3 = dq1 = first link angular rate 26 | % 4 = dq2 = second link angular rate 27 | % 28 | % angles: measured from negative j axis with positive convention 29 | % 30 | 31 | 32 | 33 | 34 | q1 = z(1); 35 | q2 = z(2); 36 | dq1 = z(3); 37 | dq2 = z(4); 38 | 39 | % D*ddq + G = B*u 40 | [D,G,B] = autoGen_acrobotDynamics(q1,q2,dq1,dq2,p.m1,p.m2,p.g,p.l1,p.l2); 41 | 42 | ddq = D\(B*u - G); 43 | 44 | dz = [dq1;dq2;ddq]; 45 | 46 | end -------------------------------------------------------------------------------- /Acrobot/acrobotEnergy.m: -------------------------------------------------------------------------------- 1 | function [E,U,T] = acrobotEnergy(z,p) 2 | % [E,U,T] = acrobotEnergy(z,p) 3 | % 4 | % This function computes the mechanical energy of the acrobot. 5 | % 6 | % INPUTS: 7 | % z = [4,n] = state vector 8 | % p = parameter struct: 9 | % .m1 = elbow mass 10 | % .m2 = wrist mass 11 | % .g = gravitational acceleration 12 | % .l1 = length shoulder to elbow 13 | % .l2 = length elbow to wrist 14 | % 15 | % OUTPUTS: 16 | % E = [1,n] = total mechanical energy 17 | % U = [1,n] = potential energy 18 | % T = [1,n] = kinetic energy 19 | % 20 | % NOTES: 21 | % 22 | % states: 23 | % 1 = q1 = first link angle 24 | % 2 = q2 = second link angle 25 | % 3 = dq1 = first link angular rate 26 | % 4 = dq2 = second link angular rate 27 | % 28 | % angles: measured from negative j axis with positive convention 29 | % 30 | 31 | 32 | q1 = z(1,:); 33 | q2 = z(2,:); 34 | dq1 = z(3,:); 35 | dq2 = z(4,:); 36 | 37 | [U,T] = autoGen_acrobotEnergy(q1,q2,dq1,dq2,p.m1,p.m2,p.g,p.l1,p.l2); 38 | 39 | E = U+T; 40 | 41 | end -------------------------------------------------------------------------------- /Acrobot/acrobotKinematics.m: -------------------------------------------------------------------------------- 1 | function [p1,p2,dp1,dp2] = acrobotKinematics(z,p) 2 | % [p1,p2,dp1,dp2] = acrobotKinematics(z,p) 3 | % 4 | % This function computes the mechanical energy of the acrobot. 5 | % 6 | % INPUTS: 7 | % z = [4,n] = state vector 8 | % p = parameter struct: 9 | % .m1 = elbow mass 10 | % .m2 = wrist mass 11 | % .g = gravitational acceleration 12 | % .l1 = length shoulder to elbow 13 | % .l2 = length elbow to wrist 14 | % 15 | % OUTPUTS: 16 | % p1 = [2,n] = position of the elbow joint 17 | % p2 = [2,n] = position of the wrist 18 | % dp1 = [2,n] = velocity of the elbow joint 19 | % dp2 = [2,n] = velocity of the wrist 20 | % 21 | % NOTES: 22 | % 23 | % states: 24 | % 1 = q1 = first link angle 25 | % 2 = q2 = second link angle 26 | % 3 = dq1 = first link angular rate 27 | % 4 = dq2 = second link angular rate 28 | % 29 | % angles: measured from negative j axis with positive convention 30 | % 31 | 32 | q1 = z(1,:); 33 | q2 = z(2,:); 34 | dq1 = z(3,:); 35 | dq2 = z(4,:); 36 | 37 | [p1,p2,dp1,dp2] = autoGen_acrobotKinematics(q1,q2,dq1,dq2,p.l1,p.l2); 38 | 39 | end -------------------------------------------------------------------------------- /Acrobot/autoGen_acrobotDynamics.m: -------------------------------------------------------------------------------- 1 | function [D,G,B] = autoGen_acrobotDynamics(q1,q2,dq1,dq2,m1,m2,g,l1,l2) 2 | %AUTOGEN_ACROBOTDYNAMICS 3 | % [D,G,B] = AUTOGEN_ACROBOTDYNAMICS(Q1,Q2,DQ1,DQ2,M1,M2,G,L1,L2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.2. 6 | % 12-Jun-2015 16:56:47 7 | 8 | t2 = cos(q1); 9 | t3 = l1.^2; 10 | t4 = sin(q1); 11 | t5 = cos(q2); 12 | t6 = l1.*t2; 13 | t7 = l2.*t5; 14 | t8 = t6+t7; 15 | t9 = sin(q2); 16 | t10 = l1.*t4; 17 | t11 = l2.*t9; 18 | t12 = t10+t11; 19 | t13 = l2.^2; 20 | D = reshape([-m1.*t2.^2.*t3-m1.*t3.*t4.^2-l1.*m2.*t2.*t8-l1.*m2.*t4.*t12,-l1.*l2.*m2.*t2.*t5-l1.*l2.*m2.*t4.*t9,-l2.*m2.*t5.*t8-l2.*m2.*t9.*t12,-m2.*t5.^2.*t13-m2.*t9.^2.*t13],[2, 2]); 21 | if nargout > 1 22 | t14 = dq1.^2; 23 | t15 = dq2.^2; 24 | t16 = l1.*t2.*t14; 25 | t17 = l2.*t5.*t15; 26 | t18 = t16+t17; 27 | t19 = l1.*t4.*t14; 28 | t20 = l2.*t9.*t15; 29 | t21 = t19+t20; 30 | G = [-g.*m2.*t12+m2.*t8.*t21-m2.*t12.*t18-g.*l1.*m1.*t4;-g.*l2.*m2.*t9+l2.*m2.*t5.*t21-l2.*m2.*t9.*t18]; 31 | end 32 | if nargout > 2 33 | B = [0.0;-1.0]; 34 | end 35 | -------------------------------------------------------------------------------- /Acrobot/autoGen_acrobotEnergy.m: -------------------------------------------------------------------------------- 1 | function [U,T] = autoGen_acrobotEnergy(q1,q2,dq1,dq2,m1,m2,g,l1,l2) 2 | %AUTOGEN_ACROBOTENERGY 3 | % [U,T] = AUTOGEN_ACROBOTENERGY(Q1,Q2,DQ1,DQ2,M1,M2,G,L1,L2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.2. 6 | % 12-Jun-2015 16:56:48 7 | 8 | t2 = cos(q1); 9 | t3 = dq1.^2; 10 | t4 = l1.^2; 11 | t5 = sin(q1); 12 | t6 = cos(q2); 13 | U = -g.*m2.*(l1.*t2+l2.*t6)-g.*l1.*m1.*t2; 14 | if nargout > 1 15 | t7 = dq1.*l1.*t2+dq2.*l2.*t6; 16 | t8 = dq2.*l2.*sin(q2)+dq1.*l1.*t5; 17 | T = m1.*(t2.^2.*t3.*t4+t3.*t4.*t5.^2).*(1.0./2.0)+m2.*(t7.^2+t8.^2).*(1.0./2.0); 18 | end 19 | -------------------------------------------------------------------------------- /Acrobot/autoGen_acrobotKinematics.m: -------------------------------------------------------------------------------- 1 | function [p1,p2,dp1,dp2] = autoGen_acrobotKinematics(q1,q2,dq1,dq2,l1,l2) 2 | %AUTOGEN_ACROBOTKINEMATICS 3 | % [P1,P2,DP1,DP2] = AUTOGEN_ACROBOTKINEMATICS(Q1,Q2,DQ1,DQ2,L1,L2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.2. 6 | % 12-Jun-2015 16:56:48 7 | 8 | t2 = sin(q1); 9 | t3 = l1.*t2; 10 | t4 = cos(q1); 11 | p1 = [t3;-l1.*t4]; 12 | if nargout > 1 13 | t5 = dq1.*l1.*t4; 14 | t6 = cos(q2); 15 | t7 = dq1.*l1.*t2; 16 | t8 = sin(q2); 17 | p2 = [t3+l2.*t8;-l1.*t4-l2.*t6]; 18 | end 19 | if nargout > 2 20 | dp1 = [t5;t7]; 21 | end 22 | if nargout > 3 23 | dp2 = [t5+dq2.*l2.*t6;t7+dq2.*l2.*t8]; 24 | end 25 | -------------------------------------------------------------------------------- /Acrobot/drawAcrobot.m: -------------------------------------------------------------------------------- 1 | function drawAcrobot(t,z,p) 2 | 3 | clf; hold on; 4 | length = p.l1+p.l2; 5 | axis equal; axis(length*[-1,1,-1,1]); 6 | 7 | [p1,p2] = acrobotKinematics(z,p); 8 | pos = [[0;0],p1,p2]; 9 | 10 | plot(0,0,'ks','MarkerSize',25,'LineWidth',4) 11 | plot(pos(1,:),pos(2,:),'Color',[0.1, 0.8, 0.1],'LineWidth',4) 12 | plot(pos(1,:),pos(2,:),'k.','MarkerSize',50) 13 | 14 | title(sprintf('Acrobot Animation, t = %6.4f',t)); 15 | 16 | drawnow; pause(0.01); 17 | 18 | end -------------------------------------------------------------------------------- /Acrobot/plotAcrobot.m: -------------------------------------------------------------------------------- 1 | function plotAcrobot(t,z,u,p) 2 | 3 | [energy.total, energy.potential, energy.kinetic] = acrobotEnergy(z,p); 4 | 5 | subplot(2,3,1); 6 | plot(t,z(1,:)) 7 | xlabel('t') 8 | ylabel('q1') 9 | title('link one angle') 10 | subplot(2,3,2); 11 | plot(t,z(2,:)) 12 | xlabel('t') 13 | ylabel('q2') 14 | title('link two angle') 15 | subplot(2,3,4); 16 | plot(t,z(3,:)) 17 | xlabel('t') 18 | ylabel('dq1') 19 | title('link one rate') 20 | subplot(2,3,5); 21 | plot(t,z(4,:)) 22 | xlabel('t') 23 | ylabel('dq2') 24 | title('link two rate') 25 | 26 | subplot(2,3,3); hold on 27 | plot(t,energy.total,'k') 28 | plot(t,energy.potential,'r') 29 | plot(t,energy.kinetic,'b') 30 | legend('total','potential','kinetic') 31 | xlabel('t') 32 | ylabel('e'); 33 | title('mechanical energy') 34 | 35 | subplot(2,3,6) 36 | plot(t,u) 37 | xlabel('t') 38 | ylabel('u') 39 | title('torque between links') 40 | 41 | 42 | 43 | end -------------------------------------------------------------------------------- /Continuous_Finite_LQR/Derive_EoM.m: -------------------------------------------------------------------------------- 1 | %Drive Equations of Motion 2 | 3 | %Non-linear 2-dimensional system, with one input 4 | 5 | syms x y u 'real' 6 | 7 | % % dx = y*sin(x) + y*y; 8 | % % dy = x*y + x*cos(x)+ u*cos(x); 9 | 10 | dx = sin(y)+1-cos(u); 11 | dy = sin(x)+sin(u); 12 | 13 | z = [x;y]; 14 | f = [dx;dy]; 15 | 16 | A = jacobian(f,z); 17 | B = jacobian(f,u); 18 | 19 | %Write the dynamics to a file 20 | matlabFunction(f(1),f(2),'file','dynamics.m','vars',{'x','y','u'},'outputs',{'dx','dy'}); 21 | 22 | %Write the linearized dynamics to a file 23 | matlabFunction(A,B,'file','getLinSys.m','vars',{'x','y','u'},'outputs',{'A','B'}); 24 | 25 | 26 | -------------------------------------------------------------------------------- /Continuous_Finite_LQR/MAIN_linearPlant.m: -------------------------------------------------------------------------------- 1 | %MAIN 2 | % 3 | % Demonstrates how to solve the finite-horizon continuous-time linear 4 | % quadratic regulator problem for a linear system 5 | % 6 | 7 | nState = 3; 8 | nInput = 1; 9 | nSoln = 100; 10 | 11 | %LTI plant 12 | A = 0.5*eye(nState) + 0.5*rand(nState); 13 | B = rand(nState,nInput); 14 | 15 | %LTI cost 16 | Q = eye(nState); 17 | R = eye(nInput); 18 | F = zeros(nState); %Terminal Cost 19 | 20 | %Solve continuous LQR problem: 21 | [K,S,E] = lqr(A,B,Q,R); 22 | 23 | %Solve the finite-horizon version 24 | tSpan = [0,30]; %Large time... 25 | tol = 1e-6; 26 | Soln = finiteLqr(tSpan,A,B,Q,R,F,nSoln,tol); 27 | 28 | %Compare the results: (Should go to zero for tSpan(2) goes to infinity) 29 | K_error = K - Soln(1).K 30 | S_error = S - Soln(1).S 31 | E_error = E - Soln(1).E 32 | 33 | %Make a plot showing what the gains look like: 34 | figure(21); clf; 35 | KK = reshape([Soln.K],nState,nSoln); 36 | t = [Soln.t]; 37 | for i=1:nState 38 | subplot(nState,1,i); hold on; 39 | plot(tSpan,[K(i), K(i)],'k--','LineWidth',2) 40 | plot(t,KK(i,:),'r-','LineWidth',2); 41 | ylabel(['K(' num2str(i) ')']); 42 | end 43 | xlabel('Time') 44 | 45 | subplot(nState,1,1); 46 | title('Finite-Horizon Gains, Compare to Infinite Horizon Soln') 47 | 48 | -------------------------------------------------------------------------------- /Continuous_Finite_LQR/README.md: -------------------------------------------------------------------------------- 1 | #README.md 2 | 3 | This directory contains a tutorial for using matlab to stabilize a trajectory of a non-linear system, using a finite-horizon continuous-time Linear Quadratic Regulator (LQR). 4 | 5 | ## Key Files: 6 | 7 | ### MAIN_trajectory: 8 | Entry-point function for the primary demo, using the full non-linear plant 9 | - Plots vector field for non-linear system 10 | - Generate a nominal trajectory 11 | - Approximate trajectory using polynomial curve fitting 12 | - Solve LQR along the trajectory 13 | - Approximate gains using polynomial curve fitting 14 | - Compare the gains obtained from finite vs infinite horizon lqr 15 | - Run simulations to demonstrate controller's stabilizing properties 16 | 17 | ### trajectoryLqr: 18 | Solves the finite-horizon continuous-time LQR problem for a time-varying plant (stabilize a non-linear trajectory) 19 | 20 | ### Derive_EoM: 21 | Derives the dynamics equations and writes function files for the non-linear plant. 22 | 23 | ### MAIN_linearPlant: 24 | A quick little demo to find the finite-horizon continuous-time LQR gains for a linear plant, and then comparing the solutions to the infinite-horizon version, generated using Matlab's `lqr` command. 25 | 26 | ### finiteLqr: 27 | Solves the finite-horizon continuous-time LQR problem for a linear time-invariant plant. 28 | -------------------------------------------------------------------------------- /Continuous_Finite_LQR/dynamics.m: -------------------------------------------------------------------------------- 1 | function [dx,dy] = dynamics(x,y,u) 2 | %DYNAMICS 3 | % [DX,DY] = DYNAMICS(X,Y,U) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.0. 6 | % 19-Nov-2014 20:59:12 7 | 8 | dx = -cos(u)+sin(y)+1.0; 9 | if nargout > 1 10 | dy = sin(u)+sin(x); 11 | end 12 | -------------------------------------------------------------------------------- /Continuous_Finite_LQR/eventFunc.m: -------------------------------------------------------------------------------- 1 | function [value, isterminal, direction] = eventFunc(~,z,xLim,yLim) 2 | %This function is used to stop the simulation if it goes outside of the 3 | %domain of the system. 4 | 5 | value = [... 6 | z(1,:) - xLim(1); 7 | z(1,:) - xLim(2); 8 | z(2,:) - yLim(1); 9 | z(2,:) - yLim(2)]; 10 | isterminal = true(size(value)); 11 | direction = zeros(size(value)); 12 | end -------------------------------------------------------------------------------- /Continuous_Finite_LQR/finiteLqr.m: -------------------------------------------------------------------------------- 1 | function Soln = finiteLqr(tSpan,A,B,Q,R,F,nSoln,tol) 2 | 3 | %This function solves the finite-horizon continuous-time linear quadratic 4 | %regulator problem for a linear plant. 5 | 6 | nState = size(A,1); 7 | nInput = size(B,2); 8 | 9 | userFun = @(t,z)rhs(t,z,A,B,Q,R,nState); 10 | z0 = reshape(F,nState*nState,1); 11 | tSpan = fliplr(tSpan); %Integrate backwards in time 12 | 13 | options = odeset(); 14 | options.RelTol = tol; 15 | options.AbsTol = tol; 16 | sol = ode45(userFun,tSpan,z0); 17 | t = linspace(tSpan(2),tSpan(1),nSoln); 18 | z = deval(sol,t); 19 | 20 | Soln(nSoln).t = 0; 21 | Soln(nSoln).K = zeros(nState,nInput); 22 | Soln(nSoln).S = zeros(nState,nState); 23 | Soln(nSoln).E = zeros(nState,1); 24 | 25 | for i=1:nSoln 26 | zNow = z(:,i); 27 | tNow = t(i); 28 | S = reshape(zNow,nState,nState); 29 | K = R\(B'*S); 30 | Soln(i).t = tNow; 31 | Soln(i).K = K; 32 | Soln(i).S = S; 33 | Soln(i).E = eig(A-B*K); 34 | end 35 | 36 | end 37 | 38 | function dz = rhs(~,z,A,B,Q,R,nState) 39 | P = reshape(z,nState,nState); 40 | dP = ricatti(A,B,Q,R,P); 41 | dz = reshape(dP,nState*nState,1); 42 | end 43 | 44 | function [dP, K] = ricatti(A,B,Q,R,P) 45 | K = R\B'*P; 46 | dP = -(A'*P + P*A - P*B*K + Q); 47 | end -------------------------------------------------------------------------------- /Continuous_Finite_LQR/getContour.m: -------------------------------------------------------------------------------- 1 | function C = getContour(g,data,level) 2 | 3 | %This function is used to obtain a set of level-curves for a surface 4 | %function. Used for computing backward reachable set. 5 | 6 | % There is some weird meshgrid vs ndgrid stuff going on here. 7 | 8 | % MESHGRID vs NDGRID -- switch x and y 9 | c = contourc(g.vs{2}, g.vs{1}, data, [level, level]); 10 | 11 | if isempty(c) 12 | C(1).x = []; 13 | C(1).y = []; 14 | else 15 | idx = 1; %Index of the next delimiter 16 | len = size(c,2); %Total number of indicies 17 | count = 0; %Check number of segments 18 | while idx 1 10 | B = [sin(u);cos(u)]; 11 | end 12 | -------------------------------------------------------------------------------- /Continuous_Finite_LQR/getLinTraj.m: -------------------------------------------------------------------------------- 1 | function [A, B] = getLinTraj(t,xFit,yFit,uFit) 2 | 3 | %Linearizes the system about the nominal trajectory. This function is used 4 | %to turn a non-linear trajectory tracking problem into a time-varying 5 | %linear problem. 6 | 7 | x = polyval(xFit,t); 8 | y = polyval(yFit,t); 9 | u = polyval(uFit,t); 10 | 11 | [A,B] = getLinSys(x,y,u); 12 | 13 | end -------------------------------------------------------------------------------- /Continuous_Finite_LQR/reachableDynamics.m: -------------------------------------------------------------------------------- 1 | function velocity = reachableDynamics(t,~,schemeData) 2 | 3 | %This function calculates the closed-loop backwards dynamics, for use in 4 | %calculation of the backwards reachable set. 5 | 6 | % FORWARD REACHABLE 7 | 8 | xFit = schemeData.fit.x; 9 | yFit = schemeData.fit.y; 10 | uFit = schemeData.fit.u; 11 | kxFit = schemeData.fit.kx; 12 | kyFit = schemeData.fit.ky; 13 | 14 | x = schemeData.grid.xs{1}; 15 | y = schemeData.grid.xs{2}; 16 | 17 | u = stabilizingController(t,x,y,xFit,yFit,uFit,kxFit,kyFit); 18 | 19 | [dx,dy] = dynamics(x,y,u); 20 | 21 | velocity{1} = dx; 22 | velocity{2} = dy; 23 | 24 | end 25 | 26 | -------------------------------------------------------------------------------- /Continuous_Finite_LQR/reachableDynamicsRel.m: -------------------------------------------------------------------------------- 1 | function velocity = reachableDynamicsRel(t,~,schemeData) 2 | 3 | %This function calculates the closed-loop backwards dynamics, for use in 4 | %calculation of the backwards reachable set. 5 | 6 | %FORWARD REACHABLE 7 | 8 | xFit = schemeData.fit.x; 9 | yFit = schemeData.fit.y; 10 | uFit = schemeData.fit.u; 11 | kxFit = schemeData.fit.kx; 12 | kyFit = schemeData.fit.ky; 13 | 14 | xNom = polyval(xFit,t); 15 | yNom = polyval(yFit,t); 16 | 17 | x = schemeData.grid.xs{1} + xNom; 18 | y = schemeData.grid.xs{2} + yNom; 19 | 20 | u = stabilizingController(t,x,y,xFit,yFit,uFit,kxFit,kyFit); 21 | 22 | [dx,dy] = dynamics(x,y,u); 23 | 24 | velocity{1} = dx; 25 | velocity{2} = dy; 26 | 27 | end 28 | 29 | -------------------------------------------------------------------------------- /Continuous_Finite_LQR/rhs.m: -------------------------------------------------------------------------------- 1 | function dz = rhs(~,z,u) 2 | %This function is a wrapper for the automatically generated dynamics 3 | %function. It is designed to be called inside of ode45 4 | 5 | x = z(1,:); y = z(2,:); 6 | [dx,dy] = dynamics(x,y,u); 7 | dz = [dx;dy]; 8 | end -------------------------------------------------------------------------------- /Continuous_Finite_LQR/stabilizingController.m: -------------------------------------------------------------------------------- 1 | function u = stabilizingController(t,x,y,xFit,yFit,uFit,kxFit,kyFit) 2 | 3 | %This function is a controller that is used to stabilize the system to a 4 | %nominal trajectory. 5 | 6 | xRef = polyval(xFit,t); 7 | yRef = polyval(yFit,t); 8 | uRef = polyval(uFit,t); 9 | Kx = polyval(kxFit,t); 10 | Ky = polyval(kyFit,t); 11 | 12 | u = uRef - Kx.*(x-xRef) - Ky.*(y-yRef); 13 | 14 | end -------------------------------------------------------------------------------- /CppSimulator/BouncingBall_1D/BouncingBall_1D: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MatthewPeterKelly/dscTutorials/e1e97a9be03ec146f88bd6ddd9e06db7ee52e242/CppSimulator/BouncingBall_1D/BouncingBall_1D -------------------------------------------------------------------------------- /CppSimulator/BouncingBall_1D/README.txt: -------------------------------------------------------------------------------- 1 | README -- Bouncing Ball 2 | 3 | This is a simple 1D simulator for a bouncing ball. It uses symplectic euler integration with simple (linear) event detection. The output is a .csv file with three columns: time, position, velocity. There is a matlab script to plot the data, as well as a python script. The python script requires SciPi (http://www.scipy.org/install.html). For now, all parameters have been hard-coded to make it easier to read and understand the source code. 4 | 5 | Compile code: ./make 6 | Run code: ./BouncingBall_1D -------------------------------------------------------------------------------- /CppSimulator/BouncingBall_1D/makefile: -------------------------------------------------------------------------------- 1 | all: 2 | g++ -o "BouncingBall_1D" src/Ball.cpp src/BouncingBall_1D.cpp -------------------------------------------------------------------------------- /CppSimulator/BouncingBall_1D/plotData.m: -------------------------------------------------------------------------------- 1 | %plotData.m 2 | % 3 | % This script loads the data file produced by the c++ code and then plots 4 | % it to a figure. 5 | % 6 | 7 | data = csvread('data.csv'); 8 | t = data(:,1); 9 | p = data(:,2); 10 | v = data(:,3); 11 | 12 | figure(1); clf; 13 | subplot(2,1,1) 14 | plot(t,p) 15 | title('position') 16 | subplot(2,1,2) 17 | plot(t,v) 18 | title('velocity') -------------------------------------------------------------------------------- /CppSimulator/BouncingBall_1D/plotData.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import numpy 4 | from matplotlib import pyplot as plt 5 | 6 | data = numpy.genfromtxt('data.csv',skiprows=0,delimiter=',') 7 | 8 | time = data[:,0] 9 | pos = data[:,1] 10 | 11 | plt.plot(time,pos) 12 | plt.title('bouncing ball') 13 | plt.xlabel('time (s)') 14 | plt.ylabel('height (m)') 15 | plt.show() -------------------------------------------------------------------------------- /CppSimulator/BouncingBall_1D/src/Ball.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Ball.cpp 3 | * 4 | * Created on: Mar 14, 2015 5 | * Author: Matthew P. Kelly 6 | */ 7 | 8 | #ifndef BALL_H_ 9 | #define BALL_H_ 10 | 11 | #include 12 | 13 | class Ball { 14 | private: 15 | double t; // time 16 | double p; // position 17 | double v; // velocity 18 | double k; // coefficient of restitution 19 | double dt; // time step for integration method 20 | double g; // gravity 21 | double quadSolve(double a, double b, double c, double tLow, double tUpp); 22 | public: 23 | Ball(); 24 | virtual ~Ball(); 25 | void printState(FILE* file); 26 | void setTime(double time); 27 | void setTimeStep(double timeStep); 28 | void setPosition(double pos); 29 | void setVelocity(double vel); 30 | bool timeStep(); // Runs a single time step of physics 31 | }; 32 | 33 | #endif /* BALL_H_ */ 34 | -------------------------------------------------------------------------------- /CppSimulator/BouncingBall_1D/src/BouncingBall_1D.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : BouncingBall_1D.cpp 3 | // Author : Matthew P. Kelly 4 | // Description : Simulation of simple bouncing ball (1D) 5 | //============================================================================ 6 | 7 | #include "Ball.h" 8 | 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | 14 | int main() { 15 | Ball* ball = new Ball(); 16 | FILE* dataFile; 17 | dataFile = fopen("data.csv","w"); 18 | 19 | int bounceCount = 0; 20 | bool bounce; 21 | ball->printState(dataFile); 22 | while (bounceCount < 4){ 23 | bounce = ball->timeStep(); 24 | ball->printState(dataFile); 25 | if (bounce){ 26 | bounceCount++; 27 | } 28 | } 29 | 30 | fclose(dataFile); 31 | // system("./plotData.py"); // Calls python plotting script 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /DoublePendulum/Cross.m: -------------------------------------------------------------------------------- 1 | function AxB = Cross(A,B) 2 | %This is a custom implementation of the Cross product operator; 3 | %A and B must be (2x1) symbolic matricies, and AxB is a symbolic variable. 4 | %It assumes that A and B are planar (with components i and j) and that AxB 5 | %is the value of the number out of plane (in the k direction) 6 | 7 | AxB = A(1)*B(2) - A(2)*B(1); 8 | 9 | end -------------------------------------------------------------------------------- /DoublePendulum/Flatten_Cpp_Params.m: -------------------------------------------------------------------------------- 1 | function Cpp_Params = Flatten_Cpp_Params(P_Cpp,Data) 2 | 3 | %FUNCTION: 4 | % This function takes a state struct S, and collapses it to a column 5 | % vector s. It relies on information about the struct format that is 6 | % stored in P.StateStruct, which is created by the 7 | % Define_StateStruct_Data.m function. 8 | % 9 | %INPUTS: 10 | % P = the struct the is being used ito pass parameters 11 | % Data = contains information about P 12 | % 13 | %OUTPUTS: 14 | % param_vec = a column vector with all of the data in P 15 | % 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 17 | 18 | Names = Data.Names; 19 | Sizes = Data.Sizes; 20 | Idx = Data.Idx; 21 | 22 | Length = Idx(end,2); %How many elements in the entire state 23 | Cpp_Params = zeros(Length,1); %Total size of the state struct 24 | N = length(Names); %total number of fields; 25 | L = Sizes(:,1).*Sizes(:,2); %Length of each field; 26 | 27 | for i=1:N 28 | Cpp_Params(Idx(i,1):Idx(i,2)) = reshape(P_Cpp.(Names{i})',L(i),1); %Transpose accounts for C++ array definition difference 29 | end 30 | 31 | end -------------------------------------------------------------------------------- /DoublePendulum/Kinematics_String_Replace.m: -------------------------------------------------------------------------------- 1 | %Kinematics_String_Replace 2 | 3 | r_P1_O_CHAR_X = strrep(r_P1_O_CHAR_X,IN,OUT); 4 | v_P1_O_CHAR_X = strrep(v_P1_O_CHAR_X,IN,OUT); 5 | a_P1_O_CHAR_X = strrep(a_P1_O_CHAR_X,IN,OUT); 6 | 7 | r_P2_O_CHAR_X = strrep(r_P2_O_CHAR_X,IN,OUT); 8 | v_P2_O_CHAR_X = strrep(v_P2_O_CHAR_X,IN,OUT); 9 | a_P2_O_CHAR_X = strrep(a_P2_O_CHAR_X,IN,OUT); 10 | 11 | r_P1_O_CHAR_Y = strrep(r_P1_O_CHAR_Y,IN,OUT); 12 | v_P1_O_CHAR_Y = strrep(v_P1_O_CHAR_Y,IN,OUT); 13 | a_P1_O_CHAR_Y = strrep(a_P1_O_CHAR_Y,IN,OUT); 14 | 15 | r_P2_O_CHAR_Y = strrep(r_P2_O_CHAR_Y,IN,OUT); 16 | v_P2_O_CHAR_Y = strrep(v_P2_O_CHAR_Y,IN,OUT); 17 | a_P2_O_CHAR_Y = strrep(a_P2_O_CHAR_Y,IN,OUT); -------------------------------------------------------------------------------- /DoublePendulum/MAIN.m: -------------------------------------------------------------------------------- 1 | %MAIN -- Run Simulation of Double Pendulum 2 | 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | %%%% 5 | %%%% Written by Matthew Kelly 6 | %%%% October 12, 2013 7 | %%%% 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | 10 | 11 | %Set the parameters for the model and derive the dynamics 12 | P = Set_Parameters(); 13 | 14 | %U is a vector of torque and force inputs on the system. For now, simulate 15 | %the unforced system. 16 | U = zeros(6,1); 17 | 18 | %What kind of dynamics function? 19 | if P.misc.CppFlag 20 | userFunc = @(t,X)Evaluate_Dynamics(X,U,P.CppVec); 21 | else 22 | userFunc = @(t,X)Double_Pendulum_Dynamics(t,X,U,P.dyn); 23 | end 24 | 25 | %Initialize the integration 26 | X0 = P.sim.InitState; 27 | Tspan = P.sim.Tspan; 28 | 29 | %Run integration 30 | disp('Running simulation...') 31 | OdeOpt = P.sim.OdeOpt; 32 | [tout, xout] = ode45(userFunc,Tspan,X0,OdeOpt); 33 | 34 | %Compute kinematics 35 | [K, E] = Double_Pendulum_Kinematics(xout',P.dyn,U); 36 | 37 | %Animation: 38 | Animate_Double_Pendulum(tout,K,P) 39 | 40 | %Plotting 41 | Plot_Double_Pendulum(tout,xout,E,P) -------------------------------------------------------------------------------- /DoublePendulumWalker/README.md: -------------------------------------------------------------------------------- 1 | Double Pendulum Walker -- Trajectory Optimization 2 | 3 | =================================================== 4 | 5 | This code runs a trajectory optimization using GPOPS2 to find an optimal periodic walking trajectory for the double pendulum model of walking: point mass at the hip and at the swing foot, ankle actuator, hip actuator, and push-off impulse. 6 | 7 | Run: MAIN.m 8 | 9 | The dynamics were derived automatically (code not included) 10 | -------------------------------------------------------------------------------- /DoublePendulumWalker/README.txt: -------------------------------------------------------------------------------- 1 | README -- Double Pendulum Walker -- Trajectory Optimization 2 | 3 | This code runs a trajectory optimization using GPOPS2 to find an optimal periodic walking trajectory for the double pendulum model of walking: point mass at the hip and at the swing foot, ankle actuator, hip actuator, and push-off impulse. 4 | 5 | Run: MAIN.m 6 | 7 | The dynamics were derived automatically (code not included) -------------------------------------------------------------------------------- /DoublePendulumWalker/WrapAngle.m: -------------------------------------------------------------------------------- 1 | function AngleMod = WrapAngle(Angle) 2 | 3 | %This function takes an angle and maps it to the range [-pi,pi]; 4 | 5 | Angle = Angle + pi; %Shift up 6 | 7 | MinAng = min(Angle); 8 | while MinAng < 0 9 | Angle = Angle + 2*pi; %Wrap around again 10 | MinAng = min(Angle); 11 | end 12 | 13 | AngleMod = mod(Angle,2*pi); 14 | 15 | AngleMod = AngleMod - pi; %Shift back 16 | 17 | end -------------------------------------------------------------------------------- /FancyDoublePendulum/Cartesian/Falling_Simulation/rhs.m: -------------------------------------------------------------------------------- 1 | function dStates = rhs(~,States,Phase,P) 2 | 3 | %This function is a wrapper for the controller and dynamics functions that 4 | %makes them easier to call from ode45. 5 | 6 | 7 | Actuators = Controller_PD(States(:)',P.Control,Phase); 8 | 9 | switch Phase 10 | case 'F' 11 | dStates = dynamics_flight(States(:)',Actuators,P.Dyn)'; 12 | case 'D' 13 | dStates = dynamics_doubleStance(States(:)',Actuators,P.Dyn)'; 14 | case 'S1' 15 | dStates = dynamics_singleStanceOne(States(:)',Actuators,P.Dyn)'; 16 | case 'S2' 17 | dStates = dynamics_singleStanceTwo(States(:)',Actuators,P.Dyn)'; 18 | otherwise 19 | error('Invalid phase, must be element of {''F'',''D'',''S1'',''S2''}'); 20 | end 21 | 22 | end -------------------------------------------------------------------------------- /FancyDoublePendulum/Cartesian/Shared/Make_Struct.m: -------------------------------------------------------------------------------- 1 | function StructOutput = Make_Struct(varargin) 2 | 3 | %This function is used to convert a list of matlab variables to elements of 4 | %a struct. 5 | 6 | N_Inputs = length(varargin); 7 | 8 | for i=1:N_Inputs 9 | name = inputname(i); 10 | StructOutput.(name) = varargin{i}; 11 | end 12 | 13 | end -------------------------------------------------------------------------------- /FancyDoublePendulum/Cartesian/Shared/SmoothAbs.m: -------------------------------------------------------------------------------- 1 | function lxl = SmoothAbs(x,alpha) 2 | % 3 | %lXl = SmoothAbs(x,alpha) 4 | % 5 | %This function is a smooth version of absolute value. There are two 6 | %versions of the function. If called with two arguments it will use 7 | %hyperbolic tangent smoothing, and if called with 4 arguments it will use a 8 | %fancy smoothing that allows for the slope to be adjusted on each side. 9 | % 10 | %INPUTS: 11 | % x = a vector of inputs to smooth 12 | % alpha = smoothing parameter, alpha > 0. 13 | % Small alpha (1e-4) => no smoothing 14 | % large alpha (1e-1) => heavy smoothing 15 | % (alpha sample values assumes x on the order of 1) 16 | %OUTPUTS: 17 | % lxl = the smooth version of x 18 | % 19 | % 20 | % Written by Matthew Kelly 21 | % October 2013 22 | % Cornell University 23 | % 24 | lxl = x.*tanh(x/alpha); 25 | 26 | end -------------------------------------------------------------------------------- /FancyDoublePendulum/Cartesian/Shared/plotStates.m: -------------------------------------------------------------------------------- 1 | function plotStates(plotInfo,figNums) 2 | 3 | %Creates a few plots to show the states of the system. 4 | 5 | %figNums must be an integer vector of length 3; 6 | 7 | D = plotInfo.data; 8 | 9 | Color_One = 'r'; 10 | Color_Two = 'b'; 11 | Color_Hip = 'm'; 12 | 13 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 14 | % Angular States % 15 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 16 | 17 | figure(figNums(1)); clf; 18 | 19 | %Leg One Angle 20 | subplot(3,1,1) 21 | 22 | 23 | 24 | end -------------------------------------------------------------------------------- /FancyDoublePendulum/Polar/Falling_Simulation/rhs.m: -------------------------------------------------------------------------------- 1 | function dStates = rhs(~,States,Phase,P) 2 | 3 | %This function is a wrapper for the controller and dynamics functions that 4 | %makes them easier to call from ode45. 5 | 6 | 7 | Actuators = Controller_PD(States,P.Control,Phase); 8 | 9 | switch Phase 10 | case 'F' 11 | dStates = dynamics_flight(States,Actuators,P.Dyn); 12 | case 'D' 13 | dStates = dynamics_doubleStance(States,Actuators,P.Dyn); 14 | case 'S1' 15 | dStates = dynamics_singleStanceOne(States,Actuators,P.Dyn); 16 | case 'S2' 17 | dStates = dynamics_singleStanceTwo(States,Actuators,P.Dyn); 18 | otherwise 19 | error('Invalid phase, must be element of {''F'',''D'',''S1'',''S2''}'); 20 | end 21 | 22 | end -------------------------------------------------------------------------------- /FancyDoublePendulum/Polar/Shared/Make_Struct.m: -------------------------------------------------------------------------------- 1 | function StructOutput = Make_Struct(varargin) 2 | 3 | %This function is used to convert a list of matlab variables to elements of 4 | %a struct. 5 | 6 | N_Inputs = length(varargin); 7 | 8 | for i=1:N_Inputs 9 | name = inputname(i); 10 | StructOutput.(name) = varargin{i}; 11 | end 12 | 13 | end -------------------------------------------------------------------------------- /FancyDoublePendulum/Polar/Shared/SmoothAbs.m: -------------------------------------------------------------------------------- 1 | function lxl = SmoothAbs(x,alpha) 2 | % 3 | %lXl = SmoothAbs(x,alpha) 4 | % 5 | %This function is a smooth version of absolute value. There are two 6 | %versions of the function. If called with two arguments it will use 7 | %hyperbolic tangent smoothing, and if called with 4 arguments it will use a 8 | %fancy smoothing that allows for the slope to be adjusted on each side. 9 | % 10 | %INPUTS: 11 | % x = a vector of inputs to smooth 12 | % alpha = smoothing parameter, alpha > 0. 13 | % Small alpha (1e-4) => no smoothing 14 | % large alpha (1e-1) => heavy smoothing 15 | % (alpha sample values assumes x on the order of 1) 16 | %OUTPUTS: 17 | % lxl = the smooth version of x 18 | % 19 | % 20 | % Written by Matthew Kelly 21 | % October 2013 22 | % Cornell University 23 | % 24 | lxl = x.*tanh(x/alpha); 25 | 26 | end -------------------------------------------------------------------------------- /FancyDoublePendulum/Polar/Shared/plotStates.m: -------------------------------------------------------------------------------- 1 | function plotStates(plotInfo,figNums) 2 | 3 | %Creates a few plots to show the states of the system. 4 | 5 | %figNums must be an integer vector of length 3; 6 | 7 | D = plotInfo.data; 8 | 9 | Color_One = 'r'; 10 | Color_Two = 'b'; 11 | Color_Hip = 'm'; 12 | 13 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 14 | % Angular States % 15 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 16 | 17 | figure(figNums(1)); clf; 18 | 19 | %Leg One Angle 20 | subplot(3,1,1) 21 | 22 | 23 | 24 | end -------------------------------------------------------------------------------- /FancyDoublePendulum/README.md: -------------------------------------------------------------------------------- 1 | Fancy Double Pendulum 2 | 3 | ================================ 4 | 5 | System of three point masses, connected by force and torque actuators. Simple PD control law can be inserted to turn into a spring-mass-damper system in 2D. 6 | 7 | There are two versions 8 | - Polar - State is angles and distances 9 | - Cartesian - State is a set of (x,y) coordinates 10 | -------------------------------------------------------------------------------- /LagrangeMechanics/README.md: -------------------------------------------------------------------------------- 1 | ## README -- Matlab Derivation of Mechanical System Equations of Motion 2 | 3 | This directory contains matlab code for deriving the equations of motion of a few standard (conservative) mechanical systems. Each example also contains an entry-point method (MAIN_systemName.m) which will run a simulation and generate some plots or an animation. 4 | -------------------------------------------------------------------------------- /LagrangeMechanics/doublePendulum/README.md: -------------------------------------------------------------------------------- 1 | ## README -- Double Pendulum 2 | 3 | This directory has methods for deriving the equations of motion of a double pendulum; automatically writing function files for computing the dynamics, kinematics, and energy; running simulations; and animation. 4 | 5 | EoM_Double_Pendulum.m -- This is the tutorial script for generating the equations of motion. 6 | 7 | MAIN_doublePendulum.m -- This script is used to run a simulation and then an animation of the double pendulum. -------------------------------------------------------------------------------- /LagrangeMechanics/doublePendulumForced/README.md: -------------------------------------------------------------------------------- 1 | ## README -- Double Pendulum Forced 2 | 3 | This directory has methods for deriving the equations of motion of a double pendulum; automatically writing function files for computing the dynamics, kinematics, and energy; running simulations; and animation. 4 | 5 | This directory differs from the other double pendulum directory in that it uses the Newton-Euler equations of motion and has torque inputs for each joint. 6 | 7 | EoM_Double_Pendulum.m -- This is the tutorial script for generating the equations of motion. 8 | 9 | MAIN_doublePendulum.m -- This script is used to run a simulation and then an animation of the double pendulum. 10 | -------------------------------------------------------------------------------- /LagrangeMechanics/nLinkPendulum/README.md: -------------------------------------------------------------------------------- 1 | ## README -- N-Link Pendulum 2 | 3 | This directory contains matlab functions and scripts for deriving the equations of motion for a n-link pendulum. 4 | 5 | EoM_nLink_pendulum.m -- Derives the equations of motion 6 | 7 | MAIN_nLink_pendulum.m -- Runs an animation and simulation -------------------------------------------------------------------------------- /LagrangeMechanics/polarPointMass/MAIN.m: -------------------------------------------------------------------------------- 1 | % MAIN -- Polar Point Mass 2 | % 3 | % This script runs a simulation of the polar point mass, with a controller 4 | % that is working to hold the mass at a fixed distance from the origin 5 | % 6 | 7 | P.m = 1; 8 | P.g = 9.81; 9 | P.l = 1; 10 | P.freq = 3*sqrt(P.g/P.l); %Response frequence in controller 11 | P.damp = 1.0; %Damping ratio in controller 12 | P.eNom = 2.5*P.m*P.g*P.l; 13 | 14 | % z = [r; th; dr; dth]; 15 | 16 | z0min = [0.5*P.l; -pi; -0.2; -0.2]; 17 | z0max = [1.5*P.l; pi; 0.2; 0.2]; 18 | 19 | 20 | z0 = z0min + (z0max-z0min).*rand(4,1); 21 | 22 | userFunc = @(t,z)dynamics(t,z,controller(z,P),P); 23 | 24 | tSpan = [0;3]; 25 | 26 | sol = ode45(userFunc,tSpan,z0); 27 | 28 | t = linspace(tSpan(1),tSpan(2),500); 29 | z = deval(sol,t); 30 | u = controller(z,P); 31 | 32 | figure(234); clf; 33 | 34 | subplot(3,2,1); 35 | plot(t,z(1,:)) 36 | ylabel('r') 37 | 38 | subplot(3,2,3); 39 | plot(t,z(3,:)) 40 | ylabel('dr') 41 | 42 | subplot(3,2,5); 43 | plot(t,u) 44 | ylabel('u') 45 | 46 | subplot(3,2,2); 47 | plot(t,z(2,:)) 48 | ylabel('th') 49 | 50 | subplot(3,2,4); 51 | plot(t,z(4,:)) 52 | ylabel('dth') 53 | 54 | -------------------------------------------------------------------------------- /LagrangeMechanics/polarPointMass/README.md: -------------------------------------------------------------------------------- 1 | ### README.md 2 | 3 | This directory contains the Matlab code to automatically derive the 4 | equations of motion for a point mass that is connected to the origin by 5 | a force actuator. 6 | 7 | MAIN.m will run a simulation of the system with a nonlinear controller 8 | that is working to maintain a constant distance between the origin and the 9 | point mass. -------------------------------------------------------------------------------- /LagrangeMechanics/polarPointMass/controller.m: -------------------------------------------------------------------------------- 1 | function u = controller(z,P) 2 | 3 | %A simple controller for the force actuator that connects the point mass to 4 | %the origin. It is just a PD-controller on the distance between the 5 | %point-mass and the origin, with a non-linear term to compensate for 6 | %dynamics of the system. 7 | 8 | m = P.m; %mass 9 | g = P.g; %Gravity 10 | l = P.l; %Nominal length 11 | wn = P.freq; %Response frequency 12 | xi = P.damp; %Damping ratio 13 | 14 | r = z(1,:); %distance from point-mass to origin 15 | th = z(2,:); %angle of vector between point-mass and origin 16 | dr = z(3,:); %dr/dt 17 | dth = z(4,:); %dth/dt 18 | 19 | % Cancels out the nonlinear terms, drives ddr -> 0 20 | % u - m*g*cos(th) = m*(ddr - r*dth*dth) 21 | % u = m*g*cos(th) + m*(0 - r*dth*dth) 22 | uNom = m*g*cos(th) - m*r.*dth.*dth; 23 | 24 | %Linear control over transients 25 | kp = wn^2; 26 | kd = 2*xi*wn; 27 | u = uNom + m*kp*(l - r) + m*kd*(0 - dr); 28 | 29 | end 30 | -------------------------------------------------------------------------------- /LagrangeMechanics/polarPointMass/dynamics.m: -------------------------------------------------------------------------------- 1 | function dz = dynamics(~,z,u,P) 2 | %DZ = DYNAMICS(T,Z,U,P) 3 | % 4 | %FUNCTION: 5 | % This function computes the dynamics for a point-mass that 6 | % is connected to the origin by a force actuator. 7 | % 8 | %INPUTS: 9 | % t = time. Dummy input for ode45. Not used. 10 | % z = [4xn] matrix of states. 11 | % P = struct of parameters 12 | %OUTPUTS: 13 | % dz = [4xn] matrix of state derivatives 14 | % 15 | %NOTES: 16 | % This file was automatically generated by write_dynamics 17 | 18 | m = P.m; %mass 19 | g = P.g ; %gravity 20 | 21 | r = z(1,:); %distance from point-mass to origin 22 | th = z(2,:); %angle of vector between point-mass and origin 23 | dr = z(3,:); %dr/dt 24 | dth = z(4,:); %dth/dt 25 | 26 | ddr = (u + dth.^2.*m.*r - g.*m.*cos(th))./m; 27 | ddth = -(2.*dr.*dth - g.*sin(th))./r; 28 | 29 | dz = [... 30 | dr; %d/dt (distance from point-mass to origin) 31 | dth; %d/dt (angle of vector between point-mass and origin) 32 | ddr; %d/dt (dr/dt) 33 | ddth; %d/dt (dth/dt) 34 | ]; 35 | end 36 | -------------------------------------------------------------------------------- /LagrangeMechanics/simpleHarmonicOscillator/MAIN_singleHarmonicOscillator.m: -------------------------------------------------------------------------------- 1 | %MAIN_simpleHarmonicOscillator.m 2 | % 3 | % This script runs a simulation of a simple harmonic oscillator 4 | 5 | % Use: EoM_Single_Pendulum to write the equations of motion 6 | 7 | m = 1.0; % (kg) mass 8 | k = 1.0; % (N/m) spring constant 9 | 10 | tSpan = [0,10]; %Simulation time interval 11 | 12 | x0 = 0.5; % (m) initial position 13 | v0 = 0; % (m/s) initial velocit 14 | z0 = [x0;v0]; 15 | 16 | userFunc = @(t,z)simpleHarmonicOscillatorDynamics(t,z,m,k); 17 | 18 | options = odeset(... 19 | 'AbsTol',1e-8,... 20 | 'RelTol',1e-8,... 21 | 'Vectorized','on'); 22 | 23 | % Run the simulation! 24 | sol = ode45(userFunc,tSpan,z0,options); 25 | 26 | % Break apart solution for plotting 27 | nPlot = 1000; 28 | time = linspace(tSpan(1),tSpan(2),nPlot); 29 | z = deval(sol,time); %Evaluate solution from ode45 at points in time 30 | x = z(1,:); 31 | v = z(2,:); 32 | 33 | 34 | % Plotting! 35 | 36 | figure(111);clf; 37 | 38 | subplot(2,1,1); 39 | plot(time,x,'k-','LineWidth',2); 40 | xlabel('time (s)') 41 | ylabel('position (m)'); 42 | 43 | subplot(2,1,2); 44 | plot(time,v,'k-','LineWidth',2); 45 | xlabel('time (s)') 46 | ylabel('velocity (m/s)'); 47 | 48 | 49 | -------------------------------------------------------------------------------- /LagrangeMechanics/simpleHarmonicOscillator/README.md: -------------------------------------------------------------------------------- 1 | ## README -- Simple Harmonic Oscillator 2 | 3 | EoM_simpleHarmonicOscillator.m -- This script is a tutorial for using the matlab symbolic toolbox and the lagrange equations to find the equations of motion for a simple harmonic osciallator. 4 | 5 | MAIN_singleHarmonicOscillator.m -- This script runs a simulation of the system and then generates a simple plot. -------------------------------------------------------------------------------- /LagrangeMechanics/simpleHarmonicOscillator/simpleHarmonicOscillatorDynamics.m: -------------------------------------------------------------------------------- 1 | function dz = simpleHarmonicOscillatorDynamics(~,z,k,m) 2 | 3 | x = z(1,:); 4 | v = z(2,:); 5 | 6 | dx = v; 7 | dv = -(k.*x)./m; 8 | 9 | dz = [dx;dv]; 10 | 11 | end -------------------------------------------------------------------------------- /LagrangeMechanics/simpleRocket/README.md: -------------------------------------------------------------------------------- 1 | ## Rocket Dynamics 2 | 3 | This directory contains code, a work in progress, for deriving the equations of motion for a simple rocket, interacting with inverse-square gravity of a planet. The dynamics are up and working, but the rest of the project is still unfinished. -------------------------------------------------------------------------------- /LagrangeMechanics/simpleRocket/controller.m: -------------------------------------------------------------------------------- 1 | function u = controller(q,dq,P) 2 | 3 | %This is a simple controller to try for a stable orbit. It does not work 4 | %very well... It is just hand tuned to get something reasonable... 5 | 6 | r = q(1,:); 7 | th1 = q(2,:); 8 | th2 = q(3,:); 9 | dr = dq(1,:); 10 | dth1 = dq(2,:); 11 | dth2 = dq(3,:); 12 | 13 | escape = sqrt(2*P.G*P.mPlanet./r); 14 | r = r/P.rPlanet; 15 | dr = dr./escape; 16 | phi = th2-th1; 17 | dphi = dth2-dth1; 18 | 19 | thrust = 0.03*(1.5-r) + 0.09*(0 - dr); 20 | tip = 0.2*(pi/2-phi) + 0.2*(0-dphi); 21 | 22 | u1 = thrust + tip; 23 | u2 = thrust - tip; 24 | 25 | u = [u1;u2]; 26 | 27 | end -------------------------------------------------------------------------------- /LagrangeMechanics/simpleRocket/rhs.m: -------------------------------------------------------------------------------- 1 | function dz = rhs(~,z,P) 2 | %Wrapper function for ode45 3 | 4 | q = z(1:3,:); 5 | dq = z(4:6,:); 6 | u = controller(q,dq,P); 7 | ddq = rocketDynamics(0,q,dq,u,P); 8 | 9 | dz = [dq;ddq]; 10 | 11 | end -------------------------------------------------------------------------------- /LagrangeMechanics/singlePendulum/README.md: -------------------------------------------------------------------------------- 1 | ## README -- Simple Pendulum 2 | 3 | The bulk of this tutorial for deriving the equations of motion for the single pendulum are in EoM_Single_Pendulum.m. This script then writes functions for computing the dynamics, kinematics, and energy for the pendulum. 4 | 5 | MAIN_singlePendulum.m can be used to run a simulation of the penulum and make some plots. -------------------------------------------------------------------------------- /LagrangeMechanics/singlePendulum/singlePendulumDynamics.m: -------------------------------------------------------------------------------- 1 | function ddth = singlePendulumDynamics(th,g,l) 2 | %SINGLEPENDULUMDYNAMICS 3 | % DDTH = SINGLEPENDULUMDYNAMICS(TH,G,L) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.0. 6 | % 15-Oct-2014 10:38:25 7 | 8 | ddth = -(g.*sin(th))./l; 9 | -------------------------------------------------------------------------------- /LagrangeMechanics/singlePendulum/singlePendulumEnergy.m: -------------------------------------------------------------------------------- 1 | function [energy,kinetic,potential] = singlePendulumEnergy(th,w,m,g,l) 2 | %SINGLEPENDULUMENERGY 3 | % [ENERGY,KINETIC,POTENTIAL] = SINGLEPENDULUMENERGY(TH,W,M,G,L) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.0. 6 | % 15-Oct-2014 10:38:26 7 | 8 | t2 = l.^2; 9 | t3 = w.^2; 10 | t4 = m.*t2.*t3.*(1.0./2.0); 11 | t5 = cos(th); 12 | t6 = t5-1.0; 13 | energy = t4-g.*l.*m.*t6; 14 | if nargout > 1 15 | kinetic = t4; 16 | end 17 | if nargout > 2 18 | potential = -g.*l.*m.*t6; 19 | end 20 | -------------------------------------------------------------------------------- /LagrangeMechanics/singlePendulum/singlePendulumRhs.m: -------------------------------------------------------------------------------- 1 | function dz = singlePendulumRhs(~,z,g,l) 2 | % This function is used inside of ode45 for running a simulation of a 3 | % single pendulum. The first argument (time) is not used. 4 | 5 | th = z(1,:); 6 | w = z(2,:); 7 | 8 | dth = w; 9 | dw = singlePendulumDynamics(th,g,l); 10 | 11 | dz = [dth;dw]; 12 | 13 | end -------------------------------------------------------------------------------- /MDP_Pendulum/README.md: -------------------------------------------------------------------------------- 1 | # README -- Markov Decision Process - Pendulum 2 | This directory contains code for finding the optimal policy for an inverted pendulum, given actuator and state limits. 3 | 4 | ## Entry-point scripts: 5 | - **MAIN_solvePendulum.m** Script containing all user-set parameters. It calls the function to build the MDP and then solve for the optimal policy. 6 | 7 | - **TEST_barycentric.m** Script that is used to test the barycentric interpolation functions, which are crucial in the conversion from a continuous to a discrete system. 8 | -------------------------------------------------------------------------------- /MDP_Pendulum/barycentricInterpolate.m: -------------------------------------------------------------------------------- 1 | function y = barycentricInterpolate(Y,Idx,W) 2 | % y = barycentricInterpolate(Y,Idx,W) 3 | % 4 | % This function interpolates a point y, based on the data in Y and the 5 | % weights (W) on the verticies Y(Idx) 6 | % 7 | % INPUTS: 8 | % Y = [N, 1] data set. 9 | % W = [d+1, n] weight to apply to each index 10 | % Idx = [d+1, n] linear index corresponding to each weight 11 | % 12 | % OUTPUTS: 13 | % y = [n,d] = function value at query points 14 | % 15 | % NOTES: 16 | % --> y = f(x), where x is d-dimensional, and y is scalar 17 | % --> Y = Y(x1,x2,...,xd) 18 | % --> n = size(Y); 19 | % --> y(X(:,i)) = W(i,:)*Y(Idx(:,i)); 20 | % 21 | % See Also: barycentricWeights 22 | 23 | n = size(Idx,2); 24 | y = zeros(n,1); 25 | 26 | for query=1:n 27 | y(query) = dot(W(:,query),Y(Idx(:,query))); 28 | end 29 | 30 | end -------------------------------------------------------------------------------- /MDP_Pendulum/pendulumController.m: -------------------------------------------------------------------------------- 1 | function u = pendulumController(X,A,P,grid) 2 | % u = pendulumController(X,A,P,grid) 3 | % 4 | % This function computes the optimal control to use at each point in the 5 | % state space for the pendulum. 6 | % 7 | % INPUTS: 8 | % X = [2 x n] = matrix of states to query the controller 9 | % A = action matrix produced by MDP_pendulum.m 10 | % P = policy matrix produced by MDP_pendulum.m 11 | % pendulum = struct with pendulum information. 12 | % 13 | % OUTPUTS: 14 | % u = [1 x n] = torque vector produced by the controller 15 | % 16 | 17 | thBnd = grid.th.bnd; 18 | thCount = grid.th.nGrid; 19 | 20 | wBnd = grid.w.bnd; 21 | wCount = grid.w.nGrid; 22 | 23 | sBnd = [thBnd;wBnd]; 24 | sN = [thCount;wCount]; 25 | 26 | [weight, index] = barycentricWeights_mex(X,sBnd,sN); 27 | u = barycentricInterpolate_mex(A(P)',index,weight)'; 28 | 29 | end -------------------------------------------------------------------------------- /MDP_Pendulum/pendulumDynamics.m: -------------------------------------------------------------------------------- 1 | function ddth = pendulumDynamics(th,dth,u,p) 2 | % ddth = pendulumDynamics(th,dth,u,p) 3 | % 4 | % This function computes the second-order dynamics of a driven damped 5 | % pendulum. 6 | % 7 | 8 | ddth = (p.g/p.l)*sin(th) + (-p.c*dth + u)/(p.m*p.l*p.l); 9 | 10 | end -------------------------------------------------------------------------------- /MDP_Pendulum/pendulumSystem.m: -------------------------------------------------------------------------------- 1 | function dx = pendulumSystem(x,A,P,grid,dyn) 2 | % dx = pendulumSystem(x,A,P,pendulum) 3 | % 4 | % This function computes the closed loop dynamics of the pendulum, using 5 | % the optimal controller from the MDP solve 6 | % 7 | 8 | th = x(1,:); 9 | w = x(2,:); 10 | 11 | u = pendulumController(x,A,P,grid); 12 | dw = pendulumDynamics(th,w,u,dyn); 13 | 14 | dx = [w;dw]; 15 | 16 | end -------------------------------------------------------------------------------- /MDP_Pendulum/smoothEdgePenalty.m: -------------------------------------------------------------------------------- 1 | function y = smoothEdgePenalty(x,xBnd,xDel,yMax) 2 | % y = smoothEdgePenalty(x,xBnd,xDel,yMax) 3 | % 4 | % This function computes a penalty curve, that is near yMax at each point 5 | % in xBnd, and near zero at mean(xBnd). The paramter xDel adjusts how 6 | % rapidly the function changes from ~0 -> ~1. It is based on the hyperbolic 7 | % tangent function. 8 | % 9 | % INPUTS: 10 | % x = list of queries 11 | % xBnd = range of x 12 | % xDel = width of boundry 13 | % yMax = function height 14 | % 15 | 16 | m = 3/xDel; 17 | yUpp = tanh(m*(x-xBnd(2))); 18 | yLow = tanh(-m*(x-xBnd(1))); 19 | y = yMax*(2 + yLow + yUpp); 20 | 21 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/2_simple_drawing/cartPolePosition.m: -------------------------------------------------------------------------------- 1 | function pos = cartPolePosition(z,p) 2 | % pos = cartPolePosition(z,p) 3 | % 4 | % This function computes the position of the cart and the pole, given the 5 | % state of the system 6 | % 7 | % INPUTS: 8 | % z = [4, n] = [x;q;dx;dq] = state of the system 9 | % p = parameter struct 10 | % .g = gravity 11 | % .m1 = cart mass 12 | % .m2 = pole mass 13 | % .l = pendulum length 14 | % OUTPUTS: 15 | % pos = [4, n] = [x1;y1;x2;y2]; = position of [cart; pole] 16 | % 17 | 18 | %%%% unpack the state 19 | x = z(1,:); %Cart position (Not used in dynamics) 20 | q = z(2,:); % pendulum (pole) angle, measure from gravity vector 21 | 22 | %%%% Unpack the physical parameters 23 | l = p.l; %Pendulum length 24 | 25 | %%%% Position of the cart: 26 | x1 = x; 27 | y1 = zeros(size(x)); 28 | 29 | %%%% Position of the pole: 30 | x2 = x1 + l*sin(q); 31 | y2 = y1 - l*cos(q); 32 | 33 | %%%% Pack up position vector: 34 | pos = [x1;y1;x2;y2]; 35 | 36 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/2_simple_drawing/plotPendulumCart.m: -------------------------------------------------------------------------------- 1 | function plotPendulumCart(t,z) 2 | % plotPendulumCart(t,z) 3 | % 4 | % INPUTS: 5 | % t = [1, n] = time stamps for each state vector 6 | % z = [4, n] = [x;q;dx;dq] = state of the system 7 | % 8 | % OUTPUTS: 9 | % a simple plot for each state of the system over the simulation 10 | % 11 | 12 | %%%% Unpack the state: 13 | x = z(1,:); 14 | q = z(2,:); 15 | dx = z(3,:); 16 | dq = z(4,:); 17 | 18 | %%%% Plots: 19 | 20 | subplot(2,2,1); 21 | plot(t,x) 22 | ylabel('x') 23 | title('Position') 24 | 25 | subplot(2,2,2); 26 | plot(t,q) 27 | ylabel('q') 28 | title('Angle') 29 | 30 | subplot(2,2,3); 31 | plot(t,dx) 32 | ylabel('dx') 33 | title('Velocity') 34 | 35 | subplot(2,2,4); 36 | plot(t,dq) 37 | ylabel('dq') 38 | title('Angle Rate') 39 | 40 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/3_multiple_drawing/cartPolePosition.m: -------------------------------------------------------------------------------- 1 | function pos = cartPolePosition(z,p) 2 | % pos = cartPolePosition(z,p) 3 | % 4 | % This function computes the position of the cart and the pole, given the 5 | % state of the system 6 | % 7 | % INPUTS: 8 | % z = [4, n] = [x;q;dx;dq] = state of the system 9 | % p = parameter struct 10 | % .g = gravity 11 | % .m1 = cart mass 12 | % .m2 = pole mass 13 | % .l = pendulum length 14 | % OUTPUTS: 15 | % pos = [4, n] = [x1;y1;x2;y2]; = position of [cart; pole] 16 | % 17 | 18 | %%%% unpack the state 19 | x = z(1,:); %Cart position (Not used in dynamics) 20 | q = z(2,:); % pendulum (pole) angle, measure from gravity vector 21 | 22 | %%%% Unpack the physical parameters 23 | l = p.l; %Pendulum length 24 | 25 | %%%% Position of the cart: 26 | x1 = x; 27 | y1 = zeros(size(x)); 28 | 29 | %%%% Position of the pole: 30 | x2 = x1 + l*sin(q); 31 | y2 = y1 - l*cos(q); 32 | 33 | %%%% Pack up position vector: 34 | pos = [x1;y1;x2;y2]; 35 | 36 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/3_multiple_drawing/drawCartPole.m: -------------------------------------------------------------------------------- 1 | function drawCartPole(time,pos,extents) 2 | % drawCartPole(time,pos,extents) 3 | % 4 | % INPUTS: 5 | % time = [scalar] = current time in the simulation 6 | % pos = [4, 1] = [x1;y1;x2;y2]; = position of [cart; pole] 7 | % extents = [xLow, xUpp, yLow, yUpp] = boundary of the draw window 8 | % 9 | % OUTPUTS: 10 | % --> A pretty drawing of a cart-pole 11 | % 12 | 13 | %%%% Unpack the positions: 14 | x1 = pos(1); 15 | y1 = pos(2); 16 | x2 = pos(3); 17 | y2 = pos(4); 18 | 19 | % Title and simulation time: 20 | title(sprintf('t = %2.2f%',time)); 21 | 22 | % Draw the rail that the cart-pole travels on 23 | plot(extents(1:2),[0,0],'k-','LineWidth',2); 24 | 25 | % Draw the cart: 26 | plot(x1, y1, 'bs','MarkerSize',30,'LineWidth',5); 27 | 28 | % Draw the pole: 29 | plot([x1,x2], [y1, y2], 'r-','LineWidth',2); 30 | 31 | % Draw the bob of the pendulum: 32 | plot(x2, y2, 'ro','MarkerSize',22,'LineWidth',4); 33 | 34 | % Format the axis so things look right: 35 | axis equal; axis(extents); axis off; % <-- Order is important here 36 | 37 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/3_multiple_drawing/plotCartPole.m: -------------------------------------------------------------------------------- 1 | function plotCartPole(t,z) 2 | % plotCartPole(t,z) 3 | % 4 | % INPUTS: 5 | % t = [1, n] = time stamps for each state vector 6 | % z = [4, n] = [x;q;dx;dq] = state of the system 7 | % 8 | % OUTPUTS: 9 | % a simple plot for each state of the system over the simulation 10 | % 11 | 12 | %%%% Unpack the state: 13 | x = z(1,:); 14 | q = z(2,:); 15 | dx = z(3,:); 16 | dq = z(4,:); 17 | 18 | %%%% Plots: 19 | 20 | subplot(2,2,1); 21 | plot(t,x) 22 | ylabel('x') 23 | title('Position') 24 | 25 | subplot(2,2,2); 26 | plot(t,q) 27 | ylabel('q') 28 | title('Angle') 29 | 30 | subplot(2,2,3); 31 | plot(t,dx) 32 | ylabel('dx') 33 | title('Velocity') 34 | 35 | subplot(2,2,4); 36 | plot(t,dq) 37 | ylabel('dq') 38 | title('Angle Rate') 39 | 40 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/4_simple_animation/cartPolePosition.m: -------------------------------------------------------------------------------- 1 | function pos = cartPolePosition(z,p) 2 | % pos = cartPolePosition(z,p) 3 | % 4 | % This function computes the position of the cart and the pole, given the 5 | % state of the system 6 | % 7 | % INPUTS: 8 | % z = [4, n] = [x;q;dx;dq] = state of the system 9 | % p = parameter struct 10 | % .g = gravity 11 | % .m1 = cart mass 12 | % .m2 = pole mass 13 | % .l = pendulum length 14 | % OUTPUTS: 15 | % pos = [4, n] = [x1;y1;x2;y2]; = position of [cart; pole] 16 | % 17 | 18 | %%%% unpack the state 19 | x = z(1,:); %Cart position (Not used in dynamics) 20 | q = z(2,:); % pendulum (pole) angle, measure from gravity vector 21 | 22 | %%%% Unpack the physical parameters 23 | l = p.l; %Pendulum length 24 | 25 | %%%% Position of the cart: 26 | x1 = x; 27 | y1 = zeros(size(x)); 28 | 29 | %%%% Position of the pole: 30 | x2 = x1 + l*sin(q); 31 | y2 = y1 - l*cos(q); 32 | 33 | %%%% Pack up position vector: 34 | pos = [x1;y1;x2;y2]; 35 | 36 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/4_simple_animation/drawCartPole.m: -------------------------------------------------------------------------------- 1 | function drawCartPole(time,pos,extents) 2 | % drawCartPole(time,pos,extents) 3 | % 4 | % INPUTS: 5 | % time = [scalar] = current time in the simulation 6 | % pos = [4, 1] = [x1;y1;x2;y2]; = position of [cart; pole] 7 | % extents = [xLow, xUpp, yLow, yUpp] = boundary of the draw window 8 | % 9 | % OUTPUTS: 10 | % --> A pretty drawing of a cart-pole 11 | % 12 | 13 | %%%% Unpack the positions: 14 | x1 = pos(1); 15 | y1 = pos(2); 16 | x2 = pos(3); 17 | y2 = pos(4); 18 | 19 | % Title and simulation time: 20 | title(sprintf('t = %2.2f%',time)); 21 | 22 | % Draw the rail that the cart-pole travels on 23 | plot(extents(1:2),[0,0],'k-','LineWidth',2); 24 | 25 | % Draw the cart: 26 | plot(x1, y1, 'bs','MarkerSize',30,'LineWidth',5); 27 | 28 | % Draw the pole: 29 | plot([x1,x2], [y1, y2], 'r-','LineWidth',2); 30 | 31 | % Draw the bob of the pendulum: 32 | plot(x2, y2, 'ro','MarkerSize',22,'LineWidth',4); 33 | 34 | % Format the axis so things look right: 35 | axis equal; axis(extents); axis off; % <-- Order is important here 36 | 37 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/4_simple_animation/plotCartPole.m: -------------------------------------------------------------------------------- 1 | function plotCartPole(t,z) 2 | % plotCartPole(t,z) 3 | % 4 | % INPUTS: 5 | % t = [1, n] = time stamps for each state vector 6 | % z = [4, n] = [x;q;dx;dq] = state of the system 7 | % 8 | % OUTPUTS: 9 | % a simple plot for each state of the system over the simulation 10 | % 11 | 12 | %%%% Unpack the state: 13 | x = z(1,:); 14 | q = z(2,:); 15 | dx = z(3,:); 16 | dq = z(4,:); 17 | 18 | %%%% Plots: 19 | 20 | subplot(2,2,1); 21 | plot(t,x) 22 | ylabel('x') 23 | title('Position') 24 | 25 | subplot(2,2,2); 26 | plot(t,q) 27 | ylabel('q') 28 | title('Angle') 29 | 30 | subplot(2,2,3); 31 | plot(t,dx) 32 | ylabel('dx') 33 | title('Velocity') 34 | 35 | subplot(2,2,4); 36 | plot(t,dq) 37 | ylabel('dq') 38 | title('Angle Rate') 39 | 40 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/5_pretty_animation/cartPolePosition.m: -------------------------------------------------------------------------------- 1 | function pos = cartPolePosition(z,p) 2 | % pos = cartPolePosition(z,p) 3 | % 4 | % This function computes the position of the cart and the pole, given the 5 | % state of the system 6 | % 7 | % INPUTS: 8 | % z = [4, n] = [x;q;dx;dq] = state of the system 9 | % p = parameter struct 10 | % .g = gravity 11 | % .m1 = cart mass 12 | % .m2 = pole mass 13 | % .l = pendulum length 14 | % OUTPUTS: 15 | % pos = [4, n] = [x1;y1;x2;y2]; = position of [cart; pole] 16 | % 17 | 18 | %%%% unpack the state 19 | x = z(1,:); %Cart position (Not used in dynamics) 20 | q = z(2,:); % pendulum (pole) angle, measure from gravity vector 21 | 22 | %%%% Unpack the physical parameters 23 | l = p.l; %Pendulum length 24 | 25 | %%%% Position of the cart: 26 | x1 = x; 27 | y1 = zeros(size(x)); 28 | 29 | %%%% Position of the pole: 30 | x2 = x1 + l*sin(q); 31 | y2 = y1 - l*cos(q); 32 | 33 | %%%% Pack up position vector: 34 | pos = [x1;y1;x2;y2]; 35 | 36 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/5_pretty_animation/getStarVerticies.m: -------------------------------------------------------------------------------- 1 | function star = getStarVerticies(n,r) 2 | % star = getStarVerticies(n,r) 3 | % 4 | % This function draws a star with n points centered on the origin. 5 | % 6 | % INPUTS: 7 | % n = number of points 8 | % r = inner radius (outer radius == 1) 9 | % 10 | % OUTPUTS: 11 | % star = [2, 2*n+1] = [x;y] coordinates of verticies 12 | % 13 | 14 | th = linspace(0,2*pi,2*n+1); 15 | star = [... 16 | sin(th); 17 | cos(th)]; 18 | idx = 2:2:(2*n); 19 | star(:,idx) = r*star(:,idx); 20 | 21 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/5_pretty_animation/plotCartPole.m: -------------------------------------------------------------------------------- 1 | function plotCartPole(t,z) 2 | % plotCartPole(t,z) 3 | % 4 | % INPUTS: 5 | % t = [1, n] = time stamps for each state vector 6 | % z = [4, n] = [x;q;dx;dq] = state of the system 7 | % 8 | % OUTPUTS: 9 | % a simple plot for each state of the system over the simulation 10 | % 11 | 12 | %%%% Unpack the state: 13 | x = z(1,:); 14 | q = z(2,:); 15 | dx = z(3,:); 16 | dq = z(4,:); 17 | 18 | %%%% Plots: 19 | 20 | subplot(2,2,1); 21 | plot(t,x) 22 | ylabel('x') 23 | title('Position') 24 | 25 | subplot(2,2,2); 26 | plot(t,q) 27 | ylabel('q') 28 | title('Angle') 29 | 30 | subplot(2,2,3); 31 | plot(t,dx) 32 | ylabel('dx') 33 | title('Velocity') 34 | 35 | subplot(2,2,4); 36 | plot(t,dq) 37 | ylabel('dq') 38 | title('Angle Rate') 39 | 40 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/6_create_gif/.gitignore: -------------------------------------------------------------------------------- 1 | # Animation data files: 2 | *.gif -------------------------------------------------------------------------------- /MatlabAnimationTutorial/6_create_gif/cartPolePosition.m: -------------------------------------------------------------------------------- 1 | function pos = cartPolePosition(z,p) 2 | % pos = cartPolePosition(z,p) 3 | % 4 | % This function computes the position of the cart and the pole, given the 5 | % state of the system 6 | % 7 | % INPUTS: 8 | % z = [4, n] = [x;q;dx;dq] = state of the system 9 | % p = parameter struct 10 | % .g = gravity 11 | % .m1 = cart mass 12 | % .m2 = pole mass 13 | % .l = pendulum length 14 | % OUTPUTS: 15 | % pos = [4, n] = [x1;y1;x2;y2]; = position of [cart; pole] 16 | % 17 | 18 | %%%% unpack the state 19 | x = z(1,:); %Cart position (Not used in dynamics) 20 | q = z(2,:); % pendulum (pole) angle, measure from gravity vector 21 | 22 | %%%% Unpack the physical parameters 23 | l = p.l; %Pendulum length 24 | 25 | %%%% Position of the cart: 26 | x1 = x; 27 | y1 = zeros(size(x)); 28 | 29 | %%%% Position of the pole: 30 | x2 = x1 + l*sin(q); 31 | y2 = y1 - l*cos(q); 32 | 33 | %%%% Pack up position vector: 34 | pos = [x1;y1;x2;y2]; 35 | 36 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/6_create_gif/getStarVerticies.m: -------------------------------------------------------------------------------- 1 | function star = getStarVerticies(n,r) 2 | % star = getStarVerticies(n,r) 3 | % 4 | % This function draws a star with n points centered on the origin. 5 | % 6 | % INPUTS: 7 | % n = number of points 8 | % r = inner radius (outer radius == 1) 9 | % 10 | % OUTPUTS: 11 | % star = [2, 2*n+1] = [x;y] coordinates of verticies 12 | % 13 | 14 | th = linspace(0,2*pi,2*n+1); 15 | star = [... 16 | sin(th); 17 | cos(th)]; 18 | idx = 2:2:(2*n); 19 | star(:,idx) = r*star(:,idx); 20 | 21 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/6_create_gif/plotCartPole.m: -------------------------------------------------------------------------------- 1 | function plotCartPole(t,z) 2 | % plotCartPole(t,z) 3 | % 4 | % INPUTS: 5 | % t = [1, n] = time stamps for each state vector 6 | % z = [4, n] = [x;q;dx;dq] = state of the system 7 | % 8 | % OUTPUTS: 9 | % a simple plot for each state of the system over the simulation 10 | % 11 | 12 | %%%% Unpack the state: 13 | x = z(1,:); 14 | q = z(2,:); 15 | dx = z(3,:); 16 | dq = z(4,:); 17 | 18 | %%%% Plots: 19 | 20 | subplot(2,2,1); 21 | plot(t,x) 22 | ylabel('x') 23 | title('Position') 24 | 25 | subplot(2,2,2); 26 | plot(t,q) 27 | ylabel('q') 28 | title('Angle') 29 | 30 | subplot(2,2,3); 31 | plot(t,dx) 32 | ylabel('dx') 33 | title('Velocity') 34 | 35 | subplot(2,2,4); 36 | plot(t,dq) 37 | ylabel('dq') 38 | title('Angle Rate') 39 | 40 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/7_create_mp4/.gitignore: -------------------------------------------------------------------------------- 1 | # Animation data files: 2 | *.mp4 -------------------------------------------------------------------------------- /MatlabAnimationTutorial/7_create_mp4/cartPolePosition.m: -------------------------------------------------------------------------------- 1 | function pos = cartPolePosition(z,p) 2 | % pos = cartPolePosition(z,p) 3 | % 4 | % This function computes the position of the cart and the pole, given the 5 | % state of the system 6 | % 7 | % INPUTS: 8 | % z = [4, n] = [x;q;dx;dq] = state of the system 9 | % p = parameter struct 10 | % .g = gravity 11 | % .m1 = cart mass 12 | % .m2 = pole mass 13 | % .l = pendulum length 14 | % OUTPUTS: 15 | % pos = [4, n] = [x1;y1;x2;y2]; = position of [cart; pole] 16 | % 17 | 18 | %%%% unpack the state 19 | x = z(1,:); %Cart position (Not used in dynamics) 20 | q = z(2,:); % pendulum (pole) angle, measure from gravity vector 21 | 22 | %%%% Unpack the physical parameters 23 | l = p.l; %Pendulum length 24 | 25 | %%%% Position of the cart: 26 | x1 = x; 27 | y1 = zeros(size(x)); 28 | 29 | %%%% Position of the pole: 30 | x2 = x1 + l*sin(q); 31 | y2 = y1 - l*cos(q); 32 | 33 | %%%% Pack up position vector: 34 | pos = [x1;y1;x2;y2]; 35 | 36 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/7_create_mp4/getStarVerticies.m: -------------------------------------------------------------------------------- 1 | function star = getStarVerticies(n,r) 2 | % star = getStarVerticies(n,r) 3 | % 4 | % This function draws a star with n points centered on the origin. 5 | % 6 | % INPUTS: 7 | % n = number of points 8 | % r = inner radius (outer radius == 1) 9 | % 10 | % OUTPUTS: 11 | % star = [2, 2*n+1] = [x;y] coordinates of verticies 12 | % 13 | 14 | th = linspace(0,2*pi,2*n+1); 15 | star = [... 16 | sin(th); 17 | cos(th)]; 18 | idx = 2:2:(2*n); 19 | star(:,idx) = r*star(:,idx); 20 | 21 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/7_create_mp4/plotCartPole.m: -------------------------------------------------------------------------------- 1 | function plotCartPole(t,z) 2 | % plotCartPole(t,z) 3 | % 4 | % INPUTS: 5 | % t = [1, n] = time stamps for each state vector 6 | % z = [4, n] = [x;q;dx;dq] = state of the system 7 | % 8 | % OUTPUTS: 9 | % a simple plot for each state of the system over the simulation 10 | % 11 | 12 | %%%% Unpack the state: 13 | x = z(1,:); 14 | q = z(2,:); 15 | dx = z(3,:); 16 | dq = z(4,:); 17 | 18 | %%%% Plots: 19 | 20 | subplot(2,2,1); 21 | plot(t,x) 22 | ylabel('x') 23 | title('Position') 24 | 25 | subplot(2,2,2); 26 | plot(t,q) 27 | ylabel('q') 28 | title('Angle') 29 | 30 | subplot(2,2,3); 31 | plot(t,dx) 32 | ylabel('dx') 33 | title('Velocity') 34 | 35 | subplot(2,2,4); 36 | plot(t,dq) 37 | ylabel('dq') 38 | title('Angle Rate') 39 | 40 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/8_animation_callBacks/cartPolePosition.m: -------------------------------------------------------------------------------- 1 | function pos = cartPolePosition(z,p) 2 | % pos = cartPolePosition(z,p) 3 | % 4 | % This function computes the position of the cart and the pole, given the 5 | % state of the system 6 | % 7 | % INPUTS: 8 | % z = [4, n] = [x;q;dx;dq] = state of the system 9 | % p = parameter struct 10 | % .g = gravity 11 | % .m1 = cart mass 12 | % .m2 = pole mass 13 | % .l = pendulum length 14 | % OUTPUTS: 15 | % pos = [4, n] = [x1;y1;x2;y2]; = position of [cart; pole] 16 | % 17 | 18 | %%%% unpack the state 19 | x = z(1,:); %Cart position (Not used in dynamics) 20 | q = z(2,:); % pendulum (pole) angle, measure from gravity vector 21 | 22 | %%%% Unpack the physical parameters 23 | l = p.l; %Pendulum length 24 | 25 | %%%% Position of the cart: 26 | x1 = x; 27 | y1 = zeros(size(x)); 28 | 29 | %%%% Position of the pole: 30 | x2 = x1 + l*sin(q); 31 | y2 = y1 - l*cos(q); 32 | 33 | %%%% Pack up position vector: 34 | pos = [x1;y1;x2;y2]; 35 | 36 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/8_animation_callBacks/getStarVerticies.m: -------------------------------------------------------------------------------- 1 | function star = getStarVerticies(n,r) 2 | % star = getStarVerticies(n,r) 3 | % 4 | % This function draws a star with n points centered on the origin. 5 | % 6 | % INPUTS: 7 | % n = number of points 8 | % r = inner radius (outer radius == 1) 9 | % 10 | % OUTPUTS: 11 | % star = [2, 2*n+1] = [x;y] coordinates of verticies 12 | % 13 | 14 | th = linspace(0,2*pi,2*n+1); 15 | star = [... 16 | sin(th); 17 | cos(th)]; 18 | idx = 2:2:(2*n); 19 | star(:,idx) = r*star(:,idx); 20 | 21 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/8_animation_callBacks/plotCartPole.m: -------------------------------------------------------------------------------- 1 | function plotCartPole(t,z) 2 | % plotCartPole(t,z) 3 | % 4 | % INPUTS: 5 | % t = [1, n] = time stamps for each state vector 6 | % z = [4, n] = [x;q;dx;dq] = state of the system 7 | % 8 | % OUTPUTS: 9 | % a simple plot for each state of the system over the simulation 10 | % 11 | 12 | %%%% Unpack the state: 13 | x = z(1,:); 14 | q = z(2,:); 15 | dx = z(3,:); 16 | dq = z(4,:); 17 | 18 | %%%% Plots: 19 | 20 | subplot(2,2,1); 21 | plot(t,x) 22 | ylabel('x') 23 | title('Position') 24 | 25 | subplot(2,2,2); 26 | plot(t,q) 27 | ylabel('q') 28 | title('Angle') 29 | 30 | subplot(2,2,3); 31 | plot(t,dx) 32 | ylabel('dx') 33 | title('Velocity') 34 | 35 | subplot(2,2,4); 36 | plot(t,dq) 37 | ylabel('dq') 38 | title('Angle Rate') 39 | 40 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/Derive_CartPole/TEST_humanReadableDynamics.m: -------------------------------------------------------------------------------- 1 | % TEST - Human readable dynamics 2 | % 3 | % This is a simple script to check that the "human readable" version of the 4 | % dynamics matches the computer generated version. 5 | 6 | 7 | p.m1 = 1.0; % (kg) Cart mass 8 | p.m2 = 0.3; % (kg) pole mass 9 | p.g = 9.81; % (m/s^2) gravity 10 | p.l = 0.5; % (m) pendulum (pole) length 11 | 12 | z = 10*randn(4,1000); %Random state 13 | u = 10*randn(1,1000); %Random control 14 | 15 | dzSym = cartPoleDynamics(z,u,p); 16 | dzHuman = cartPoleDynamicsHumanReadable(z,u,p); 17 | 18 | err = abs(dzSym-dzHuman); 19 | maxErr = max(max(err)); 20 | if maxErr > 1e-12 21 | error('Dynamics do not match!'); 22 | end 23 | 24 | fprintf('\n\n Max Error: %8.8g \n\n',max(max(err))); 25 | -------------------------------------------------------------------------------- /MatlabAnimationTutorial/Derive_CartPole/autoGen_cartPoleDynamics.m: -------------------------------------------------------------------------------- 1 | function [ddx,ddq] = autoGen_cartPoleDynamics(q,dq,u,m1,m2,g,l) 2 | %AUTOGEN_CARTPOLEDYNAMICS 3 | % [DDX,DDQ] = AUTOGEN_CARTPOLEDYNAMICS(Q,DQ,U,M1,M2,G,L) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.3. 6 | % 30-Nov-2015 20:13:06 7 | 8 | t2 = cos(q); 9 | t3 = sin(q); 10 | t4 = t2.^2; 11 | t5 = m1+m2-m2.*t4; 12 | t6 = 1.0./t5; 13 | t7 = dq.^2; 14 | ddx = t6.*(u+g.*m2.*t2.*t3+l.*m2.*t3.*t7); 15 | if nargout > 1 16 | ddq = -(t6.*(t2.*u+g.*m1.*t3+g.*m2.*t3+l.*m2.*t2.*t3.*t7))./l; 17 | end 18 | -------------------------------------------------------------------------------- /MatlabAnimationTutorial/Derive_CartPole/autoGen_cartPoleEnergy.m: -------------------------------------------------------------------------------- 1 | function [U,T] = autoGen_cartPoleEnergy(x,q,dx,dq,m1,m2,g,l) 2 | %AUTOGEN_CARTPOLEENERGY 3 | % [U,T] = AUTOGEN_CARTPOLEENERGY(X,Q,DX,DQ,M1,M2,G,L) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.3. 6 | % 30-Nov-2015 20:13:07 7 | 8 | t2 = cos(q); 9 | U = -g.*l.*m2.*t2; 10 | if nargout > 1 11 | t3 = dx+dq.*l.*t2; 12 | t4 = sin(q); 13 | T = dx.^2.*m1.*(1.0./2.0)+m2.*(t3.^2+dq.^2.*l.^2.*t4.^2).*(1.0./2.0); 14 | end 15 | -------------------------------------------------------------------------------- /MatlabAnimationTutorial/Derive_CartPole/autoGen_cartPoleKinematics.m: -------------------------------------------------------------------------------- 1 | function [p1,p2,dp1,dp2] = autoGen_cartPoleKinematics(x,q,dx,dq,l,empty) 2 | %AUTOGEN_CARTPOLEKINEMATICS 3 | % [P1,P2,DP1,DP2] = AUTOGEN_CARTPOLEKINEMATICS(X,Q,DX,DQ,L,EMPTY) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.3. 6 | % 30-Nov-2015 20:13:07 7 | 8 | p1 = [x;empty]; 9 | if nargout > 1 10 | t2 = cos(q); 11 | t3 = sin(q); 12 | p2 = [x+l.*t3;-l.*t2]; 13 | end 14 | if nargout > 2 15 | dp1 = [dx;empty]; 16 | end 17 | if nargout > 3 18 | dp2 = [dx+dq.*l.*t2;dq.*l.*t3]; 19 | end 20 | -------------------------------------------------------------------------------- /MatlabAnimationTutorial/Derive_CartPole/cartPoleDynamics.m: -------------------------------------------------------------------------------- 1 | function dz = cartPoleDynamics(z,u,p) 2 | % dz = cartPoleDynamics(z,u,p) 3 | % 4 | % This function computes the first-order dynamics of the cart-pole. 5 | % 6 | % INPUTS: 7 | % z = [4, n] = [x;q;dx;dq] = state of the system 8 | % u = [1, n] = horizontal force applied to the cart 9 | % p = parameter struct 10 | % .g = gravity 11 | % .m1 = cart mass 12 | % .m2 = pole mass 13 | % .l = pendulum length 14 | % OUTPUTS: 15 | % dz = dz/dt = time derivative of state 16 | % 17 | % 18 | 19 | % x = z(1,:); %Not used in dynamics 20 | q = z(2,:); 21 | dx = z(3,:); 22 | dq = z(4,:); 23 | 24 | [ddx,ddq] = autoGen_cartPoleDynamics(q, dq, u, p.m1, p.m2, p.g, p.l); 25 | 26 | dz = [dx;dq;ddx;ddq]; 27 | 28 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/Derive_CartPole/cartPoleEnergy.m: -------------------------------------------------------------------------------- 1 | function [energy, potential, kinetic] = cartPoleEnergy(z,p) 2 | % [energy, potential, kinetic] = cartPoleEnergy(z,p) 3 | % 4 | % This function computes the mechanical energy of the cart-pole. 5 | % 6 | % INPUTS: 7 | % z = [4, n] = [x;q;dx;dq] = state of the system 8 | % p = parameter struct 9 | % .g = gravity 10 | % .m1 = cart mass 11 | % .m2 = pole mass 12 | % .l = pendulum length 13 | % OUTPUTS: 14 | % energy = total mechanical energy 15 | % potential = potential energy 16 | % kinetic = kinetic energy 17 | % 18 | % 19 | 20 | x = z(1,:); 21 | q = z(2,:); 22 | dx = z(3,:); 23 | dq = z(4,:); 24 | 25 | [potential, kinetic] = autoGen_cartPoleEnergy(x, q, dx, dq, p.m1, p.m2, p.g, p.l); 26 | 27 | energy = potential + kinetic; 28 | 29 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/Derive_CartPole/plotCartPole.m: -------------------------------------------------------------------------------- 1 | function data = plotCartPole(t,z,u,p) 2 | % data = plotCartPole(t,z,u,p) 3 | % 4 | % This function plots the simulation of the pendulum cart 5 | 6 | x = z(1,:); 7 | q = z(2,:); 8 | dx = z(3,:); 9 | dq = z(4,:); 10 | 11 | [energy, potential, kinetic] = cartPoleEnergy(z,p); 12 | 13 | subplot(3,2,1); 14 | plot(t,x) 15 | ylabel('x') 16 | 17 | subplot(3,2,2); 18 | plot(t,q) 19 | ylabel('q') 20 | 21 | subplot(3,2,3); 22 | plot(t,dx) 23 | ylabel('dx') 24 | 25 | subplot(3,2,4); 26 | plot(t,dq) 27 | ylabel('dq') 28 | 29 | subplot(3,2,5) 30 | plot(t,u) 31 | ylabel('u') 32 | xlabel('t') 33 | 34 | subplot(3,2,6); hold on; 35 | plot(t,potential,'b-','LineWidth',1) 36 | plot(t,kinetic,'r-','LineWidth',1) 37 | plot(t,energy,'k-','LineWidth',2) 38 | ylabel('energy') 39 | xlabel('t') 40 | legend('potential','kinetic','total'); 41 | 42 | data.energy = energy; 43 | data.potential = potential; 44 | data.kinetic = kinetic; 45 | 46 | data.x = x; 47 | data.q = q; 48 | data.dx = dx; 49 | data.dq = dq; 50 | 51 | end -------------------------------------------------------------------------------- /MatlabAnimationTutorial/Derive_CartPole/plotPendulumCart.m: -------------------------------------------------------------------------------- 1 | function data = plotCartPole(t,z,u,p) 2 | % data = plotCartPole(t,z,u,p) 3 | % 4 | % This function plots the simulation of the pendulum cart 5 | 6 | x = z(1,:); 7 | q = z(2,:); 8 | dx = z(3,:); 9 | dq = z(4,:); 10 | 11 | [energy, potential, kinetic] = cartPoleEnergy(z,p); 12 | 13 | subplot(3,2,1); 14 | plot(t,x) 15 | ylabel('x') 16 | 17 | subplot(3,2,2); 18 | plot(t,q) 19 | ylabel('q') 20 | 21 | subplot(3,2,3); 22 | plot(t,dx) 23 | ylabel('dx') 24 | 25 | subplot(3,2,4); 26 | plot(t,dq) 27 | ylabel('dq') 28 | 29 | subplot(3,2,5) 30 | plot(t,u) 31 | ylabel('u') 32 | xlabel('t') 33 | 34 | subplot(3,2,6); hold on; 35 | plot(t,potential,'b-','LineWidth',1) 36 | plot(t,kinetic,'r-','LineWidth',1) 37 | plot(t,energy,'k-','LineWidth',2) 38 | ylabel('energy') 39 | xlabel('t') 40 | legend('potential','kinetic','total'); 41 | 42 | data.energy = energy; 43 | data.potential = potential; 44 | data.kinetic = kinetic; 45 | 46 | data.x = x; 47 | data.q = q; 48 | data.dx = dx; 49 | data.dq = dq; 50 | 51 | end -------------------------------------------------------------------------------- /ParticleSwarmOptimization/MAIN.m: -------------------------------------------------------------------------------- 1 | % MAIN.m -- Particle Swarm Optimization 2 | % 3 | % This script is used to run particle swarm optimization on several test 4 | % functions. 5 | % 6 | 7 | quadraticBowl = @(x)( sum(x.^2,1) ); 8 | quadraticBowlSkew2d = @(x)( 0.3*x(1,:).^2 + 4.2*x(2,:).^2 ); 9 | noisyBowl = @(x)( sum(x.^2 + 0.1*randn(size(x)),1) ); 10 | 11 | problem.options.populationCount = 15; % Number of particles in the search 12 | problem.options.w = 0.3; % Particle damping coefficient 13 | problem.options.pg = 0.7; % Global search coefficient 14 | problem.options.pl = 0.5; % Local search coefficient 15 | problem.options.maxIter = 20; % Number of iterations 16 | 17 | problem.xLow = -ones(2,1); 18 | problem.xUpp = ones(2,1); 19 | 20 | problem.objFun = quadraticBowl; 21 | % problem.objFun = quadraticBowlSkew2d; 22 | % problem.objFun = noisyBowl; 23 | 24 | soln = particleSwarmOptimization(problem); 25 | 26 | %%%% Plotting: 27 | figure(1); 28 | for i=1:problem.options.maxIter 29 | plotIterData(soln,i); %Only works for 2D objective function 30 | pause(0.5); 31 | end 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /ParticleSwarmOptimization/plotIterData.m: -------------------------------------------------------------------------------- 1 | function plotIterData(soln,i) 2 | % 3 | % This function plots all of the points for a given iteration, assuming 4 | % that the objective function is 2 dimensional with the origin at zero. 5 | % 6 | 7 | g = soln.log.Gx(:,1,i); %global best 8 | f = soln.log.Gf(1,1,i); %best objective function value 9 | x = soln.log.X(:,:,i); %local search point 10 | p = soln.log.Lx(:,:,i); %local best point 11 | 12 | clf; hold on; 13 | plot(0,0,'ko','MarkerSize',20,'LineWidth',3); 14 | plot(g(1),g(2),'g.','MarkerSize',30); 15 | plot(x(1,:),x(2,:),'k.','MarkerSize',15); 16 | plot(p(1,:),p(2,:),'b.','MarkerSize',8); 17 | title(sprintf('Objective function: %4.4f',f)); 18 | 19 | axis([soln.problem.xLow(1),soln.problem.xUpp(1),soln.problem.xLow(2),soln.problem.xUpp(2)]); 20 | 21 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Chebyshev_Grid_Soln/NonLinCon.m: -------------------------------------------------------------------------------- 1 | function [C, Ceq] = NonLinCon(x,P) 2 | 3 | %This function is used to compute the nonlinear constraints for the 4 | %pendulum cart swing-up problem. In this case, all constraints are defect 5 | %constraints on the state. 6 | 7 | 8 | %Expand the decision variable vector to a struct 9 | X = convertVecStruct(x,P.structDat); 10 | 11 | %Get the full state matrix (but the starting and ending states back on) 12 | States = [P.MS.Start, X.state, P.MS.Finish]; 13 | 14 | %Get the state derivatives using the differentiation matrix: 15 | dStates_Cheby = chebyshevDerivative(States,[0,X.duration]); 16 | 17 | %Get the state derivatives using the system dynamics: 18 | dynFunc = P.Dyn.dynamics_func; 19 | dStates_Dyn = feval(dynFunc,States,X.force,P.Dyn); 20 | 21 | %Compute the defect matrix: 22 | defects = dStates_Cheby - dStates_Dyn; 23 | 24 | %Format things nicely for returning to fmincon: 25 | [n,m] = size(defects); 26 | Ceq = reshape(defects,n*m,1); 27 | C = []; 28 | 29 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Chebyshev_Grid_Soln/Set_Bounds.m: -------------------------------------------------------------------------------- 1 | function [Xlow, Xupp] = Set_Bounds(P) 2 | 3 | Ns = P.MS.nGrid - 2; %We already know the initial and final states! 4 | LOW = 1; 5 | UPP = 2; 6 | 7 | %% Lower Bounds: 8 | Xlow.duration = P.Bnd.duration(LOW); 9 | Xlow.state = P.Bnd.state(:,LOW)*ones(1,Ns); 10 | Xlow.force = P.Bnd.force(LOW)*ones(1,P.MS.nGrid); 11 | 12 | %% Upper Bounds: 13 | Xupp.duration = P.Bnd.duration(UPP); 14 | Xupp.state = P.Bnd.state(:,UPP)*ones(1,Ns); 15 | Xupp.force = P.Bnd.force(UPP)*ones(1,P.MS.nGrid); 16 | 17 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Chebyshev_Grid_Soln/chebyshevDifferentiationMatrix.m: -------------------------------------------------------------------------------- 1 | function D = chebyshevDifferentiationMatrix(n,d) 2 | 3 | if nargin==1 4 | d = [-1,1]; %Default domain 5 | end 6 | 7 | %Get the chebyshev points 8 | x = chebyshevPoints(n,d); 9 | 10 | %Get the weight vector 11 | w = ones(1,n); 12 | w(2:2:n) = -1; 13 | w([1,end]) = w([1,end])/2; 14 | 15 | %First, compute the weighting matrix: 16 | W = (1./w)'*w; 17 | 18 | %Next, compute the matrix with inverse of the node distance 19 | X = zeros(n); 20 | for i=1:n 21 | idx = (i+1):n; 22 | X(i,idx) = 1./(x(i)-x(idx)); 23 | end 24 | 25 | %Use the property that this matrix is anti-symetric 26 | X = X - X'; 27 | 28 | %Compute the i~=j case: 29 | D = W.*X; 30 | 31 | %Deal with the i=j case: 32 | D = D - diag(sum(D,2)); 33 | 34 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Chebyshev_Grid_Soln/chebyshevIntegral.m: -------------------------------------------------------------------------------- 1 | function If = chebyshevIntegral(f,d) 2 | %If = chebyshevIntegral(f,d) 3 | % 4 | % FUNCTION: 5 | % This function computes the integrals of the chebyshev approximation 6 | % using Clenshaw-Curtis quadrature. 7 | % 8 | % INPUTS: 9 | % f = [nState x nPoints] values at each chebyshev node 10 | % d = [1 x 2] domain of the chebyshev function 11 | % 12 | % OUTPUTS: 13 | % If = the integral of the chebyshev interpolant at each chebyshev node 14 | % 15 | 16 | %Get the quadrature weights: 17 | [k,n] = size(f); 18 | [~,w] = chebyshevPoints(n,d); 19 | 20 | %Perform the integral 21 | ONE = ones(k,1); %get the matrix size correct for .* operation 22 | If = cumsum(f.*(ONE*w))*diff(d)/2; 23 | 24 | end 25 | -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Chebyshev_Grid_Soln/makeStruct.m: -------------------------------------------------------------------------------- 1 | function StructOutput = makeStruct(varargin) 2 | 3 | %A struct is created with the property that each field corresponds to one 4 | %of the arguments passed to this function. 5 | % 6 | % Example: 7 | % 8 | % R = makeStruct(a,b,c,d,e); 9 | % 10 | % R.a == a; 11 | % R.b == b; 12 | % R.c == c; 13 | % R.d == d; 14 | % R.e == e; 15 | % 16 | % 17 | 18 | N_Inputs = length(varargin); 19 | 20 | for i=1:N_Inputs 21 | name = inputname(i); 22 | StructOutput.(name) = varargin{i}; 23 | end 24 | 25 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Chebyshev_Grid_Soln/oldSoln.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MatthewPeterKelly/dscTutorials/e1e97a9be03ec146f88bd6ddd9e06db7ee52e242/PendulumCart_SwingUp/Chebyshev_Grid_Soln/oldSoln.mat -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Chebyshev_Grid_Soln/snoptObjective.m: -------------------------------------------------------------------------------- 1 | function F = snoptObjective(x) 2 | 3 | global PARAMETERS; 4 | 5 | %READ from global variable 6 | P = PARAMETERS; 7 | 8 | cost = Objective_Function(x,P); 9 | 10 | [C, Ceq] = NonLinCon(x,P); 11 | 12 | if ~isempty(C) 13 | error('snoptObjective cannot deal with inequality constraints.') 14 | end 15 | 16 | F = [cost;Ceq]; 17 | 18 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Chebyshev_Grid_Soln/wrapper_FMINCON.m: -------------------------------------------------------------------------------- 1 | function Results = wrapper_FMINCON(P,X,Xlow,Xupp) 2 | 3 | %Function Handles: 4 | objfunc = @(x)Objective_Function(x,P); 5 | nonlcon = @(x)NonLinCon(x,P); 6 | 7 | %No linear constraints: 8 | A=[]; Aeq=[]; B=[]; Beq=[]; 9 | 10 | %Store things in a Problem struct: 11 | Problem.objective = objfunc; 12 | Problem.x0 = convertVecStruct(X,P.structDat); 13 | Problem.Aineq = A; 14 | Problem.bineq = B; 15 | Problem.Aeq = Aeq; 16 | Problem.beq = Beq; 17 | Problem.lb = convertVecStruct(Xlow,P.structDat); 18 | Problem.ub = convertVecStruct(Xupp,P.structDat); 19 | Problem.nonlcon = nonlcon; 20 | Problem.options = P.options; 21 | Problem.solver = 'fmincon'; 22 | 23 | %Run fmincon: 24 | tic 25 | [xSoln, Fval, ExitFlag, Output] = fmincon(Problem); 26 | cpuTime = toc; 27 | 28 | %Get the solution struct: 29 | Xsoln = convertVecStruct(xSoln,P.structDat); 30 | 31 | %Store the solver output: 32 | name = 'fmincon'; 33 | input = makeStruct(name, Problem); 34 | output = makeStruct(xSoln, Fval, ExitFlag, Output, cpuTime); 35 | Results = makeStruct(P,input,output,Xsoln); 36 | 37 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Chebyshev_Grid_Soln/wrapper_SNOPT.m: -------------------------------------------------------------------------------- 1 | function Results = wrapper_SNOPT(P,X,Xlow,Xupp) 2 | 3 | global PARAMETERS; 4 | 5 | %WRITE to global variable 6 | PARAMETERS = P; 7 | 8 | x = convertVecStruct(X,P.structDat); 9 | xlow = convertVecStruct(Xlow,P.structDat); 10 | xupp = convertVecStruct(Xupp,P.structDat); 11 | 12 | F = snoptObjective(x); 13 | 14 | %For now, we only have equality constraints 15 | Flow = zeros(size(F)); 16 | Fupp = Flow; 17 | 18 | %The first element is the cost, which is unbounded: 19 | Flow(1) = -inf; 20 | Fupp(end) = inf; 21 | 22 | %Set accuracy for SNOPT 23 | snsetr('Major feasibility tolerance',P.options.TolFun) 24 | snsetr('Major optimality tolerance',P.options.TolFun) 25 | 26 | userfun = 'snoptObjective'; 27 | 28 | %Run snopt: 29 | tic 30 | [xSoln,F,inform,xmul,Fmul] = snopt(x,xlow,xupp,Flow,Fupp,userfun); 31 | cpuTime = toc; 32 | 33 | %Get the solution struct: 34 | Xsoln = convertVecStruct(xSoln,P.structDat); 35 | 36 | %Store the solver output: 37 | name = 'snopt'; 38 | Fval = F(1); 39 | input = makeStruct(name,xSoln,xlow,xupp,Flow,Fupp,userfun); 40 | output = makeStruct(x,F,Fval,inform,xmul,Fmul, cpuTime); 41 | Results = makeStruct(P,input,output,Xsoln); 42 | 43 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/GPOPS_2_Soln/.gitignore: -------------------------------------------------------------------------------- 1 | #GPOPS Log Files: 2 | *.OPTinfo.txt 3 | 4 | # GPOPS auto derivative files 5 | *ADiGatorGrd.* 6 | 7 | # matlab autosave 8 | *.m~ 9 | -------------------------------------------------------------------------------- /PendulumCart_SwingUp/GPOPS_2_Soln/oldSoln.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MatthewPeterKelly/dscTutorials/e1e97a9be03ec146f88bd6ddd9e06db7ee52e242/PendulumCart_SwingUp/GPOPS_2_Soln/oldSoln.mat -------------------------------------------------------------------------------- /PendulumCart_SwingUp/GPOPS_2_Soln/pendulumCart_Continuous.m: -------------------------------------------------------------------------------- 1 | function output = pendulumCart_Continuous(input) 2 | 3 | %Dynamical state 4 | X = input.phase(1).state; 5 | 6 | %Control force 7 | F = input.phase(1).control; 8 | 9 | %Define Parameters 10 | P = input.auxdata; 11 | 12 | %Call continuous dynamics function: 13 | Xd = Pendulum_Cart_Dynamics_Forced(X',F',P)'; 14 | 15 | %Get the cost integrand: (force-squared for now) 16 | costIntegrand = F.^2; 17 | 18 | %Pack up the output: 19 | output.dynamics = Xd; 20 | output.integrand = costIntegrand; 21 | 22 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/GPOPS_2_Soln/pendulumCart_Endpoint.m: -------------------------------------------------------------------------------- 1 | function output = pendulumCart_Endpoint(input) 2 | 3 | %The cost function is the integral of force.^2 4 | output.objective = input.phase(1).integral; 5 | 6 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Piecewise_Linear_Soln/NonLinCon.m: -------------------------------------------------------------------------------- 1 | function [C, Ceq] = NonLinCon(x,P) 2 | 3 | %This function is used to compute the nonlinear constraints for the 4 | %pendulum cart swing-up problem. In this case, all constraints are defect 5 | %constraints on the state. 6 | 7 | %Expand the decision variable vector to a struct 8 | X = convertVecStruct(x,P.structDat); 9 | 10 | %Get the full state matrix (but the starting and ending states back on) 11 | States = [P.MS.Start, X.state, P.MS.Finish]; 12 | 13 | %Set up for integration using RK4, include cost in integral 14 | Ylow = States(:,1:(end-1)); 15 | Yupp = States(:,2:end); 16 | 17 | Ulow = X.force(1:(end-1)); 18 | Uupp = X.force(2:end); 19 | Umid = 0.5*(Ulow + Uupp); 20 | 21 | 22 | userFunc = P.Dyn.dynamics_func; %ARG(state,force); 23 | h = X.duration/(P.MS.nGrid-1); 24 | 25 | %Runge-Kutta 4th order 26 | k1 = feval(userFunc,Ylow,Ulow); 27 | k2 = feval(userFunc,Ylow + 0.5*h*k1,Umid); 28 | k3 = feval(userFunc,Ylow + 0.5*h*k2,Umid); 29 | k4 = feval(userFunc,Ylow + h*k3,Uupp); 30 | Ynew = Ylow + (h/6)*(k1 + 2*k2 + 2*k3 + k4); 31 | 32 | %Compute the defects: 33 | defects = Ynew-Yupp; 34 | 35 | %Format things nicely for returning to fmincon: 36 | [n,m] = size(defects); 37 | Ceq = reshape(defects,n*m,1); 38 | C = []; 39 | 40 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Piecewise_Linear_Soln/Objective_Function.m: -------------------------------------------------------------------------------- 1 | function cost = Objective_Function(x,P) 2 | 3 | %Special case - no cost function 4 | if P.MS.cost_function==0 5 | cost = 0; 6 | return 7 | end 8 | 9 | %Expand the decision variable vector to a struct 10 | X = convertVecStruct(x,P.structDat); 11 | 12 | switch P.MS.cost_function 13 | case 1 %force squared 14 | 15 | uLow = X.force(1:(end-1)); 16 | uUpp = X.force(2:end); 17 | uMid = 0.5*(uLow + uUpp); 18 | 19 | cLow = uLow.^2; 20 | cMid = uMid.^2; 21 | cUpp = uUpp.^2; 22 | 23 | %Time step 24 | h = X.duration/(P.MS.nGrid-1); 25 | 26 | %Use simpson's rule to approximate cost: 27 | cost = (h/6)*sum(cLow + 4*cMid + cUpp); 28 | 29 | otherwise 30 | error('Invalid cost function type') 31 | 32 | end 33 | 34 | end 35 | -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Piecewise_Linear_Soln/Set_Bounds.m: -------------------------------------------------------------------------------- 1 | function [Xlow, Xupp] = Set_Bounds(P) 2 | 3 | Ns = P.MS.nGrid - 2; %We already know the initial and final states! 4 | LOW = 1; 5 | UPP = 2; 6 | 7 | %% Lower Bounds: 8 | Xlow.duration = P.Bnd.duration(LOW); 9 | Xlow.state = P.Bnd.state(:,LOW)*ones(1,Ns); 10 | Xlow.force = P.Bnd.force(LOW)*ones(1,P.MS.nGrid); 11 | 12 | %% Upper Bounds: 13 | Xupp.duration = P.Bnd.duration(UPP); 14 | Xupp.state = P.Bnd.state(:,UPP)*ones(1,Ns); 15 | Xupp.force = P.Bnd.force(UPP)*ones(1,P.MS.nGrid); 16 | 17 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Piecewise_Linear_Soln/animatePendulumCart.m: -------------------------------------------------------------------------------- 1 | %Unpack the data 2 | pos = State(1,:); 3 | angle = State(3,:); 4 | 5 | %Position of the cart over time: 6 | Cart = [pos;zeros(size(pos))]; 7 | 8 | %Position of the pendulum bob over time: (Assume that length==1) 9 | Bob = Cart + [-sin(angle);cos(angle)]; 10 | 11 | %Pack up for plotting: 12 | x = [Cart;Bob]; 13 | t = Time; 14 | 15 | %Figure out the extents of the axis 16 | horizontalAll = [Cart(1,:), Bob(1,:)]; 17 | verticalAll = [Cart(2,:), Bob(2,:)]; 18 | param.axis = [min(horizontalAll),max(horizontalAll),... 19 | min(verticalAll),max(verticalAll)]; 20 | param.clearFigure = true; 21 | 22 | %Pass the trace of the Bob's path to the plotting function. 23 | param.Bob = Bob; 24 | 25 | %Set up parameters for animation: 26 | P.plotFunc = @(t,x)plotPendulumCart(t,x,param); 27 | P.speed = 0.25; 28 | P.figNum = gcf; 29 | 30 | %Call the animation function 31 | Animate(t,x,P) -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Piecewise_Linear_Soln/makeStruct.m: -------------------------------------------------------------------------------- 1 | function StructOutput = makeStruct(varargin) 2 | 3 | %A struct is created with the property that each field corresponds to one 4 | %of the arguments passed to this function. 5 | % 6 | % Example: 7 | % 8 | % R = makeStruct(a,b,c,d,e); 9 | % 10 | % R.a == a; 11 | % R.b == b; 12 | % R.c == c; 13 | % R.d == d; 14 | % R.e == e; 15 | % 16 | % 17 | 18 | N_Inputs = length(varargin); 19 | 20 | for i=1:N_Inputs 21 | name = inputname(i); 22 | StructOutput.(name) = varargin{i}; 23 | end 24 | 25 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Piecewise_Linear_Soln/oldSoln.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MatthewPeterKelly/dscTutorials/e1e97a9be03ec146f88bd6ddd9e06db7ee52e242/PendulumCart_SwingUp/Piecewise_Linear_Soln/oldSoln.mat -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Piecewise_Linear_Soln/snoptObjective.m: -------------------------------------------------------------------------------- 1 | function F = snoptObjective(x) 2 | 3 | global PARAMETERS; 4 | 5 | %READ from global variable 6 | P = PARAMETERS; 7 | 8 | cost = Objective_Function(x,P); 9 | 10 | [C, Ceq] = NonLinCon(x,P); 11 | 12 | if ~isempty(C) 13 | error('snoptObjective cannot deal with inequality constraints.') 14 | end 15 | 16 | F = [cost;Ceq]; 17 | 18 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Piecewise_Linear_Soln/stopActionPendulumCart.m: -------------------------------------------------------------------------------- 1 | %Unpack the data 2 | pos = State(1,:); 3 | angle = State(3,:); 4 | 5 | %Position of the cart over time: 6 | Cart = [pos;zeros(size(pos))]; 7 | 8 | %Position of the pendulum bob over time: (Assume that length==1) 9 | Bob = Cart + [-sin(angle);cos(angle)]; 10 | 11 | %Pack up for plotting: 12 | x = [Cart;Bob]; 13 | t = Time; 14 | 15 | %Figure out the extents of the axis 16 | horizontalAll = [Cart(1,:), Bob(1,:)]; 17 | verticalAll = [Cart(2,:), Bob(2,:)]; 18 | param.axis = [min(horizontalAll),max(horizontalAll),... 19 | min(verticalAll),max(verticalAll)]; 20 | 21 | %Don't clear the figure each time 22 | param.clearFigure = false; 23 | 24 | %Pass the trace of the Bob's path to the plotting function. 25 | nPts = 500; 26 | traceTime = linspace(t(1),t(end),nPts); 27 | traceState = interp1(t',Bob',traceTime','pchip','extrap')'; 28 | param.Bob = traceState; 29 | 30 | 31 | %Plotting: 32 | clf; 33 | nFrames = 20; 34 | frameTime = linspace(t(1),t(end),nFrames); 35 | frameState = interp1(t',x',frameTime','pchip','extrap')'; 36 | for i=1:nFrames 37 | plotPendulumCart(frameTime(i),frameState(:,i),param); 38 | end 39 | 40 | 41 | -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Piecewise_Linear_Soln/wrapper_FMINCON.m: -------------------------------------------------------------------------------- 1 | function Results = wrapper_FMINCON(P,X,Xlow,Xupp) 2 | 3 | %Function Handles: 4 | objfunc = @(x)Objective_Function(x,P); 5 | nonlcon = @(x)NonLinCon(x,P); 6 | 7 | %No linear constraints: 8 | A=[]; Aeq=[]; B=[]; Beq=[]; 9 | 10 | %Store things in a Problem struct: 11 | Problem.objective = objfunc; 12 | Problem.x0 = convertVecStruct(X,P.structDat); 13 | Problem.Aineq = A; 14 | Problem.bineq = B; 15 | Problem.Aeq = Aeq; 16 | Problem.beq = Beq; 17 | Problem.lb = convertVecStruct(Xlow,P.structDat); 18 | Problem.ub = convertVecStruct(Xupp,P.structDat); 19 | Problem.nonlcon = nonlcon; 20 | Problem.options = P.options; 21 | Problem.solver = 'fmincon'; 22 | 23 | %Run fmincon: 24 | tic 25 | [xSoln, Fval, ExitFlag, Output] = fmincon(Problem); 26 | cpuTime = toc; 27 | 28 | %Get the solution struct: 29 | Xsoln = convertVecStruct(xSoln,P.structDat); 30 | 31 | %Store the solver output: 32 | name = 'fmincon'; 33 | input = makeStruct(name, Problem); 34 | output = makeStruct(xSoln, Fval, ExitFlag, Output, cpuTime); 35 | Results = makeStruct(P,input,output,Xsoln); 36 | 37 | end -------------------------------------------------------------------------------- /PendulumCart_SwingUp/Piecewise_Linear_Soln/wrapper_SNOPT.m: -------------------------------------------------------------------------------- 1 | function Results = wrapper_SNOPT(P,X,Xlow,Xupp) 2 | 3 | global PARAMETERS; 4 | 5 | %WRITE to global variable 6 | PARAMETERS = P; 7 | 8 | x = convertVecStruct(X,P.structDat); 9 | xlow = convertVecStruct(Xlow,P.structDat); 10 | xupp = convertVecStruct(Xupp,P.structDat); 11 | 12 | F = snoptObjective(x); 13 | 14 | %For now, we only have equality constraints 15 | Flow = zeros(size(F)); 16 | Fupp = Flow; 17 | 18 | %The first element is the cost, which is unbounded: 19 | Flow(1) = -inf; 20 | Fupp(end) = inf; 21 | 22 | %Set accuracy for SNOPT 23 | snsetr('Major feasibility tolerance',P.options.TolFun) 24 | snsetr('Major optimality tolerance',P.options.TolFun) 25 | 26 | userfun = 'snoptObjective'; 27 | 28 | %Run snopt: 29 | tic 30 | [xSoln,F,inform,xmul,Fmul] = snopt(x,xlow,xupp,Flow,Fupp,userfun); 31 | cpuTime = toc; 32 | 33 | %Get the solution struct: 34 | Xsoln = convertVecStruct(xSoln,P.structDat); 35 | 36 | %Store the solver output: 37 | name = 'snopt'; 38 | Fval = F(1); 39 | input = makeStruct(name,xSoln,xlow,xupp,Flow,Fupp,userfun); 40 | output = makeStruct(x,F,Fval,inform,xmul,Fmul, cpuTime); 41 | Results = makeStruct(P,input,output,Xsoln); 42 | 43 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | dscTutorials 2 | ============ 3 | 4 | A few simple tutorials for dynamical systems and control. Most require Matlab. 5 | -------------------------------------------------------------------------------- /RobotArmTrajctory/README.md: -------------------------------------------------------------------------------- 1 | # Robot Arm Trajectory 2 | 3 | This directory contains Matlab code to pose and then solve for a minimum-jerk trajectory for a robot arm that satisfies either either boundary-values or node constraints, subject to joint angle and rate constraints. 4 | 5 | The problem is posed and solved as a quadratic program, using multi-segment Chebyshev orthogonal collocation for transcription. 6 | 7 | -------------------------------------------------------------------------------- /RobotArmTrajctory/getDefaultPlotColors.m: -------------------------------------------------------------------------------- 1 | function Color = getDefaultPlotColors() 2 | 3 | n = 7; %Number of unique colors in matlab's default line color pallet 4 | 5 | Color = zeros(n,3); 6 | hFig = figure('visible','off'); 7 | hold on; 8 | 9 | for i=1:n 10 | hLine = plot(rand(1,2),rand(1,2)); 11 | Color(i,:) = get(hLine,'Color'); 12 | end 13 | 14 | close(hFig); 15 | 16 | end -------------------------------------------------------------------------------- /RobotArmTrajctory/structBlkDiag.m: -------------------------------------------------------------------------------- 1 | function F = structBlkDiag(S,field) 2 | % F = structBlkDiag(S,field) 3 | % 4 | % This function creates a block diagional matrix from a field of a struct 5 | % array 6 | % 7 | 8 | n = length(S); 9 | cmd = ['F' ' = blkdiag(']; 10 | for i=1:n 11 | cmd = [cmd 'S(' num2str(i) ').' field]; %#ok 12 | if i < n 13 | cmd = [cmd ', ']; %#ok 14 | else 15 | cmd = [cmd ');'];%#ok 16 | end 17 | end 18 | 19 | eval(cmd); 20 | 21 | end -------------------------------------------------------------------------------- /ThreeMassPeriodicOrbits/bvp4c/boundaryConditions.m: -------------------------------------------------------------------------------- 1 | function C = boundaryConditions(X0, XF) 2 | % C = boundaryConditions(X0, XF) 3 | % 4 | % This function computes the boundary conditions for periodic solutions. 5 | % 6 | % INPUTS: 7 | % X0 = Initial State 8 | % XF = Final State 9 | % 10 | % OUTPUTS: 11 | % C = defect constraint 12 | % 13 | 14 | C = X0 - XF; 15 | 16 | end 17 | -------------------------------------------------------------------------------- /ThreeMassPeriodicOrbits/rk4ms/boundaryConditions.m: -------------------------------------------------------------------------------- 1 | function C = boundaryConditions(X0, XF) 2 | % C = boundaryConditions(X0, XF) 3 | % 4 | % This function computes the boundary conditions for periodic solutions. 5 | % 6 | % INPUTS: 7 | % X0 = Initial State 8 | % XF = Final State 9 | % 10 | % OUTPUTS: 11 | % C = defect constraint 12 | % 13 | 14 | C = X0 - XF; 15 | 16 | end 17 | -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_1_Cannon/cannonDynamics.m: -------------------------------------------------------------------------------- 1 | function dz = cannonDynamics(~,z,c) 2 | % function dz = cannonDynamics(~,z,c) 3 | % 4 | % This function computes the dynamics of a cannon ball travelling through 5 | % the air with quadratic drag 6 | % 7 | % INPUTS: 8 | % ~ = [1 x N] time vector (not used) 9 | % z = [4 x N] state vectory 10 | % z(1,:) = x = horizontal position 11 | % z(2,:) = y = vertical position 12 | % z(3,:) = dx = horizontal speed 13 | % z(4,:) = dy = vertical speed 14 | % c = quadratic drag coefficient 15 | % 16 | % OUTPUTS: 17 | % dz = d(x)/dt = time-derivative of the state 18 | % 19 | % NOTES: 20 | % - Assume unit gravity and mass 21 | % - Assume that cannon ball is a point mass (no rotational inertia) 22 | % 23 | 24 | dz = zeros(size(z)); 25 | 26 | %First-order form (derivative of position states is velocity states) 27 | dz(1:2,:) = z(3:4,:); 28 | 29 | % Compute the speed (used for drag force calculation) 30 | dx = z(3,:); 31 | dy = z(4,:); 32 | v = sqrt(dx.*dx + dy.*dy); 33 | 34 | % Compute the force vectors 35 | fx = -c*dx.*v; 36 | fy = -c*dy.*v - 1; %Assume unit gravity 37 | 38 | % Record the accelerations (derivative of velocity states) 39 | dz(3,:) = fx; %Assume unit point-mass 40 | dz(4,:) = fy; %Assume unit point-mass 41 | 42 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_1_Cannon/cannon_directCollocation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MatthewPeterKelly/dscTutorials/e1e97a9be03ec146f88bd6ddd9e06db7ee52e242/TrajectoryOptimization/Example_1_Cannon/cannon_directCollocation.gif -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_1_Cannon/cannon_multipleShooting.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MatthewPeterKelly/dscTutorials/e1e97a9be03ec146f88bd6ddd9e06db7ee52e242/TrajectoryOptimization/Example_1_Cannon/cannon_multipleShooting.gif -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_1_Cannon/cannon_singleShooting.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MatthewPeterKelly/dscTutorials/e1e97a9be03ec146f88bd6ddd9e06db7ee52e242/TrajectoryOptimization/Example_1_Cannon/cannon_singleShooting.gif -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_1_Cannon/drawTree.m: -------------------------------------------------------------------------------- 1 | function drawTree(x,y,h) 2 | % function drawTree(x,y,h) 3 | % 4 | % This function is used to draw a pine tree, with trunk at position (x,y), 5 | % with the desired height (h). It is used for giving a scale to a figure. 6 | % 7 | 8 | xGreen = x + h*[0.0, 0.2, 0.1, 0.3, 0.16, 0.4, -0.4, -0.16, -0.3, -0.1, -0.2, 0.0]; 9 | yGreen = y + h*[1.0, 0.75, 0.75, 0.50, 0.50, 0.2, 0.2, 0.50, 0.50, 0.75, 0.75, 1.0]; 10 | 11 | xBrown = x + h*0.08*[1,1,-1,-1]; 12 | yBrown = y + h*0.2*[0,1,1,0]; 13 | 14 | patch(xGreen, yGreen, [0.1,0.6,0.1]); 15 | patch(xBrown, yBrown, [0.4, 0.2, 0.1]); 16 | 17 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_1_Cannon/groundEvent.m: -------------------------------------------------------------------------------- 1 | function [value, isterminal, direction] = groundEvent(~,z) 2 | % 3 | % This function is called by ode45 event-detection to determine when a 4 | % collision with the ground occurs. 5 | % 6 | 7 | height = z(2,:); %Vertical position of the cannon ball 8 | 9 | value = height; 10 | isterminal = true; 11 | direction = -1; 12 | 13 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_1_Cannon/objective.m: -------------------------------------------------------------------------------- 1 | function cost = objective(dx,dy) 2 | % Objective function for optimization. Here we set the objective function 3 | % to be proportional to the initial energy of the cannon ball. Note that 4 | 5 | cost = dx.*dx + dy.*dy; 6 | 7 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_1_Cannon/plotBackground.m: -------------------------------------------------------------------------------- 1 | function plotAxis = plotBackground(target,param) 2 | 3 | hold on; 4 | 5 | %%% Figure out the axis for the figure: 6 | pixelDim = param.diagnostics.gifPixelDim; 7 | axisRatio = pixelDim(2)/pixelDim(1); 8 | xBnd = [-0.2*target.x, 1.2*target.x]; 9 | yBnd = xBnd*axisRatio; 10 | plotAxis = [xBnd,yBnd]; 11 | 12 | %%% Set the color for the background and size of figure 13 | set(gca,'Color',0.7*[1 1 1]); %Grey 14 | position = get(gcf,'Position'); 15 | position(3:4) = pixelDim; 16 | set(gcf,'Position',position); 17 | 18 | %%% Plot the ground 19 | xGround = [-0.2*target.x, 1.2*target.x]; 20 | yGround = [0, 0]; 21 | brown = [0.5, 0.2, 0.1]; %Color for ground 22 | plot(xGround, yGround,'color',brown,'LineWidth',6); 23 | 24 | %%% Plot the tree, start position, and target position 25 | drawTree(0.6*target.x, 0, 1); %Plot a tree for scale 26 | plot(0,0,'k.','MarkerSize',35); %Start 27 | plot(target.x,target.y,'ko','LineWidth',4,'MarkerSize',14) %Finish 28 | 29 | %%% Axis Stuff 30 | xlabel('Horizontal Position') 31 | ylabel('Vertical Position') 32 | axis equal; axis(plotAxis); drawnow; hold off; 33 | 34 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_1_Cannon/plotSoln.m: -------------------------------------------------------------------------------- 1 | function plotSoln(traj, target, param) 2 | % Plots a cannon ball trajectory (x,y), with speed trajectory (dx,dy) 3 | % 4 | % traj.x = [1 x n] horizontal position vector 5 | % traj.y = [1 x n] vertical position vector 6 | % traj.dx = [1 x n] horizontal speed vector 7 | % traj.dy = [1 x n] vertical speed vector 8 | % 9 | % target.x = horizontal target 10 | % target.y = vertical target 11 | % 12 | 13 | plotBackground(target,param); hold on; 14 | 15 | color = 'k'; 16 | message = ''; 17 | if isfield(traj,'success') 18 | if ~traj.success 19 | color = 'r'; 20 | message = 'FAILED'; 21 | end 22 | end 23 | 24 | % Plot the trajectory 25 | x = traj.x; y = traj.y; 26 | dx = traj.dx; dy = traj.dy; 27 | plot(x,y,'LineWidth',3,'Color',color); 28 | 29 | %%% Add labels and title 30 | objVal = objective(dx(1),dy(1)); 31 | title(sprintf('Trajectory via %s (initial speed = %6.3f) %s',traj.method ,sqrt(objVal),message)); 32 | xlabel('Y Position') 33 | ylabel('X Position') 34 | hold off; 35 | 36 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_1_Cannon/rainbow.m: -------------------------------------------------------------------------------- 1 | function map = rainbow(n) 2 | % map = RAINBOW(n) 3 | % 4 | % This function returns a colormap that ranges from red to blue 5 | % 6 | % INPUTS: 7 | % n = number of rows in colormap 8 | % 9 | % OUTPUTS: 10 | % map = [n x 3] color map 11 | % 12 | % See also HSV2RGB, JET, HSV, HOT, PINK, FLAG, COLORMAP, RGBPLOT. 13 | 14 | hue = linspace(0,2/3,n)'; 15 | sat = 0.95*ones(n,1); 16 | val= 0.95*ones(n,1); 17 | map = hsv2rgb([hue,sat,val]); 18 | 19 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_1_Cannon/simulateCannon.m: -------------------------------------------------------------------------------- 1 | function traj = simulateCannon(init,param) 2 | 3 | v0 = init.speed; 4 | th0 = init.angle; 5 | 6 | c = param.c; %Quadratic drag coefficient 7 | nGrid = param.nGrid; 8 | 9 | % Set up initial conditions for ode45 10 | x0 = 0; y0 = 0; %Start at the origin 11 | dx0 = v0*cos(th0); 12 | dy0 = v0*sin(th0); 13 | if dy0 < 0, error('Cannot point cannon through ground! sin(th0) > 0 required.'); end; 14 | 15 | % Set up arguments for ode45 16 | userFun = @(t,z)cannonDynamics(t,z,c); %Dynamics function 17 | tSpan = [0,100]; %Never plan on reaching final time 18 | z0 = [x0;y0;dx0;dy0]; %Initial condition 19 | options = odeset('Events',@groundEvent,'Vectorized','on'); 20 | 21 | % Run a simulation 22 | sol = ode45(userFun, tSpan, z0, options); 23 | 24 | % Extract the solution on uniform grid: 25 | traj.t = linspace(sol.x(1), sol.x(end), nGrid); 26 | z = deval(sol,traj.t); 27 | traj.x = z(1,:); 28 | traj.y = z(2,:); 29 | traj.dx = z(3,:); 30 | traj.dy = z(4,:); 31 | 32 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_2_CartPole/autoGen_cartPoleDynamics.m: -------------------------------------------------------------------------------- 1 | function [ddx,ddq] = autoGen_cartPoleDynamics(q,dq,F,T,m1,m2,g,l) 2 | %AUTOGEN_CARTPOLEDYNAMICS 3 | % [DDX,DDQ] = AUTOGEN_CARTPOLEDYNAMICS(Q,DQ,F,T,M1,M2,G,L) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.2. 6 | % 14-Jun-2015 09:12:08 7 | 8 | t2 = cos(q); 9 | t3 = sin(q); 10 | t4 = t2.^2; 11 | t5 = m1+m2-m2.*t4; 12 | t6 = 1.0./t5; 13 | t7 = dq.^2; 14 | t8 = l.^2; 15 | ddx = (t6.*(F.*l-T.*t2+m2.*t3.*t7.*t8+g.*l.*m2.*t2.*t3))./l; 16 | if nargout > 1 17 | t9 = m2.^2; 18 | ddq = -(1.0./l.^2.*t6.*(-T.*m1-T.*m2+g.*l.*t3.*t9+F.*l.*m2.*t2+g.*l.*m1.*m2.*t3+t2.*t3.*t7.*t8.*t9))./m2; 19 | end 20 | -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_2_CartPole/autoGen_cartPoleInvDyn.m: -------------------------------------------------------------------------------- 1 | function [F,T] = autoGen_cartPoleInvDyn(q,dq,ddq,ddx,m1,m2,g,l) 2 | %AUTOGEN_CARTPOLEINVDYN 3 | % [F,T] = AUTOGEN_CARTPOLEINVDYN(Q,DQ,DDQ,DDX,M1,M2,G,L) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.2. 6 | % 14-Jun-2015 09:12:09 7 | 8 | t2 = cos(q); 9 | t3 = sin(q); 10 | F = ddx.*m1+ddx.*m2+ddq.*l.*m2.*t2-dq.^2.*l.*m2.*t3; 11 | if nargout > 1 12 | T = l.*m2.*(ddq.*l+ddx.*t2+g.*t3); 13 | end 14 | -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_2_CartPole/autoGen_cartPoleKinematics.m: -------------------------------------------------------------------------------- 1 | function [p,dp] = autoGen_cartPoleKinematics(x,q,dx,dq,l) 2 | %AUTOGEN_CARTPOLEKINEMATICS 3 | % [P,DP] = AUTOGEN_CARTPOLEKINEMATICS(X,Q,DX,DQ,L) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.2. 6 | % 14-Jun-2015 09:12:09 7 | 8 | t2 = cos(q); 9 | t3 = sin(q); 10 | p = [x+l.*t3;-l.*t2]; 11 | if nargout > 1 12 | dp = [dx+dq.*l.*t2;dq.*l.*t3]; 13 | end 14 | -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_2_CartPole/cartPoleDynamics.m: -------------------------------------------------------------------------------- 1 | function dz = cartPoleDynamics(z,u,p) 2 | % dz = cartPoleDynamics(z,u,p) 3 | % 4 | % This function computes the dynamics for a simple cart-pole: a point-mass 5 | % cart that travels on a friction-less horizontal track. A pendulum hangs 6 | % from the cart. There are two actuators: a horizontal force that pushes on 7 | % the cart, and a torque acting on the base of the pendulum. 8 | % 9 | % INPUTS: 10 | % z = [4, n] = state vector = [x;q;dx;dq] 11 | % u = [1, n] = actuation vector = F = force on cart 12 | % p = struct of parameters: 13 | % .m1 = cart mass 14 | % .m2 = pendulum point-mass 15 | % .g = gravity 16 | % .l = length of the pendulum 17 | % 18 | % OUTPUTS: 19 | % dz = [4, n] = derivative of the state vector 20 | % 21 | 22 | % x = z(1,:); 23 | q = z(2,:); 24 | dx = z(3,:); 25 | dq = z(4,:); 26 | 27 | F = u(1,:); 28 | T = zeros(1,length(F)); 29 | 30 | [ddx,ddq] = autoGen_cartPoleDynamics(q,dq,F,T,p.m1,p.m2,p.g,p.l); 31 | 32 | dz = [dx;dq;ddx;ddq]; 33 | 34 | end 35 | 36 | 37 | -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_2_CartPole/chebyshevScalePoints.m: -------------------------------------------------------------------------------- 1 | function [x,w] = chebyshevScalePoints(xx,ww,d) 2 | % [x,w] = chebyshevScalePoints(xx,ww,d) 3 | % 4 | % This function scales the chebyshev points to an arbitrary interval 5 | % 6 | % INPUTS: 7 | % xx = chebyshev points on the domain [-1,1] 8 | % ww = chebysehv weights on the domain [-1,1] 9 | % d = [low, upp] = new domain 10 | % 11 | % OUTPUTS: 12 | % x = chebyshev points on the new domain d 13 | % w = chebyshev weights on the new domain d 14 | % 15 | 16 | x = ((d(2)-d(1))*xx + sum(d))/2; 17 | w = ww*(d(2)-d(1))/2; 18 | 19 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_2_CartPole/computeGuess.m: -------------------------------------------------------------------------------- 1 | function guess = computeGuess(config) 2 | % guess = computeGuess(config) 3 | % 4 | % This function computes an initial guess at the trajectory for swinging up 5 | % the pendulum. 6 | 7 | % Unpack physical parameters 8 | m1 = config.dyn.m1; 9 | m2 = config.dyn.m2; 10 | g = config.dyn.g; 11 | l = config.dyn.l; 12 | 13 | n = config.grid.nTrajPts; %Number of grid points in guess 14 | 15 | tEnd = 2*pi*sqrt(l/g); %Period of a simple pendulum 16 | t = linspace(0,tEnd,n); 17 | 18 | % Assume that the cart follows a sinusoidal trajectory, conserving CoM 19 | xAmp = l*m2/(m1+m2); %CoM calculations... 20 | x = xAmp*sin(pi*t/tEnd); 21 | dx = xAmp*(2*pi/tEnd)*cos(2*pi*t/tEnd); 22 | ddx = -xAmp*(2*pi/tEnd)^2*sin(2*pi*t/tEnd); 23 | 24 | % Assume that the pendulum goes from bottom to top in one simple motion 25 | q = -sign(xAmp)*pi*t/tEnd; 26 | dq = -sign(xAmp)*pi*ones(size(t))/tEnd; 27 | ddq = zeros(size(t)); 28 | 29 | % Compute the inverse dynamics to produce this trajectory: 30 | F = autoGen_cartPoleInvDyn(q,dq,ddq,ddx,m1,m2,g,l); 31 | 32 | % Store the result for return: 33 | guess.time = t; 34 | guess.state = [x;q;dx;dq]; 35 | guess.control = F; 36 | 37 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_2_CartPole/costFunction.m: -------------------------------------------------------------------------------- 1 | function cost = costFunction(t,x,u) 2 | 3 | cost = u.^2; 4 | % cost = du.^2; 5 | % cost = du.^2 + u.^2; 6 | % cost = dz(1,:).^2 + dz(2,:).^2 + ddz(1,:).^2 + ddz(2,:).^2 +u.^2 + du.^2; 7 | 8 | 9 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_2_CartPole/drawCartPole.m: -------------------------------------------------------------------------------- 1 | function drawCartPole(t,z,dyn) 2 | %drawCartPole(t,z,dyn) 3 | % 4 | % This function draws the cart pole's current state 5 | 6 | 7 | cartWidth = 0.3*dyn.l; 8 | cartHeight = 0.2*dyn.l; 9 | 10 | extents = [-3.5,1,-cartHeight - 1.2*dyn.l, cartHeight + 1.2*dyn.l]; 11 | 12 | cartColor = [33,87,177]/256; 13 | poleColor = [183,27,73]/256; 14 | groundColor = [84,64,64]/256; 15 | 16 | x = z(1,:); 17 | q = z(2,:); 18 | dx = z(3,:); 19 | dq = z(4,:); 20 | 21 | p = autoGen_cartPoleKinematics(x,q,dx,dq,dyn.l); 22 | 23 | p1 = [x;0]; %Center of cart 24 | p2 = p; % Tip of pendulum 25 | 26 | clf; hold on; 27 | axis equal; axis(extents); 28 | 29 | % Draw the cart: 30 | h = rectangle(... 31 | 'Position',[(p1'-0.5*[cartWidth,cartHeight]),[cartWidth,cartHeight]],... 32 | 'Curvature',0.2*[1,1],... 33 | 'EdgeColor',cartColor,... 34 | 'LineWidth',1,... 35 | 'FaceColor',cartColor); 36 | 37 | plot(extents(1:2),-0.55*cartHeight*[1,1],'LineWidth',4,'Color', groundColor); 38 | 39 | pos = [p1,p2]; 40 | plot(pos(1,:),pos(2,:),'Color',poleColor,'LineWidth',3); 41 | plot(p2(1),p2(2),'.','Color',poleColor','MarkerSize',80); 42 | 43 | title(['Cart-Pole, t = ', num2str(t,2)]); 44 | 45 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_2_CartPole/getIndex.m: -------------------------------------------------------------------------------- 1 | function [tIdx,xIdx,uIdx] = getIndex(pack) 2 | 3 | % This function returns the indices to extract decision variables for use 4 | % in linear constraints to be passed to fmincon. 5 | 6 | nDecVar = 2 + pack.nState(1)*pack.nState(2) + ... 7 | pack.nControl(1)*pack.nControl(2); 8 | [tIdx,xIdx,uIdx] = unPackDecVar(1:nDecVar,pack); 9 | 10 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_2_CartPole/packDecVar.m: -------------------------------------------------------------------------------- 1 | function [z,dim] = packDecVar(tSpan,x,u) 2 | % [z,dim] = dimDecVar(t,x,u) 3 | % 4 | % This function collapses the time-span (t), state (x) 5 | % and control (u) matricies into a single vector 6 | % 7 | % INPUTS: 8 | % tSpan = [1, 2] = [t0, tF] = time span 9 | % x = [ns, ms] = state matrix = [dimension in state space, grid point] 10 | % u = [nc, mc] = control matrix = [dimension in control space, grid point] 11 | % 12 | % OUTPUTS: 13 | % z = [2+ns*ms+nc*nc, 1] = vector of decision variables 14 | % dim = struct with the size of the state and control matricies 15 | % .nState = size(x); 16 | % .nControl = size(u); 17 | % 18 | % See Also: UNPACKDECVAR 19 | 20 | dim.nState = size(x); 21 | dim.nControl = size(u); 22 | 23 | z = [tSpan(1);tSpan(2); reshape(x,numel(x),1); reshape(u,numel(u),1)]; 24 | 25 | end 26 | -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_2_CartPole/plotTraj.m: -------------------------------------------------------------------------------- 1 | function plotTraj(traj,config) 2 | % This function plots a trajectory 3 | 4 | markerSize = 25; 5 | lineWidth = 2; 6 | 7 | t = traj.time; 8 | tt = linspace(t(1),t(end),150); 9 | 10 | x = traj.state; 11 | xx = traj.interp.state(tt); 12 | 13 | u = traj.control; 14 | uu = traj.interp.control(tt); 15 | 16 | subplot(2,3,1); hold on; 17 | plot(t,x(1,:),'k.','MarkerSize',markerSize); 18 | plot(tt,xx(1,:),'b-','LineWidth',lineWidth); 19 | xlabel('t') 20 | ylabel('x') 21 | 22 | subplot(2,3,2); hold on; 23 | plot(t,x(2,:),'k.','MarkerSize',markerSize); 24 | plot(tt,xx(2,:),'b-','LineWidth',lineWidth); 25 | xlabel('t') 26 | ylabel('q') 27 | 28 | subplot(2,3,3); hold on; 29 | plot(t,u(1,:),'k.','MarkerSize',markerSize); 30 | plot(tt,uu(1,:),'b-','LineWidth',lineWidth); 31 | xlabel('t') 32 | ylabel('F') 33 | 34 | subplot(2,3,4); hold on; 35 | plot(t,x(3,:),'k.','MarkerSize',markerSize); 36 | plot(tt,xx(3,:),'b-','LineWidth',lineWidth); 37 | xlabel('t') 38 | ylabel('dx') 39 | 40 | subplot(2,3,5); hold on; 41 | plot(t,x(4,:),'k.','MarkerSize',markerSize); 42 | plot(tt,xx(4,:),'b-','LineWidth',lineWidth); 43 | xlabel('t') 44 | ylabel('dq') 45 | 46 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/Example_2_CartPole/unPackDecVar.m: -------------------------------------------------------------------------------- 1 | function [tSpan,x,u] = unPackDecVar(z,dim) 2 | % function [tSpan,x,u] = unPackDecVar(z,dim) 3 | % 4 | % This function unpacks the decision variables for 5 | % trajectory optimization into the duration (t), 6 | % state (x), and control (u) matricies 7 | % 8 | % INPUTS: 9 | % z = [2+ns*ms+nc*nc, 1] = vector of decision variables 10 | % dim = struct with the size of the state and control matricies 11 | % .nState = size(x); 12 | % .nControl = size(u); 13 | % 14 | % OUTPUTS: 15 | % tSpan = [1, 2] = [t0, tF] = time span 16 | % x = [ns, ms] = state matrix = [dimension in state space, grid point] 17 | % u = [nc, mc] = control matrix = [dimension in control space, grid point] 18 | % 19 | % See Also: PACKDECVAR 20 | 21 | ns = dim.nState; % ns = size(x) 22 | nc = dim.nControl; % nc = size(u) 23 | 24 | is = 1:(ns(1)*ns(2)); 25 | ic = 1:(nc(1)*nc(2)); 26 | 27 | tSpan = [z(1), z(2)]; 28 | x = reshape(z(2+is),ns(1),ns(2)); 29 | u = reshape(z(2+is(end)+ic), nc(1),nc(2)); 30 | 31 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/README.md: -------------------------------------------------------------------------------- 1 | # README.md -- Trajectory Optimization 2 | 3 | ## Overview: 4 | This directory contains a progression of tutorials that teach transcription methods for trajectory optimization. 5 | 6 | ## Examples: 7 | * Cannon Targeting 8 | - Single Shooting 9 | - Multiple Shooting 10 | - Collocation (Hermite-Simpson) 11 | -------------------------------------------------------------------------------- /TrajectoryOptimization/RiverCrossing/DirectCollocation_HermiteSimpson/riverBoatDynamics.m: -------------------------------------------------------------------------------- 1 | function dz = riverBoatDynamics(z,u,param) 2 | % dz = riverBoatDynamics(z,u) 3 | % 4 | % This function copmutes the dynamics of a boat as it crosses a river. The 5 | % water in the river is flowing with a parabolic speed profile. The boat 6 | % can travel in any direction at a constant speed with respect to the 7 | % water. 8 | % 9 | % INPUTS: 10 | % z = [2, n] = [x;y] = state matrix 11 | % u = [1, n] = control 12 | % 13 | % OUTPUTS: 14 | % dz = [2, n] = dz/dt = first time derivative of the state 15 | % 16 | % NOTES: 17 | % The river is flowing in the positive x direction. 18 | % 19 | % 20 | 21 | %%% Unpack the state 22 | % x = z(1,:); % dz does not depend on position along the river 23 | y = z(2,:); 24 | 25 | %%% River water speed (positive x direction) 26 | v = -param.maxWaterSpeed*y.*(y-param.riverWidth); 27 | 28 | %%% Dynamics 29 | dx = v + param.boatSpeed*cos(u); 30 | dy = param.boatSpeed*sin(u); 31 | 32 | %%% Pack up state: 33 | dz = [dx;dy]; 34 | 35 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/RiverCrossing/DirectCollocation_Trapazoid/riverBoatDynamics.m: -------------------------------------------------------------------------------- 1 | function dz = riverBoatDynamics(z,u,param) 2 | % dz = riverBoatDynamics(z,u) 3 | % 4 | % This function copmutes the dynamics of a boat as it crosses a river. The 5 | % water in the river is flowing with a parabolic speed profile. The boat 6 | % can travel in any direction at a constant speed with respect to the 7 | % water. 8 | % 9 | % INPUTS: 10 | % z = [2, n] = [x;y] = state matrix 11 | % u = [1, n] = control 12 | % 13 | % OUTPUTS: 14 | % dz = [2, n] = dz/dt = first time derivative of the state 15 | % 16 | % NOTES: 17 | % The river is flowing in the positive x direction. 18 | % 19 | % 20 | 21 | %%% Unpack the state 22 | % x = z(1,:); % dz does not depend on position along the river 23 | y = z(2,:); 24 | 25 | %%% River water speed (positive x direction) 26 | v = -param.maxWaterSpeed*y.*(y-param.riverWidth); 27 | 28 | %%% Dynamics 29 | dx = v + param.boatSpeed*cos(u); 30 | dy = param.boatSpeed*sin(u); 31 | 32 | %%% Pack up state: 33 | dz = [dx;dy]; 34 | 35 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/RiverCrossing/MultipleShooting_Euler/riverBoatDynamics.m: -------------------------------------------------------------------------------- 1 | function dz = riverBoatDynamics(z,u,param) 2 | % dz = riverBoatDynamics(z,u) 3 | % 4 | % This function copmutes the dynamics of a boat as it crosses a river. The 5 | % water in the river is flowing with a parabolic speed profile. The boat 6 | % can travel in any direction at a constant speed with respect to the 7 | % water. 8 | % 9 | % INPUTS: 10 | % z = [2, n] = [x;y] = state matrix 11 | % u = [1, n] = control 12 | % 13 | % OUTPUTS: 14 | % dz = [2, n] = dz/dt = first time derivative of the state 15 | % 16 | % NOTES: 17 | % The river is flowing in the positive x direction. 18 | % 19 | % 20 | 21 | %%% Unpack the state 22 | % x = z(1,:); % dz does not depend on position along the river 23 | y = z(2,:); 24 | 25 | %%% River water speed (positive x direction) 26 | v = -param.maxWaterSpeed*y.*(y-param.riverWidth); 27 | 28 | %%% Dynamics 29 | dx = v + param.boatSpeed*cos(u); 30 | dy = param.boatSpeed*sin(u); 31 | 32 | %%% Pack up state: 33 | dz = [dx;dy]; 34 | 35 | end -------------------------------------------------------------------------------- /TrajectoryOptimization/RiverCrossing/README.md: -------------------------------------------------------------------------------- 1 | # README.md 2 | 3 | ## Problem Statement: 4 | A boat needs to cross a river between two docks. The water is flowing in the river with a parabolic velocity profile. The boat travels at a fixed speed, but can choose its orientation. What is the minimum-time path between the two points? 5 | 6 | ## NOTES: 7 | This turns out to be a rather tricky trajectory optimization, and I've not completely solved it here. There are two key difficulties: 8 | - The solution is discontinuous 9 | - The solution is not unique 10 | 11 | There are probably some modifications that can be made to the problem statement to adress one or both of these problems. 12 | 13 | This code is still in progress. Let me know if you want to improve it, or if you find a bug! 14 | 15 | ## Methods: 16 | 0. Multiple Shooting - Euler's Method. This is included to get an idea of what multiple shooting is, but it is not an efficient implementation. 17 | 0. Direct Collocation - Trapazoid Rule. This is a pretty good implementation of the trapazoid rule, and gets reasonable solutions for most of the problems. 18 | 0. Hermite-Simpson Collocation. Seems a bit buggy, which I think is due to the nature of the problem, although there could be a bug. 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /Trajectory_Optimization_Hammer_Example/Get_Bounds.m: -------------------------------------------------------------------------------- 1 | function [Xlow, Xupp] = Get_Bounds(P) 2 | 3 | %Used as enum constants: 4 | LOW = 1; UPP = 2; 5 | 6 | %Rename the number of gridpoints for convienance: 7 | nGridPts = P.nGridPts; 8 | 9 | %Store the low bounds: 10 | Xlow.duration = P.bnd.duration(LOW); 11 | Xlow.state = [P.bnd.angle(LOW); 12 | P.bnd.rate(LOW)]*ones(1,nGridPts); 13 | Xlow.torque = P.bnd.torque(LOW)*ones(1,nGridPts); 14 | 15 | %Store the upper bounds: 16 | Xupp.duration = P.bnd.duration(UPP); 17 | Xupp.state = [P.bnd.angle(UPP); 18 | P.bnd.rate(UPP)]*ones(1,nGridPts); 19 | Xupp.torque = P.bnd.torque(UPP)*ones(1,nGridPts); 20 | 21 | %Some of the constraints can be treated as state bounds: 22 | ImpactState = [P.cst.strikeAngle; P.cst.strikeRate]; 23 | Xlow.state(:,end) = ImpactState; 24 | Xupp.state(:,end) = ImpactState; 25 | 26 | 27 | end -------------------------------------------------------------------------------- /Trajectory_Optimization_Hammer_Example/HammerImpact.m: -------------------------------------------------------------------------------- 1 | function Yplus = HammerImpact(Yminus,P) 2 | 3 | %This function models a simple point-mass pendulum bouncing off of the 4 | %groun with a known coefficient of restitution. 5 | 6 | e = P.dyn.coeffRestitution; %Coefficient of restitution 7 | 8 | %Initialize memory: 9 | Yplus = zeros(size(Yminus)); 10 | Yplus(1,:) = Yminus(1,:); %Th - angle - no change 11 | Yplus(2,:) = -e*Yminus(2,:); %W - rate - impact map 12 | 13 | end -------------------------------------------------------------------------------- /Trajectory_Optimization_Hammer_Example/Initial_State.m: -------------------------------------------------------------------------------- 1 | function [X,P] = Initial_State(P) 2 | 3 | %This function generates a guess at the state struct for fmincon 4 | 5 | %Rename for easy reading: 6 | angleGuess = P.init.angleGuess; 7 | rateGuess = P.init.rateGuess; 8 | torqueGuess = P.init.torqueGuess; 9 | durationGuess = P.init.durationGuess; 10 | timeGuess = linspace(0,durationGuess,length(angleGuess)); 11 | 12 | 13 | %Now interpolate between the guess points: 14 | nGridPts = P.nGridPts; 15 | Time = linspace(0, durationGuess, nGridPts); 16 | 17 | angle = interp1(timeGuess',angleGuess',Time')'; 18 | rate = interp1(timeGuess',rateGuess',Time')'; 19 | torque = interp1(timeGuess',torqueGuess',Time')'; 20 | 21 | %Store the guess as a struct: 22 | X.duration = durationGuess; 23 | X.state = [angle;rate]; 24 | X.torque = torque; 25 | 26 | end -------------------------------------------------------------------------------- /Trajectory_Optimization_Hammer_Example/NonLinCon.m: -------------------------------------------------------------------------------- 1 | function [C, Ceq] = NonLinCon(x,P) 2 | 3 | %NonLinCon is used to get the non-linear constraints for the problem. 4 | %Here, the only nonlinear cosntraints are the defects, which we get from 5 | %the function: PhysicsIntegration. In more complicated problems there 6 | %would be other constraints, which would be included in either C or Ceq. 7 | 8 | Soln = PhysicsIntegration(x,P); 9 | C = []; 10 | Ceq = Soln.Defect_Vec; 11 | 12 | end -------------------------------------------------------------------------------- /Trajectory_Optimization_Hammer_Example/Objective_Function.m: -------------------------------------------------------------------------------- 1 | function Cost = Objective_Function(x,P) 2 | 3 | %The cost function is actually computed in PhysicsIntegration. Since the 4 | %cost functions for this type of problem are time-integrals, it makes sense 5 | %to use the same integration for the system dynamics as for the cost 6 | %function. If there was also a discrete cost, or other non-integral cost, 7 | %it would be added in this function. 8 | 9 | Soln = PhysicsIntegration(x,P); 10 | Cost = Soln.Total_Cost; 11 | 12 | end 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /Trajectory_Optimization_Hammer_Example/Save_Figure_Script.m: -------------------------------------------------------------------------------- 1 | %Save_Figure_Script 2 | 3 | %Variables that must be defined in the workspace: 4 | % P 5 | % iterationNum 6 | % 7 | 8 | %Now, save the figure if desired: 9 | if P.disp.saveIntermediatePlots 10 | BaseName = ['MS_Fig_Iter_' num2str(iterationNum)]; 11 | FigureName = [BaseName '.pdf']; 12 | DirName = 'img/'; %Which subdirectory are the images in? 13 | 14 | % %Force Figure Location (Good for extracting figures -> LaTeX) 15 | set(gcf,'position',800*[0,0,2,1]) 16 | 17 | %This is a magical function that I got on file exchange 18 | %This should not be so hard to do in Matlab... 19 | save2pdf(FigureName,gcf,400) 20 | 21 | %Now, create the latex code if desired: 22 | if P.disp.createTex 23 | FileName = P.disp.TexFileName; 24 | fid = fopen(FileName,'a'); %Open file for appending 25 | fprintf(fid,'\n'); 26 | fprintf(fid,'\\begin{figure} \n'); 27 | fprintf(fid,' \\centering \n'); 28 | fprintf(fid,[' \\includegraphics[width = \\columnwidth]{' DirName FigureName '} \n']); 29 | fprintf(fid,' \\caption{} \n'); 30 | fprintf(fid,[' \\label{fig: ' BaseName '} \n']); 31 | fprintf(fid,'\\end{figure} \n'); 32 | fclose(fid); 33 | end 34 | end -------------------------------------------------------------------------------- /Trajectory_Optimization_Hammer_Example/SmoothBnd.m: -------------------------------------------------------------------------------- 1 | function xBnd = SmoothBnd(x,alpha,Bnd) 2 | % 3 | % xBnd = SmoothBnd(x,alpha,Bnd) 4 | % 5 | % This function is used to smoothly bound the input x; 6 | % 7 | % INPUTS: 8 | % x = a vector of real numbers to apply bounds 9 | % alpha = a positive smoothing parameter. Small values correspond to 10 | % little smoothing 11 | % Bnd = the desired bounds of the function, expressed as a 2-element row 12 | % vector. If ommitted it will default to [0,1]; 13 | 14 | if nargin==2 15 | Low = 0; 16 | Upp = 1; 17 | else 18 | Low = Bnd(1); 19 | Upp = Bnd(2); 20 | end 21 | 22 | %Apply bounding to each part of the input: 23 | xLow = Low + alpha*log(exp((x-Low)/alpha)+1); 24 | xUpp = Upp - alpha*log(exp(-(x-Upp)/alpha)+1); 25 | 26 | %Combine: 27 | xBnd = xLow + xUpp - x; 28 | 29 | end 30 | 31 | -------------------------------------------------------------------------------- /Trajectory_Optimization_Hammer_Example/SmoothRamp.m: -------------------------------------------------------------------------------- 1 | function y = SmoothRamp(x,alpha) 2 | 3 | %y = SmoothRamp(x,alpha) 4 | % 5 | % This function is used to make a smooth version of the ramp function 6 | % 7 | %INPUTS: 8 | % x = a vector of real numbers to be smoothed 9 | % alpha = a positive smoothing parameter. Small values of alpha correspond 10 | % to little smoothing, large values correspond to heavy smoothing 11 | 12 | y = alpha*log(exp(x/alpha)+1); 13 | 14 | end -------------------------------------------------------------------------------- /Trajectory_Optimization_Hammer_Example/TexFigureCode.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MatthewPeterKelly/dscTutorials/e1e97a9be03ec146f88bd6ddd9e06db7ee52e242/Trajectory_Optimization_Hammer_Example/TexFigureCode.txt -------------------------------------------------------------------------------- /bezierCurves/DEMO_bezierCurve.m: -------------------------------------------------------------------------------- 1 | % TEST_bezierCurve.m 2 | % 3 | % This script runs a simple demo of a bezier curve 4 | 5 | order = 5; 6 | 7 | p = rand(2,order+1); 8 | 9 | tSpan = [1,4]; 10 | t = linspace(tSpan(1),tSpan(2),100); 11 | 12 | tic 13 | x = bezierCurve(p,t,tSpan); 14 | toc 15 | 16 | figure(1); clf; 17 | plot(x(1,:),x(2,:)); hold on; 18 | plot(p(1,:),p(2,:),'x'); 19 | -------------------------------------------------------------------------------- /bezierCurves/DEMO_bezierFunction.m: -------------------------------------------------------------------------------- 1 | %TEST_bezierFunction.m 2 | % 3 | % This script tests the use of a bezier curve to represent a vector 4 | % function, rather than an arbitrary space curve 5 | % 6 | 7 | order = 4; 8 | 9 | p = rand(1,order+1); 10 | 11 | 12 | tSpan = [1,4]; 13 | t = linspace(tSpan(1),tSpan(2),100); 14 | pGrid = linspace(tSpan(1),tSpan(2),order+1); 15 | 16 | tic 17 | x = bezierCurve(p,t,tSpan); 18 | toc 19 | 20 | figure(1); clf; 21 | plot(t,x); hold on; 22 | plot(pGrid,p,'x'); -------------------------------------------------------------------------------- /bezierCurves/DEMO_fitBezierCurve.m: -------------------------------------------------------------------------------- 1 | %TEST_fitBezierCurve.m 2 | % 3 | % This script runs a test to make sure that the bezier curve fitting 4 | % function is working properly. 5 | % 6 | 7 | nData = 25; 8 | tSpan = [-1,2]; 9 | t = linspace(tSpan(1),tSpan(2),nData); 10 | x = 2*sin(2*t) + cos(3*t+1); 11 | y = 1-exp(-2*t).*cos(2*t); 12 | z = [x;y]; 13 | 14 | nCtrlPt = 4; 15 | xBnd = [-3,3]; 16 | yBnd = [-5,5]; 17 | zBnd = [xBnd;yBnd]; 18 | 19 | [p,w] = fitBezierCurve(t,z,nCtrlPt,tSpan,zBnd); 20 | 21 | tFit = linspace(tSpan(1),tSpan(2),100); 22 | zFit = rationalBezierCurve(p,w,tFit,tSpan); 23 | 24 | figure(2); clf; 25 | subplot(2,1,1); 26 | plot(t,z(1,:),'x',tFit,zFit(1,:)); 27 | legend('data','fit'); 28 | xlabel('time'); 29 | ylabel('function one'); 30 | subplot(2,1,2); 31 | plot(t,z(2,:),'x',tFit,zFit(2,:)); 32 | legend('data','fit'); 33 | xlabel('time'); 34 | ylabel('function two'); -------------------------------------------------------------------------------- /bezierCurves/DEMO_rationalBezierCurve.m: -------------------------------------------------------------------------------- 1 | % TEST_rationalBezierCurve.m 2 | % 3 | % This script runs a simple demo of a rational bezier curve 4 | 5 | order = 5; 6 | 7 | p = rand(2,order+1); 8 | w = 0.1+rand(1,order+1); %Must be positive for convex hull property to hold! 9 | 10 | tSpan = [1,4]; 11 | t = linspace(tSpan(1),tSpan(2),100); 12 | 13 | tic 14 | x = rationalBezierCurve(p,w,t,tSpan); 15 | toc 16 | 17 | figure(1); clf; 18 | plot(x(1,:),x(2,:)); hold on; 19 | plot(p(1,:),p(2,:),'x'); -------------------------------------------------------------------------------- /bezierCurves/DEMO_rationalBezierFunction.m: -------------------------------------------------------------------------------- 1 | %TEST_bezierFunction.m 2 | % 3 | % This script tests the use of a bezier curve to represent a vector 4 | % function, rather than an arbitrary space curve 5 | % 6 | 7 | order = 2; 8 | 9 | p = rand(1,order+1); 10 | w = 0.05 + 0.95*rand(1,order+1); 11 | 12 | tSpan = [1,4]; 13 | t = linspace(tSpan(1),tSpan(2),100); 14 | pGrid = linspace(tSpan(1),tSpan(2),order+1); 15 | 16 | tic 17 | x = rationalBezierCurve(p,w,t,tSpan); 18 | toc 19 | 20 | figure(1); clf; 21 | plot(t,x); hold on; 22 | scatter(pGrid,p,50*w,'filled'); -------------------------------------------------------------------------------- /bezierCurves/README.txt: -------------------------------------------------------------------------------- 1 | README.txt -- Bezier Curves 2 | 3 | This directory contains functions for evaluating and fitting bezier curves. There are several scripts TEST_* that are used to demonstrate how to use the functions. -------------------------------------------------------------------------------- /bezierCurves/bezierCurve.m: -------------------------------------------------------------------------------- 1 | function x = bezierCurve(p,t,tSpan) 2 | % x = bezierCurve(p,t,tSpan) 3 | % 4 | % This function evaluates a bezier curve, defined by the set of control 5 | % points p, at each value in t. 6 | % 7 | % INPUTS: 8 | % p = [nCurve x nPoint] = control points 9 | % t = [1 x nTime] = times at which to evaluate curve 10 | % tSpan = [1 x 2] = tSpan(1) <= t <= tSpan(2) 11 | % 12 | % OUTPUTS: 13 | % x = [nCurve x nTime] = bezier curve, evaluated at t 14 | % 15 | % NOTES: 16 | % It is not advisable to use this function for high-order polynomials. 17 | % 18 | % See also: RATIONALBEZIERCURVE, FITBEZIERCURVE 19 | % 20 | 21 | [nCurve, nPoint] = size(p); 22 | nTime = length(t); 23 | 24 | t = (t-tSpan(1))/diff(tSpan); 25 | 26 | x = zeros(nCurve,nTime); 27 | n = nPoint - 1; 28 | for i=0:n 29 | tt = (t.^i).*(1-t).^(n-i); 30 | binom = nchoosek(n,i); 31 | x = x + binom*p(:,i+1)*tt; 32 | end 33 | 34 | end 35 | -------------------------------------------------------------------------------- /bezierCurves/rationalBezierCurve.m: -------------------------------------------------------------------------------- 1 | function x = rationalBezierCurve(p,w,t,tSpan) 2 | % x = rationalBezierCurve(p,w,t,tSpan) 3 | % 4 | % This function evaluates a rational bezier curve, defined by points p and 5 | % weights w. 6 | % 7 | % INPUTS: 8 | % p = [nCurve x nPoint] = control points 9 | % w = [1 x nPoint] = control point weights 10 | % t = [1 x nTime] = times at which to evaluate curve 11 | % tSpan = [1 x 2] = tSpan(1) <= t <= tSpan(2) 12 | % 13 | % OUTPUTS: 14 | % x = [nCurve x nTime] = bezier curve, evaluated at t 15 | % 16 | % NOTES: 17 | % It is not advisable to use this function for high-order polynomials. 18 | % 19 | % See also: BEZIERCURVE, FITBEZIERCURVE 20 | % 21 | 22 | [nCurve, nPoint] = size(p); 23 | nTime = length(t); 24 | 25 | t = (t-tSpan(1))/diff(tSpan); 26 | 27 | %%% Compute the numerator and denominator: 28 | num = zeros(nCurve,nTime); 29 | den = zeros(nCurve,nTime); 30 | ONE = ones(nCurve,1); 31 | n = nPoint - 1; 32 | for i=0:n 33 | tt = (t.^i).*(1-t).^(n-i); 34 | binom = nchoosek(n,i); 35 | num = num + binom *w(i+1)*p(:,i+1)*tt; 36 | den = den + binom *w(i+1)*ONE*tt; 37 | end 38 | x = num./den; 39 | 40 | end -------------------------------------------------------------------------------- /bouncingBall/Ball_Dynamics.m: -------------------------------------------------------------------------------- 1 | function dZ = Ball_Dynamics(~,Z,P) 2 | 3 | %This function computes the dynamics of the ball as it travels through the 4 | %air. The ball is treated as a point mass, acting under the uniform 5 | %acceleration of gravity and quadratic drag from the air. 6 | % 7 | % INPUTS: 8 | % (~) = time (not used) 9 | % Z = a [4xN] matrix of states, where each column is a state vector 10 | % P = a parameter struct created by Set_Parameters.m 11 | % 12 | % OUTPUTS: 13 | % dZ = a [4xN] matrix giving the derivative of Z with respect to time 14 | % 15 | 16 | %Unpack the physical parameters 17 | g = P.gravity; %(m/s^2) gravitational acceleration 18 | m = P.mass; %(kg) mass of the ball 19 | c = P.drag; %(N*s^2/m^2) quadratic drag coefficient 20 | 21 | %Get the velocity vector 22 | % pos = Z(1:2,:); %Position vector is not used 23 | vel = Z(3:4,:); %Velocity vector 24 | 25 | % Compute gravity and drag accelerations 26 | speed = sqrt(vel(1,:).^2+vel(2,:).^2); %(m/s) scalar speed 27 | Drag = -c*vel.*speed/m; %(m/s^2) quadratic drag 28 | Gravity = [0;-g]*ones(size(speed)); %(m/s^2) gravitational acceleration 29 | 30 | % Return derivative of the state 31 | acc = Drag + Gravity; 32 | dZ = [vel;acc]; 33 | 34 | end -------------------------------------------------------------------------------- /bouncingBall/EventFunction.m: -------------------------------------------------------------------------------- 1 | function [value,isterminal,direction] = EventFunction(~,X) 2 | 3 | x = X(1,:); 4 | y = X(2,:); 5 | ground = groundHeight(x); 6 | 7 | value = y-ground; 8 | 9 | %Stop if the hit the ground 10 | isterminal = true; 11 | 12 | %Should only be coming to the ground from above 13 | direction = -1; 14 | 15 | end -------------------------------------------------------------------------------- /bouncingBall/PlotEnergy.m: -------------------------------------------------------------------------------- 1 | function PlotEnergy(timeAll, stateAll, P) 2 | 3 | clf; hold on; 4 | 5 | %break out the data: 6 | time = timeAll; 7 | %xPos = stateAll(1,:); 8 | yPos = stateAll(2,:); 9 | xVel = stateAll(3,:); 10 | yVel = stateAll(4,:); 11 | 12 | m = P.mass; 13 | g = P.gravity; 14 | 15 | %Kinetic energy 16 | KE = 0.5*m*(xVel.^2+yVel.^2); 17 | 18 | %Potential energy 19 | PE = m*g*yPos; 20 | 21 | %Total energy 22 | Energy = KE+PE; 23 | 24 | %Plot the system energy 25 | plot(timeAll,KE,'b-','LineWidth',P.CurveLineWidth) 26 | plot(timeAll,PE,'r-','LineWidth',P.CurveLineWidth) 27 | plot(timeAll,Energy,'k-','LineWidth',P.CurveLineWidth+2) 28 | hLegend = legend('kinetic','potential','total'); 29 | title('System Energy','fontsize',P.TitleFontSize) 30 | xlabel('Time (s)','FontSize',P.LabelFontSize) 31 | ylabel('Energy (J)','FontSize',P.LabelFontSize) 32 | set(gca,'fontsize',P.AxisFontSize); 33 | set(hLegend,'fontsize',P.LegendFontSize); 34 | 35 | end -------------------------------------------------------------------------------- /bouncingBall/README.md: -------------------------------------------------------------------------------- 1 | bouncingBall 2 | ============ 3 | 4 | Entry-point: MAIN.m 5 | 6 | Runs a simulation of a ball bouncing over hilly terrain. Simulation implemented using ode45 and event detection. 7 | -------------------------------------------------------------------------------- /bouncingBall/groundHeight.m: -------------------------------------------------------------------------------- 1 | function [y, slope] = groundHeight(x) 2 | % 3 | % This function returns the height (y) and slope (Dy) of the ground as a 4 | % function of the horizontal position. An interesting ground profile is 5 | % generated by summing three sine waves. This also makes the derivative 6 | % (slope) easy to compute. 7 | % 8 | 9 | %Parameters for each of the three sine ewaves: 10 | Amp1 = 0.3; 11 | Freq1 = 1; 12 | Phase1 = 1.58; 13 | 14 | Amp2 = 0.1; 15 | Freq2 = 6; 16 | Phase2 = 1.9; 17 | 18 | Amp3 = 0.2; 19 | Freq3 = 3.7; 20 | Phase3 = 1.58; 21 | 22 | %Compute the ground height 23 | y = Amp1*sin(Freq1*x+Phase1)+... 24 | Amp2*sin(Freq2*x+Phase2)+... 25 | Amp3*sin(Freq3*x+Phase3); 26 | 27 | %Compute the ground slope 28 | slope = Amp1*Freq1*cos(Freq1*x+Phase1)+... 29 | Amp2*Freq2*cos(Freq2*x+Phase2)+... 30 | Amp3*Freq3*cos(Freq3*x+Phase3); 31 | 32 | end -------------------------------------------------------------------------------- /chebyshevPolynomials/DEMO_9_chebEval.m: -------------------------------------------------------------------------------- 1 | %DEMO_9_chebEval.m 2 | 3 | % This script is designed to test chebEval, a fast version of 4 | % chebyshevInterpolate that is to be used as a compiled mex function. 5 | 6 | nCurves = 3; 7 | nChebPts = 12; 8 | 9 | f = rand(nCurves,nChebPts); 10 | d = [0,4]; %Domain for the functions 11 | t = linspace(d(1),d(2),1000); 12 | x = chebyshevPoints(nChebPts,d); 13 | 14 | %%%% Attempt to build the project if the mex function does not exist 15 | if ~exist('chebEval_mex','file') 16 | coder -build chebyshevMexProject.prj 17 | end 18 | 19 | tic 20 | y1 = chebEval(f,t,d); 21 | time = toc; 22 | 23 | tic 24 | y2 = chebEval_mex(f,t,d); 25 | mexTime = toc; 26 | 27 | %Check that both compute the same result: 28 | error = mean(mean((y1-y2).^2)); 29 | 30 | if error > 100*eps 31 | disp('Error - chebEval and chebEval_mex do not produce identical results') 32 | end 33 | 34 | figure(1001); clf; hold on; 35 | plot(t,y1,'LineWidth',2); 36 | plot(x,f,'kx','MarkerSize',8,'LineWidth',2); 37 | extents = axis; 38 | for i=1:nChebPts 39 | plot([1,1]*x(i),extents(3:4),'k:') 40 | end 41 | ylabel('Function value') 42 | xlabel('Function input') 43 | 44 | fprintf('Matlab function time: %6.6f ms\n',1000*time); 45 | fprintf('Mex function time: %6.6f ms\n',1000*mexTime); -------------------------------------------------------------------------------- /chebyshevPolynomials/Description.html: -------------------------------------------------------------------------------- 1 | Methods for function approximation using Chebyshev Polynomials. These methods use 2 | Barycentric Interpolation, 3 | which allows for efficient computation and numerical stability, even for high-order approximations. 4 | I use these approximations for solving ordinary differential equations and in the 5 | background of trajectory optimization problems. -------------------------------------------------------------------------------- /chebyshevPolynomials/README.md: -------------------------------------------------------------------------------- 1 | chebyshevPolynomials 2 | 3 | =============================== 4 | 5 | Updated October 28, 2013 6 | 7 | Written by Matthew Kelly 8 | Cornell University 9 | 10 | Functions: 11 | - chebyshevFit 12 | - chebyshevInterpolate 13 | - chebyshevODEsolve 14 | - chebyshevDerivative 15 | - chebyshevIntegral 16 | 17 | Helper Functions: 18 | - chebyshevPoints 19 | - chebyshevDifferentiationMatrix 20 | 21 | Demonstrations: 22 | - DEMO_1_compare 23 | - DEMO_2_derivatives 24 | - DEMO_3_nodeDerivatives 25 | - DEMO_4_Order_vs_Accuracy 26 | - DEMO_5_Multivariate 27 | - DEMO_6_ODE_solve 28 | - DEMO_7_Order_vs_Accuracy_ODE_solve 29 | - DEMO_8_integral 30 | 31 | Methods for function approximation using Chebyshev Polynomials. These methods use 32 | Barycentric Interpolation, which allows for efficient computation and numerical stability, even for high-order approximations. 33 | 34 | I use these approximations for solving ordinary differential equations and in the background of trajectory optimization problems. 35 | 36 | 37 | -------------------------------------------------------------------------------- /chebyshevPolynomials/README.txt: -------------------------------------------------------------------------------- 1 | chebyshevPolynomials 2 | 3 | Updated October 28, 2013 4 | 5 | Written by Matthew Kelly 6 | Cornell University 7 | 8 | Functions: 9 | chebyshevFit 10 | chebyshevInterpolate 11 | chebyshevODEsolve 12 | chebyshevDerivative 13 | chebyshevIntegral 14 | 15 | Helper Functions: 16 | chebyshevPoints 17 | chebyshevDifferentiationMatrix 18 | 19 | Demonstrations: 20 | DEMO_1_compare 21 | DEMO_2_derivatives 22 | DEMO_3_nodeDerivatives 23 | DEMO_4_Order_vs_Accuracy 24 | DEMO_5_Multivariate 25 | DEMO_6_ODE_solve 26 | DEMO_7_Order_vs_Accuracy_ODE_solve 27 | DEMO_8_integral 28 | 29 | Methods for function approximation using Chebyshev Polynomials. These methods use 30 | Barycentric Interpolation, which allows for efficient computation and numerical stability, even for high-order approximations. 31 | 32 | I use these approximations for solving ordinary differential equations and in the background of trajectory optimization problems. 33 | 34 | 35 | -------------------------------------------------------------------------------- /chebyshevPolynomials/chebyshevIntegral.m: -------------------------------------------------------------------------------- 1 | function If = chebyshevIntegral(f,d) 2 | %If = chebyshevIntegral(f,d) 3 | % 4 | % FUNCTION: 5 | % This function computes the integrals of the chebyshev approximation 6 | % using Clenshaw-Curtis quadrature. 7 | % 8 | % INPUTS: 9 | % f = [nState x nPoints] values at each chebyshev node 10 | % d = [1 x 2] domain of the chebyshev function 11 | % 12 | % OUTPUTS: 13 | % If = the integral of the chebyshev interpolant at each chebyshev node, 14 | % with the integral defined to be zero at the first node. 15 | % 16 | 17 | %Get the quadrature weights: 18 | [k,n] = size(f); 19 | [~,w] = chebyshevPoints(n,d); 20 | 21 | %Perform the integral 22 | ONE = ones(k,1); %get the matrix size correct for .* operation 23 | If = cumsum(f.*(ONE*w)); 24 | 25 | end 26 | -------------------------------------------------------------------------------- /chebyshevPolynomials/simpleHarmonicOscillator.m: -------------------------------------------------------------------------------- 1 | function dz = simpleHarmonicOscillator(z,P) 2 | %dz = simpleHarmonicOscillator(z,P) 3 | % 4 | % FUNCTION: 5 | % This function is used to compute the dynamics of a simple harmonic 6 | % oscillator. This is used for testing ode solvers with a simple system. 7 | % 8 | % INPUTS: 9 | % t = [1 x Nt] input time vector. (NOT USED) 10 | % z = [Ns x Nt] input state vector. 11 | % P = struct of parameteres with fields: 12 | % m = massls 13 | % c = damping constant 14 | % k = spring constant 15 | % 16 | % OUTPUTS: 17 | % dz = the derivative of z with respect to time. 18 | % 19 | 20 | %Physical parameters 21 | m = P.m; 22 | c = P.c; 23 | k = P.k; 24 | 25 | %States 26 | x = z(1,:); 27 | v = z(2,:); 28 | 29 | %Solution: 30 | dz = zeros(size(z)); 31 | dz(1,:) = v; 32 | dz(2,:) = -(c/m)*v - (k/m)*x; 33 | 34 | end -------------------------------------------------------------------------------- /cmaes_controlDesign/README.md: -------------------------------------------------------------------------------- 1 | cmaes_controlDesign 2 | ============ 3 | 4 | Entry-point: MAIN.m 5 | 6 | Finds the parameters of a controller for a simple inverted pendulum. Optimization is done using CMAES. 7 | 8 | Dependancies: 9 | 10 | - CMAES: [optimization algorithm](https://www.lri.fr/~hansen/cmaes.m) and [plotting routine](https://www.lri.fr/~hansen/plotcmaesdat.m). Main page for source code is [here](https://www.lri.fr/~hansen/cmaes_inmatlab.html) 11 | 12 | -------------------------------------------------------------------------------- /cmaes_controlDesign/control.m: -------------------------------------------------------------------------------- 1 | function u = control(z,k) 2 | 3 | %PD controller with gravity compensation 4 | 5 | x = z(1,:); % angle 6 | v = z(2,:); % rate 7 | 8 | %Unpack controller 9 | kp = k(1,:); 10 | kd = k(2,:); 11 | kg = k(3,:); 12 | 13 | u = kp.*x + kd.*v + kg.*sin(x); 14 | 15 | end -------------------------------------------------------------------------------- /cmaes_controlDesign/dynamics.m: -------------------------------------------------------------------------------- 1 | function zdot = dynamics(z,u) 2 | 3 | %Dynamics for a damped inverted pendulum 4 | 5 | m = 1.25; 6 | g = 9.81; 7 | l = 0.5; 8 | c = 0.01; %Damping 9 | I = m*l*l; 10 | 11 | x = z(1,:); % angle 12 | v = z(2,:); % rate 13 | 14 | dx = v; 15 | dv = -(c/I)*v - (m*g*l/I)*sin(-x) + u/I; 16 | 17 | zdot = [dx;dv]; 18 | 19 | end -------------------------------------------------------------------------------- /cmaes_controlDesign/objective.m: -------------------------------------------------------------------------------- 1 | function val = objective(guess,IC) 2 | 3 | %Runs a large number of simulations of the controlled simulation and then 4 | %returns a combination of state and actuation error 5 | 6 | duration = 3; 7 | dt = 0.05; 8 | n = round(duration/dt); 9 | 10 | z = IC; 11 | 12 | %Weighting between relative cost. 13 | x_cost = 0.7^-2; 14 | v_cost = 1.5^-2; 15 | u_cost = 1.0^-2; 16 | 17 | %Run simulation using RK4 integration in parallel 18 | val = 0; %cost accumulator 19 | for i=1:n 20 | [k1, u] = rhs(z,guess); 21 | k2 = rhs(z + 0.5*dt*k1,guess); 22 | k3 = rhs(z + 0.5*dt*k2,guess); 23 | k4 = rhs(z + k3,guess); 24 | 25 | z = z + (dt/6)*(k1 + 2*k2 + 2*k3 + k4); 26 | 27 | val = val + x_cost*sum(z(1,:).^2); 28 | val = val + v_cost*sum(z(2,:).^2); 29 | val = val + u_cost*sum(u.^2); 30 | end 31 | 32 | end 33 | 34 | function [xdot, u] = rhs(z,k) 35 | 36 | u = control(z,k); 37 | xdot = dynamics(z,u); 38 | 39 | end 40 | -------------------------------------------------------------------------------- /cmaes_controlDesign/simulate.m: -------------------------------------------------------------------------------- 1 | function [t, z, u] = simulate(gains,z0) 2 | 3 | duration = 3; 4 | dt = 0.01; 5 | n = round(duration/dt); 6 | 7 | z = zeros(2,n); 8 | 9 | z(:,1) = z0; 10 | t = zeros(1,n); 11 | for i=2:n 12 | k1 = rhs(z(:,i-1),gains); 13 | k2 = rhs(z(:,i-1) + 0.5*dt*k1,gains); 14 | k3 = rhs(z(:,i-1) + 0.5*dt*k2,gains); 15 | k4 = rhs(z(:,i-1) + k3,gains); 16 | 17 | z(:,i) = z(:,i-1) + (dt/6)*(k1 + 2*k2 + 2*k3 + k4); 18 | t(i) = t(i-1) + dt; 19 | end 20 | 21 | u = control(z,gains); 22 | 23 | end 24 | 25 | function [xdot, u] = rhs(z,k) 26 | 27 | u = control(z,k); 28 | xdot = dynamics(z,u); 29 | 30 | end 31 | -------------------------------------------------------------------------------- /dynamicsFuncGradients/MAIN.m: -------------------------------------------------------------------------------- 1 | % MAIN - Dynamics Function Gradients 2 | % 3 | % This script demonstrates how to use the functions for computing gradients 4 | % of dynamics functions, as well as testing that the results match the 5 | % analytic solutions. 6 | % 7 | % 8 | % See derivations in Derive_Equations 9 | % 10 | % Core of the gradient of the backslash is from: 11 | % http://www.atmos.washington.edu/~dennis/MatrixCalculus.pdf 12 | 13 | clc; clear; 14 | 15 | N = 100; 16 | q = randn(2,N); 17 | dq = randn(2,N); 18 | u = rand(3,N); 19 | 20 | tic 21 | [A_ddq, A_ddqz] = dynamicsAnalytic(q,dq,u); 22 | toc 23 | tic 24 | [N_ddq, N_ddqz] = dynamicsNumeric(q,dq,u); 25 | toc 26 | 27 | error_dyn = A_ddq-N_ddq; 28 | error_grad = A_ddqz-N_ddqz; 29 | 30 | fprintf('Max error in dynamics: %6.6g\n',max(max(abs(error_dyn)))); 31 | fprintf('Max error in gradients: %6.6g\n',max(max(max(abs(error_grad))))); 32 | -------------------------------------------------------------------------------- /dynamicsFuncGradients/autoGen_dynamicsNumeric.m: -------------------------------------------------------------------------------- 1 | function [A,b,Az,bz] = autoGen_dynamicsNumeric(q1,q2,dq1,dq2,u0,u1,u2,empty) 2 | %AUTOGEN_DYNAMICSNUMERIC 3 | % [A,B,AZ,BZ] = AUTOGEN_DYNAMICSNUMERIC(Q1,Q2,DQ1,DQ2,U0,U1,U2,EMPTY) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.2. 6 | % 22-Sep-2015 19:45:58 7 | 8 | t2 = q1-q2; 9 | t3 = cos(t2); 10 | t4 = sin(t2); 11 | t5 = dq1.^2; 12 | t6 = cos(q2); 13 | t7 = t6.*u0; 14 | t8 = empty+t4; 15 | t9 = empty-t4; 16 | t10 = dq2.^2; 17 | t11 = t3.*t10; 18 | t12 = t3.*t5; 19 | t13 = sin(q2); 20 | t14 = empty-dq1.*t4.*2.0; 21 | t15 = cos(q1); 22 | t16 = empty-1.0; 23 | A = [empty-t3-2.0;empty-t3;empty-t3-1.0;t16]; 24 | if nargout > 1 25 | b = [empty+t7-u1-t4.*t5+t4.*t10+t15.*u0.*2.0;empty+t7-u2-t4.*t5]; 26 | end 27 | if nargout > 2 28 | Az = [t8;t8;t8;empty;t9;t9;t9;empty;empty;empty;empty;empty;empty;empty;empty;empty;empty;empty;empty;empty;empty;empty;empty;empty;empty;empty;empty;empty]; 29 | end 30 | if nargout > 3 31 | bz = [empty+t11-t3.*t5-u0.*sin(q1).*2.0;empty-t3.*t5;empty-t11+t12-t13.*u0;empty+t12-t13.*u0;t14;t14;empty+dq2.*t4.*2.0;empty;empty+t6+t15.*2.0;empty+t6;t16;empty;empty;t16]; 32 | end 33 | -------------------------------------------------------------------------------- /dynamicsFuncGradients/dynamicsAnalytic.m: -------------------------------------------------------------------------------- 1 | function [ddq, ddqz] = dynamicsAnalytic(q,dq,u) 2 | % [ddq, ddqz] = dynamicsAnalytic(q,dq,u) 3 | % 4 | % This function computes the dynamics and the gradient of the dynamics for 5 | % the system by numerically solving linear systems 6 | % 7 | % INPUTS: 8 | % q = [nq, nt] = matrix of configurations 9 | % dq = [nq, nt] = matrix of rates 10 | % u = ,nu, nt] = matrix of inputs 11 | % 12 | % OUTPUTS: 13 | % ddq = [nq,nt] = matrix of accelerations 14 | % ddqz = [nq, (nq+nq+nu), nt] = gradients of ddq wrt [q;dq;u]; 15 | % 16 | 17 | empty = zeros(size(q(1,:))); %Stupid hack for vectorization 18 | [ddq,ddqz] = autoGen_dynamicsAnalytic(q(1,:),q(2,:),dq(1,:),dq(2,:),u(1,:),u(2,:),u(3,:),empty); 19 | 20 | [nq, nt] = size(q); 21 | nu = size(u,1); 22 | 23 | ddqz = reshape(ddqz, nq, (nq+nq+nu), nt); 24 | 25 | end -------------------------------------------------------------------------------- /dynamicsFuncGradients/dynamicsNumeric.m: -------------------------------------------------------------------------------- 1 | function [ddq, ddqz] = dynamicsNumeric(q,dq,u) 2 | % [ddq, ddqz] = dynamicsNumeric(q,dq,u) 3 | % 4 | % This function computes the dynamics and the gradient of the dynamics for 5 | % the system by numerically solving linear systems 6 | % 7 | % INPUTS: 8 | % q = [nq, nt] = matrix of configurations 9 | % dq = [nq, nt] = matrix of rates 10 | % u = ,nu, nt] = matrix of inputs 11 | % 12 | % OUTPUTS: 13 | % ddq = [nq,nt] = matrix of accelerations 14 | % ddqz = [nq, (nq+nq+nu), nt] = gradients of ddq wrt [q;dq;u]; 15 | % 16 | 17 | empty = zeros(size(q(1,:))); %Stupid hack for vectorization 18 | [A,B,Az,Bz] = autoGen_dynamicsNumeric(q(1,:),q(2,:),dq(1,:),dq(2,:),u(1,:),u(2,:),u(3,:),empty); 19 | nx = size(q,1); 20 | nu = size(u,1); 21 | nz = 2*nx + nu; 22 | [ddq, ddqz] = computeGradientOfBackSlash(A,B,Az,Bz,nx,nz); 23 | 24 | end -------------------------------------------------------------------------------- /dynamicsFuncGradients/symbolicGradients.m: -------------------------------------------------------------------------------- 1 | function [Az, Bz] = symbolicGradients(A,b,q,dq,u) 2 | % [Az, Bz] = symbolicGradients(A,b,q,dq,u) 3 | % 4 | % This function computes the symbolic gradients of the dynamics of a system 5 | % in mass matrix form: 6 | % 7 | % A(q,dq,u)*ddq = b(q,dq,u); 8 | % 9 | % q = [n, 1] = configuration 10 | % dq = [n, 1] = rates 11 | % ddq = [n,1] = accelerations 12 | % u = [m,1] = system inputs 13 | % 14 | % Let: 15 | % z = [q;dq;u] 16 | % 17 | % Then Az = dA/dz, which has been reshaped to a column vector: 18 | % Az = [n*n*(2*n+m),1]; 19 | % bz = [n*(2*n+m),1]; 20 | % 21 | % 22 | 23 | % DecisionVars: 24 | z = [q;dq;u]; 25 | 26 | n = length(q); 27 | m = length(u); 28 | l = 2*n+m; 29 | 30 | % Compute jacobian of mass matrix and generalized forces: 31 | Az = reshape(jacobian(reshape(A,n*n,1),z),n*n*l,1); 32 | Bz = reshape(jacobian(b,z),n*l,1); 33 | 34 | end -------------------------------------------------------------------------------- /feedbackLinearization/MAIN_passiveSimulate.m: -------------------------------------------------------------------------------- 1 | % MAIN_passiveSimulate.m 2 | % 3 | % This script runs a passive simulation of the acrobot, to get a rough 4 | % trajectory that is similar to a single walking step. 5 | % 6 | 7 | clc; clear; 8 | 9 | p.m1 = 10; 10 | p.m2 = 1; 11 | p.g = 5; 12 | p.l1 = 2; 13 | p.l2 = 2; 14 | 15 | % Initial state: 16 | th0 = (pi/180)*30; 17 | dth0 = (pi/180)*150; 18 | q1 = (pi/180)*180 + th0; 19 | q2 = -th0; 20 | dq1 = -dth0; 21 | dq2 = dth0; 22 | z0 = [q1;q2;dq1;dq2]; %Pack up initial state 23 | 24 | tSpan = [0,1]; %time span for the simulation 25 | dynFun = @(t,z)( acrobotDynamics(z,0,p) ); %passive dynamics function 26 | 27 | % Run simulation: 28 | sol = ode45(dynFun,tSpan,z0); 29 | t = linspace(tSpan(1),tSpan(2),500); 30 | z = deval(sol,t); 31 | u = zeros(size(t)); 32 | 33 | % Animate the results: 34 | A.plotFunc = @(t,z)( drawAcrobot(t,z,p) ); 35 | A.speed = 0.2; 36 | A.figNum = 101; 37 | animate(t,z,A) 38 | 39 | % Plot the results: 40 | figure(1337); clf; plotAcrobot(t,z,u,p) 41 | -------------------------------------------------------------------------------- /feedbackLinearization/acrobotDynamics.m: -------------------------------------------------------------------------------- 1 | function dz = acrobotDynamics(z,u,p) 2 | % dz = acrobotDynamics(z,u,p) 3 | % 4 | % This function computes the dynamics of the acrobot: double pendulum, two 5 | % point masses, torque motor between links, no friction. 6 | % 7 | % INPUTS: 8 | % z = [4,1] = state vector 9 | % u = [1,1] = input torque 10 | % p = parameter struct: 11 | % .m1 = elbow mass 12 | % .m2 = wrist mass 13 | % .g = gravitational acceleration 14 | % .l1 = length shoulder to elbow 15 | % .l2 = length elbow to wrist 16 | % 17 | % OUTPUTS: 18 | % dz = [4,1] = dz/dt = time derivative of the state 19 | % 20 | % NOTES: 21 | % 22 | % states: 23 | % 1 = q1 = first link angle 24 | % 2 = q2 = second link angle 25 | % 3 = dq1 = first link angular rate 26 | % 4 = dq2 = second link angular rate 27 | % 28 | % angles: measured from negative j axis with positive convention 29 | % 30 | 31 | 32 | 33 | 34 | q1 = z(1); 35 | q2 = z(2); 36 | dq1 = z(3); 37 | dq2 = z(4); 38 | 39 | % D*ddq + G = B*u 40 | [D,G,B] = autoGen_acrobotDynamics(q1,q2,dq1,dq2,p.m1,p.m2,p.g,p.l1,p.l2); 41 | 42 | ddq = D\(B*u - G); 43 | 44 | dz = [dq1;dq2;ddq]; 45 | 46 | end -------------------------------------------------------------------------------- /feedbackLinearization/acrobotEnergy.m: -------------------------------------------------------------------------------- 1 | function [E,U,T] = acrobotEnergy(z,p) 2 | % [E,U,T] = acrobotEnergy(z,p) 3 | % 4 | % This function computes the mechanical energy of the acrobot. 5 | % 6 | % INPUTS: 7 | % z = [4,n] = state vector 8 | % p = parameter struct: 9 | % .m1 = elbow mass 10 | % .m2 = wrist mass 11 | % .g = gravitational acceleration 12 | % .l1 = length shoulder to elbow 13 | % .l2 = length elbow to wrist 14 | % 15 | % OUTPUTS: 16 | % E = [1,n] = total mechanical energy 17 | % U = [1,n] = potential energy 18 | % T = [1,n] = kinetic energy 19 | % 20 | % NOTES: 21 | % 22 | % states: 23 | % 1 = q1 = first link angle 24 | % 2 = q2 = second link angle 25 | % 3 = dq1 = first link angular rate 26 | % 4 = dq2 = second link angular rate 27 | % 28 | % angles: measured from negative j axis with positive convention 29 | % 30 | 31 | 32 | q1 = z(1,:); 33 | q2 = z(2,:); 34 | dq1 = z(3,:); 35 | dq2 = z(4,:); 36 | 37 | [U,T] = autoGen_acrobotEnergy(q1,q2,dq1,dq2,p.m1,p.m2,p.g,p.l1,p.l2); 38 | 39 | E = U+T; 40 | 41 | end -------------------------------------------------------------------------------- /feedbackLinearization/acrobotKinematics.m: -------------------------------------------------------------------------------- 1 | function [p1,p2,dp1,dp2] = acrobotKinematics(z,p) 2 | % [p1,p2,dp1,dp2] = acrobotKinematics(z,p) 3 | % 4 | % This function computes the mechanical energy of the acrobot. 5 | % 6 | % INPUTS: 7 | % z = [4,n] = state vector 8 | % p = parameter struct: 9 | % .m1 = elbow mass 10 | % .m2 = wrist mass 11 | % .g = gravitational acceleration 12 | % .l1 = length shoulder to elbow 13 | % .l2 = length elbow to wrist 14 | % 15 | % OUTPUTS: 16 | % p1 = [2,n] = position of the elbow joint 17 | % p2 = [2,n] = position of the wrist 18 | % dp1 = [2,n] = velocity of the elbow joint 19 | % dp2 = [2,n] = velocity of the wrist 20 | % 21 | % NOTES: 22 | % 23 | % states: 24 | % 1 = q1 = first link angle 25 | % 2 = q2 = second link angle 26 | % 3 = dq1 = first link angular rate 27 | % 4 = dq2 = second link angular rate 28 | % 29 | % angles: measured from negative j axis with positive convention 30 | % 31 | 32 | q1 = z(1,:); 33 | q2 = z(2,:); 34 | dq1 = z(3,:); 35 | dq2 = z(4,:); 36 | 37 | [p1,p2,dp1,dp2] = autoGen_acrobotKinematics(q1,q2,dq1,dq2,p.l1,p.l2); 38 | 39 | end -------------------------------------------------------------------------------- /feedbackLinearization/autoGen_acrobotDynamics.m: -------------------------------------------------------------------------------- 1 | function [D,G,B] = autoGen_acrobotDynamics(q1,q2,dq1,dq2,m1,m2,g,l1,l2) 2 | %AUTOGEN_ACROBOTDYNAMICS 3 | % [D,G,B] = AUTOGEN_ACROBOTDYNAMICS(Q1,Q2,DQ1,DQ2,M1,M2,G,L1,L2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.2. 6 | % 12-Jun-2015 16:56:47 7 | 8 | t2 = cos(q1); 9 | t3 = l1.^2; 10 | t4 = sin(q1); 11 | t5 = cos(q2); 12 | t6 = l1.*t2; 13 | t7 = l2.*t5; 14 | t8 = t6+t7; 15 | t9 = sin(q2); 16 | t10 = l1.*t4; 17 | t11 = l2.*t9; 18 | t12 = t10+t11; 19 | t13 = l2.^2; 20 | D = reshape([-m1.*t2.^2.*t3-m1.*t3.*t4.^2-l1.*m2.*t2.*t8-l1.*m2.*t4.*t12,-l1.*l2.*m2.*t2.*t5-l1.*l2.*m2.*t4.*t9,-l2.*m2.*t5.*t8-l2.*m2.*t9.*t12,-m2.*t5.^2.*t13-m2.*t9.^2.*t13],[2, 2]); 21 | if nargout > 1 22 | t14 = dq1.^2; 23 | t15 = dq2.^2; 24 | t16 = l1.*t2.*t14; 25 | t17 = l2.*t5.*t15; 26 | t18 = t16+t17; 27 | t19 = l1.*t4.*t14; 28 | t20 = l2.*t9.*t15; 29 | t21 = t19+t20; 30 | G = [-g.*m2.*t12+m2.*t8.*t21-m2.*t12.*t18-g.*l1.*m1.*t4;-g.*l2.*m2.*t9+l2.*m2.*t5.*t21-l2.*m2.*t9.*t18]; 31 | end 32 | if nargout > 2 33 | B = [0.0;-1.0]; 34 | end 35 | -------------------------------------------------------------------------------- /feedbackLinearization/autoGen_acrobotEnergy.m: -------------------------------------------------------------------------------- 1 | function [U,T] = autoGen_acrobotEnergy(q1,q2,dq1,dq2,m1,m2,g,l1,l2) 2 | %AUTOGEN_ACROBOTENERGY 3 | % [U,T] = AUTOGEN_ACROBOTENERGY(Q1,Q2,DQ1,DQ2,M1,M2,G,L1,L2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.2. 6 | % 12-Jun-2015 16:56:48 7 | 8 | t2 = cos(q1); 9 | t3 = dq1.^2; 10 | t4 = l1.^2; 11 | t5 = sin(q1); 12 | t6 = cos(q2); 13 | U = -g.*m2.*(l1.*t2+l2.*t6)-g.*l1.*m1.*t2; 14 | if nargout > 1 15 | t7 = dq1.*l1.*t2+dq2.*l2.*t6; 16 | t8 = dq2.*l2.*sin(q2)+dq1.*l1.*t5; 17 | T = m1.*(t2.^2.*t3.*t4+t3.*t4.*t5.^2).*(1.0./2.0)+m2.*(t7.^2+t8.^2).*(1.0./2.0); 18 | end 19 | -------------------------------------------------------------------------------- /feedbackLinearization/autoGen_acrobotKinematics.m: -------------------------------------------------------------------------------- 1 | function [p1,p2,dp1,dp2] = autoGen_acrobotKinematics(q1,q2,dq1,dq2,l1,l2) 2 | %AUTOGEN_ACROBOTKINEMATICS 3 | % [P1,P2,DP1,DP2] = AUTOGEN_ACROBOTKINEMATICS(Q1,Q2,DQ1,DQ2,L1,L2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.2. 6 | % 12-Jun-2015 16:56:48 7 | 8 | t2 = sin(q1); 9 | t3 = l1.*t2; 10 | t4 = cos(q1); 11 | p1 = [t3;-l1.*t4]; 12 | if nargout > 1 13 | t5 = dq1.*l1.*t4; 14 | t6 = cos(q2); 15 | t7 = dq1.*l1.*t2; 16 | t8 = sin(q2); 17 | p2 = [t3+l2.*t8;-l1.*t4-l2.*t6]; 18 | end 19 | if nargout > 2 20 | dp1 = [t5;t7]; 21 | end 22 | if nargout > 3 23 | dp2 = [t5+dq2.*l2.*t6;t7+dq2.*l2.*t8]; 24 | end 25 | -------------------------------------------------------------------------------- /feedbackLinearization/controlMap.m: -------------------------------------------------------------------------------- 1 | function u = controlMap(dq, v, dhRef, ddhRef, c, H, D, B, G) 2 | % u = controlMap(dq, v, dhRef, ddhRef, c, H, D, B, G) 3 | % 4 | % This function computes the control mapping from v (acceleration of error) 5 | % to u (joint torques) 6 | % 7 | % INPUTS: 8 | % 9 | % dq = [nConfig, 1] = time rate of change in configuration 10 | % v = [nMeasure, 1] = desired acceleration of error 11 | % 12 | % dhRef = [nMeasure, 1] = derivative of reference wrt phase 13 | % ddhRef = [nMeasure, 1] = second derivative of ref wrt phase 14 | % 15 | % c = [1, nConfig] = mapping from configuration to phase 16 | % H = [nMeasure, nConfig] = mapping from configuration to measurement 17 | % 18 | % {D, B, G} = Dynamics Matricies: D*ddq + G = B*u 19 | % D = [nConfig, nConfig] = mass matrix 20 | % B = [nConfig, nControl] = control matrix 21 | % G = [nConfig, 1] = everything else 22 | % 23 | % 24 | % OUTPUTS: 25 | % 26 | % u = [nControl, 1] = joint torque matrix 27 | % 28 | 29 | % Temp variables to keep the code readable: 30 | T1 = H-dhRef*c; 31 | T2 = T1/D; 32 | T3 = v + ddhRef*(c*dq)^2; 33 | T4 = T2\T3; 34 | 35 | u = B\(T4 + G); 36 | 37 | end -------------------------------------------------------------------------------- /feedbackLinearization/controller.m: -------------------------------------------------------------------------------- 1 | function u = controller(z,dyn,ref) 2 | 3 | q1 = z(1); 4 | q2 = z(2); 5 | dq1 = z(3); 6 | dq2 = z(4); 7 | 8 | q = [q1;q2]; 9 | dq = [dq1;dq2]; 10 | 11 | % Compute reference trajectories in phase: 12 | p = ref.c*q; 13 | hRef = ppval(ref.pp.h, p); 14 | dhRef = ppval(ref.pp.dh, p); 15 | ddhRef = ppval(ref.pp.ddh, p); 16 | h = ref.H*q; 17 | dhdt = ref.H*dq; 18 | 19 | % Compute reference trajectory for velocity: 20 | dhRefdt = ppval(ref.pp.dhdt , p); 21 | 22 | % Linear controller: 23 | kp = ref.wn^2; 24 | kd = 2*ref.xi*ref.wn; 25 | v = kp*(hRef-h) + kd*(dhRefdt-dhdt); 26 | 27 | % D*ddq + G = B*u 28 | [D,G,B] = autoGen_acrobotDynamics(... 29 | q1,q2,dq1,dq2,... 30 | dyn.m1,dyn.m2,dyn.g,dyn.l1,dyn.l2); 31 | 32 | u = controlMap(dq, v, dhRef, ddhRef, ref.c, ref.H, D, B, G); 33 | 34 | end -------------------------------------------------------------------------------- /feedbackLinearization/drawAcrobot.m: -------------------------------------------------------------------------------- 1 | function drawAcrobot(t,z,p) 2 | 3 | clf; hold on; 4 | length = p.l1+p.l2; 5 | axis equal; axis(length*[-1,1,-1,1]); 6 | 7 | [p1,p2] = acrobotKinematics(z,p); 8 | pos = [[0;0],p1,p2]; 9 | 10 | plot(0,0,'ks','MarkerSize',25,'LineWidth',4) 11 | plot(pos(1,:),pos(2,:),'Color',[0.1, 0.8, 0.1],'LineWidth',4) 12 | plot(pos(1,:),pos(2,:),'k.','MarkerSize',50) 13 | 14 | title(['Acrobot Animation, t = ' num2str(t,2)]) 15 | 16 | drawnow; pause(0.01); 17 | 18 | end -------------------------------------------------------------------------------- /feedbackLinearization/plotAcrobot.m: -------------------------------------------------------------------------------- 1 | function plotAcrobot(t,z,u,p) 2 | 3 | [energy.total, energy.potential, energy.kinetic] = acrobotEnergy(z,p); 4 | 5 | subplot(2,3,1); hold on; 6 | plot(t,z(1,:)) 7 | xlabel('t') 8 | ylabel('q1') 9 | title('link one angle') 10 | subplot(2,3,2); hold on; 11 | plot(t,z(2,:)) 12 | xlabel('t') 13 | ylabel('q2') 14 | title('link two angle') 15 | subplot(2,3,4); hold on; 16 | plot(t,z(3,:)) 17 | xlabel('t') 18 | ylabel('dq1') 19 | title('link one rate') 20 | subplot(2,3,5); hold on; 21 | plot(t,z(4,:)) 22 | xlabel('t') 23 | ylabel('dq2') 24 | title('link two rate') 25 | 26 | subplot(2,3,3); hold on 27 | plot(t,energy.total,'k') 28 | plot(t,energy.potential,'r') 29 | plot(t,energy.kinetic,'b') 30 | legend('total','potential','kinetic') 31 | xlabel('t') 32 | ylabel('e'); 33 | title('mechanical energy') 34 | 35 | subplot(2,3,6); hold on; 36 | plot(t,u) 37 | xlabel('t') 38 | ylabel('u') 39 | title('torque between links') 40 | 41 | 42 | 43 | end -------------------------------------------------------------------------------- /mex_springPendulum/Cpp_Integrator.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MatthewPeterKelly/dscTutorials/e1e97a9be03ec146f88bd6ddd9e06db7ee52e242/mex_springPendulum/Cpp_Integrator.mexa64 -------------------------------------------------------------------------------- /mex_springPendulum/README.md: -------------------------------------------------------------------------------- 1 | SpringPendulum -- Tutorial for Matlab MEX files 2 | 3 | ============================== 4 | 5 | This code demonstrates how to use a simple MEX interface to call a simulation in C++ from Matlab. This system is a point-mass pendulum with an axial spring connecting the point mass to the hinge. There is a sinusoidal forcing torque on the system. Integration is done using 4th-order Runge Kutta, implemented in C++. Matlab is only used for setting the simulation parameters, compiling the C++ code, and then plotting the results. 6 | 7 | RUN: MAIN.m 8 | -------------------------------------------------------------------------------- /mex_springPendulum/README.txt: -------------------------------------------------------------------------------- 1 | README.txt -- SpringPendulum -- Tutorial for Matlab MEX files 2 | 3 | This code demonstrates how to use a simple MEX interface to call a simulation in C++ from Matlab. This system is a point-mass pendulum with an axial spring connecting the point mass to the hinge. There is a sinusoidal forcing torque on the system. Integration is done using 4th-order Runge Kutta, implemented in C++. Matlab is only used for setting the simulation parameters, compiling the C++ code, and then plotting the results. 4 | 5 | RUN: MAIN.m -------------------------------------------------------------------------------- /planarOrbitFallingWeight/autoGen_dynamics.m: -------------------------------------------------------------------------------- 1 | function [ddx,ddy] = autoGen_dynamics(x,y,dx,dy,m1,m2,g) 2 | %AUTOGEN_DYNAMICS 3 | % [DDX,DDY] = AUTOGEN_DYNAMICS(X,Y,DX,DY,M1,M2,G) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.2. 6 | % 19-May-2015 22:50:27 7 | 8 | t2 = x.^2; 9 | t3 = y.^2; 10 | t4 = t2+t3; 11 | t5 = 1.0./t4.^2; 12 | t6 = m1+m2; 13 | t7 = 1.0./t6; 14 | t8 = t4.^(3.0./2.0); 15 | t9 = g.*t8; 16 | t10 = dy.^2; 17 | t11 = t2.*t10; 18 | t12 = dx.^2; 19 | t13 = t3.*t12; 20 | t14 = t9+t11+t13-dx.*dy.*x.*y.*2.0; 21 | ddx = -m2.*t5.*t7.*t14.*x; 22 | if nargout > 1 23 | ddy = -m2.*t5.*t7.*t14.*y; 24 | end 25 | -------------------------------------------------------------------------------- /planarOrbitFallingWeight/dynamics.m: -------------------------------------------------------------------------------- 1 | function ds = dynamics(s,p) 2 | % ds = dynamics(s,p) 3 | % 4 | % This function computes the dynamics for a point mass that is travelling 5 | % on a frictionless plane, acted on by another point mass that is hanging 6 | % below the origin. They are connected by an inextensible massless string 7 | % 8 | 9 | x = s(1,:); 10 | y = s(2,:); 11 | dx = s(3,:); 12 | dy = s(4,:); 13 | 14 | [ddx,ddy] = autoGen_dynamics(x,y,dx,dy,p.m1,p.m2,p.g); 15 | 16 | ds = [dx;dy;ddx;ddy]; 17 | 18 | end -------------------------------------------------------------------------------- /pwPoly/DEMO_fitPoly5.m: -------------------------------------------------------------------------------- 1 | % DEMO_fitPoly5.m 2 | % 3 | % Demonstrates useing pwPoly5 to approximate a data set 4 | % 5 | 6 | clc; clear; 7 | 8 | nKnot = 6; %Number of knot points for piecewise-polynomial 9 | 10 | % Create a test data set 11 | t = linspace(-pi,pi,100); 12 | T = linspace(-pi,pi,nKnot); 13 | x = sin(1.5*t) + 1.8*exp(-1.5*t.^2); 14 | x = x + 0.04*randn(size(t)); % Add some noise: 15 | 16 | % Find the best-fit polynomial 17 | P = fitPoly5(T,t,x); 18 | 19 | % Evaluate the fit: 20 | xFit = pwPoly5(T,P,t); 21 | 22 | % Plot the solution: 23 | figure(5); clf; hold on; 24 | plot(t,xFit,'r-') 25 | plot(t,x,'k.') 26 | plot(T,P(1,:),'bo','MarkerSize',15) 27 | xlabel('t') 28 | ylabel('x') 29 | title('test function') -------------------------------------------------------------------------------- /pwPoly/DEMO_fitPoly5p.m: -------------------------------------------------------------------------------- 1 | % DEMO_fitPoly5.m 2 | % 3 | % Demonstrates useing pwPoly5 to approximate a periodic function 4 | % 5 | 6 | clc; clear; 7 | 8 | %How many knot points to use on the curve 9 | nKnot = 5; 10 | 11 | % Build up a test function: (polar coordinates) 12 | t = linspace(-pi,pi,200); 13 | T = linspace(-pi,pi,nKnot); 14 | r = 2+sin(2*t)+0.6*cos(4*t+2.05); 15 | r = r + 0.05*randn(size(t)); % Add some noise: 16 | 17 | % Fit the piecewise-cubic function 18 | P = fitPoly5p(T,t,r); 19 | 20 | % Evaluate the fit: 21 | rFit = pwPoly5(T,P,t); 22 | 23 | % Convert to cartesian for plotting: 24 | [x,y] = pol2cart(t,r); 25 | [xFit,yFit] = pol2cart(t,rFit); 26 | [xKnot,yKnot] = pol2cart(T,P(1,:)); 27 | 28 | % Plot the solution against data set: 29 | figure(5); clf; hold on; 30 | plot(xFit,yFit,'r-') 31 | plot(x,y,'k.') 32 | plot(xKnot,yKnot,'bo','MarkerSize',15) 33 | xlabel('x') 34 | ylabel('y') 35 | title('periodic test function') -------------------------------------------------------------------------------- /pwPoly/DEMO_fitSpline.m: -------------------------------------------------------------------------------- 1 | % DEMO_fitSpline.m 2 | % 3 | % Fit a spline to data, which can then be evaluated by ppval 4 | 5 | 6 | tSpan = [0,10]; 7 | nData = 100; 8 | nKnot = 10; 9 | 10 | tData = linspace(tSpan(1),tSpan(2),nData); 11 | xData = sin(tData); 12 | yData = cos(tData); 13 | 14 | tKnot = linspace(tSpan(1),tSpan(2),nKnot); 15 | 16 | pp = fitSpline(tData, [xData;yData], tKnot); 17 | 18 | zSpline = ppval(pp,tData); 19 | 20 | figure(15); clf; 21 | 22 | subplot(2,1,1); hold on; 23 | plot(tData,xData,'ko') 24 | plot(tData,zSpline(1,:),'b-') 25 | legend('data','spline'); 26 | 27 | subplot(2,1,2); hold on; 28 | plot(tData,yData,'ko') 29 | plot(tData,zSpline(2,:),'b-') 30 | legend('data','spline'); -------------------------------------------------------------------------------- /pwPoly/DEMO_pwPoly2.m: -------------------------------------------------------------------------------- 1 | % DEMO_pwPoly2.m 2 | % 3 | % Demonstrate usage of pwPoly2 for quadratic interpolation 4 | 5 | clc; clear; 6 | 7 | tGrid = linspace(0,7,15); 8 | xGrid = [sin(tGrid); cos(tGrid)]; 9 | 10 | t = linspace(tGrid(1),tGrid(end),200); 11 | x = pwPoly2(tGrid,xGrid,t); 12 | 13 | 14 | figure(1); clf; 15 | subplot(2,1,1); hold on; 16 | plot(t,sin(t),'o') 17 | plot(t,x(1,:)) 18 | legend('interp','analytic'); 19 | subplot(2,1,2); hold on; 20 | plot(t,cos(t),'o') 21 | plot(t,x(2,:)) 22 | legend('interp','analytic'); -------------------------------------------------------------------------------- /pwPoly/DEMO_pwPoly4.m: -------------------------------------------------------------------------------- 1 | % DEMO_pwPoly4.m 2 | % 3 | % Demonstrate usage of pwPoly4 for quadratic interpolation 4 | 5 | clc; clear; 6 | 7 | tGrid = linspace(0,7,15); 8 | xGrid = sin(tGrid); 9 | dxGrid = cos(tGrid); 10 | 11 | t = linspace(tGrid(1),tGrid(end),200); 12 | x = pwPoly4(tGrid,xGrid,dxGrid,t); 13 | 14 | 15 | figure(2); clf; hold on; 16 | plot(t,sin(t),'o') 17 | plot(t,x) 18 | legend('interp','analytic'); -------------------------------------------------------------------------------- /pwPoly/DEMO_pwPoly5.m: -------------------------------------------------------------------------------- 1 | % DEMO_smoothCurve.m 2 | % 3 | % This script demonstrates how to use smoothCurve to create a piecewise 4 | % smooth curve that is twice differentiable 5 | % 6 | 7 | % The number of knots is the number of segments + 1 8 | nKnot = 5; 9 | 10 | % P = function [value; slope; curvature] at knot points 11 | P = [... 12 | 2*(1-2*rand(1,nKnot)); %Fucntion value 13 | 1*randn(1,nKnot); %Function slope 14 | 5*randn(1,nKnot)]; %Function curvature 15 | 16 | % T is the location of each knot point: 17 | T = cumsum(0.8+0.2*rand(1,nKnot)); 18 | 19 | % Evaluate the piecewise function: 20 | t = linspace(T(1),T(end),250); 21 | [x,dx,ddx] = pwPoly5(T,P,t); 22 | 23 | % Plot the results: 24 | figure(87); clf; 25 | subplot(3,1,1); hold on; 26 | plot(t,x,'k-'); 27 | plot(T,P(1,:),'bo','MarkerSize',10) 28 | ylabel('x') 29 | title('Test Function') 30 | subplot(3,1,2); hold on; 31 | plot(t,dx,'k-'); 32 | plot(T,P(2,:),'bo','MarkerSize',10) 33 | ylabel('dx') 34 | subplot(3,1,3); hold on; 35 | plot(t,ddx,'k-'); 36 | plot(T,P(3,:),'bo','MarkerSize',10) 37 | ylabel('ddx') 38 | xlabel('t') 39 | 40 | 41 | -------------------------------------------------------------------------------- /pwPoly/Derive_pwPoly2.m: -------------------------------------------------------------------------------- 1 | % Derive_pwPoly2.m 2 | % 3 | % This script derives the equations for quadratic interpolation, where the 4 | % function is defined by its endpoint and midpoint values. 5 | % 6 | 7 | clc; clear; 8 | 9 | 10 | syms a b c 'real' %Coefficients 11 | syms x 'real' %Independant variable 12 | syms yLow yMid yUpp 'real' %Values at endpoints and midpoints 13 | 14 | % Assume that: 15 | % xLow = -1 16 | % xMid = 0 17 | % xUpp = 1 18 | % 19 | % y = a*x*x + b*x + c; 20 | 21 | eqnLow = yLow - (a - b + c); %subs x = -1 22 | eqnMid = yMid - (c); %subs x = 0 23 | eqnUpp = yUpp - (a + b + c); %subs x = 1 24 | 25 | eqns = [eqnLow; eqnMid; eqnUpp]; 26 | vars = [a;b;c]; 27 | [A,b] = equationsToMatrix(eqns,vars); 28 | soln = A\b; 29 | 30 | fprintf('y = a*x^2 + b*x + c \n\n'); 31 | fprintf('y(-1) = yLow \n') 32 | fprintf('y(0) = yMid \n') 33 | fprintf('y(1) = yUpp \n\n') 34 | fprintf(['a = ' char(soln(1)) '\n']); 35 | fprintf(['b = ' char(soln(2)) '\n']); 36 | fprintf(['c = ' char(soln(3)) '\n']); 37 | -------------------------------------------------------------------------------- /pwPoly/Derive_pwPoly4.m: -------------------------------------------------------------------------------- 1 | % Derive_pwPoly2.m 2 | % 3 | % This script derives the equations for quartic interpolation, where the 4 | % function is defined by its value and slope at the endpoitns, and the 5 | % value at the midpoint 6 | % 7 | 8 | clc; clear; 9 | 10 | 11 | syms a b c d e 'real' %Coefficients 12 | syms x 'real' %Independant variable 13 | syms yLow dyLow yMid yUpp dyUpp 'real' %Values at endpoints and midpoints 14 | 15 | % Assume that: 16 | % xLow = -1 17 | % xMid = 0 18 | % xUpp = 1 19 | % 20 | 21 | y = simplify(e + x*(d + x*(c + x*(b + x*a)))); 22 | dy = simplify(diff(y,x)); 23 | 24 | eqnLow = yLow - subs(y,x,sym(-1)); 25 | eqnLowD = dyLow - subs(dy,x,sym(-1)); 26 | eqnMid = yMid - subs(y,x,sym(0)); 27 | eqnUpp = yUpp - subs(y,x,sym(1)); 28 | eqnUppD = dyUpp - subs(dy,x,sym(1)); 29 | 30 | eqns = [eqnLow; eqnLowD; eqnMid; eqnUpp; eqnUppD]; 31 | vars = [a;b;c;d;e]; 32 | [A,b] = equationsToMatrix(eqns,vars); 33 | soln = simplify(A\b); 34 | 35 | disp(['a = ' char(soln(1))]); 36 | disp(['b = ' char(soln(2))]); 37 | disp(['c = ' char(soln(3))]); 38 | disp(['d = ' char(soln(4))]); 39 | disp(['e = ' char(soln(5))]); -------------------------------------------------------------------------------- /pwPoly/README.md: -------------------------------------------------------------------------------- 1 | # Piece-wise Polynomial (pwPoly) 2 | 3 | A collection of functions for working with piece-wise polynomial representations of functions in Matlab. 4 | 5 | - pwPoly2 = quadratic spline. useful if function value is known at mid-points 6 | - pwPoly3 = cubic spline. Wrappers for matlab's pchip, spline, ppval, and related commands 7 | - pwPoly4 = quartic spline. Curiosity, not too useful. first-order continuous at knot points, with function value defined at the mid-point 8 | - pwPoly5 = 5th-order spline, second-order smooth at knot points. These functions are the most developed in the library. 9 | 10 | A few demonstrations (DEMO_*.m) are provided to show how to use the program 11 | 12 | -------------------------------------------------------------------------------- /pwPoly/autoGen_pwPoly5.m: -------------------------------------------------------------------------------- 1 | function [x,dx,ddx] = autoGen_pwPoly5(t,x0,dx0,ddx0,x1,dx1,ddx1) 2 | %AUTOGEN_PWPOLY5 3 | % [X,DX,DDX] = AUTOGEN_PWPOLY5(T,X0,DX0,DDX0,X1,DX1,DDX1) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.2. 6 | % 15-Apr-2015 11:55:56 7 | 8 | t2 = t-1.0; 9 | t3 = t2.^2; 10 | t4 = t3.^2; 11 | t5 = t.^2; 12 | t6 = t5.^2; 13 | t7 = dx1.*(1.0./5.0); 14 | t8 = t7-x1; 15 | t9 = dx0.*(1.0./5.0); 16 | t10 = t9+x0; 17 | t11 = ddx0.*(1.0./2.0e1); 18 | t12 = dx0.*(2.0./5.0); 19 | t13 = t11+t12+x0; 20 | t14 = ddx1.*(1.0./2.0e1); 21 | t16 = dx1.*(2.0./5.0); 22 | t15 = t14-t16+x1; 23 | x = t.*t4.*t10.*5.0+t2.*t6.*t8.*5.0-t2.*t4.*x0+t.*t6.*x1+t.*t3.*t5.*t15.*1.0e1-t2.*t3.*t5.*t13.*1.0e1; 24 | if nargout > 1 25 | dx = t4.*t10.*5.0+t6.*t8.*5.0-t4.*x0.*5.0+t6.*x1.*5.0-t3.*t5.*t13.*3.0e1+t3.*t5.*t15.*3.0e1+t.*t2.*t3.*t10.*2.0e1+t.*t2.*t5.*t8.*2.0e1-t.*t2.*t3.*t13.*2.0e1+t.*t5.*t15.*(t.*2.0-2.0).*1.0e1; 26 | end 27 | if nargout > 2 28 | ddx = ddx0-ddx0.*t.*9.0+ddx1.*t.*3.0+ddx0.*t5.*1.8e1-ddx1.*t5.*1.2e1-dx0.*t.*3.6e1-dx1.*t.*2.4e1+dx0.*t5.*9.6e1+dx1.*t5.*8.4e1-t.*x0.*6.0e1+t.*x1.*6.0e1+t5.*x0.*1.8e2-t5.*x1.*1.8e2-ddx0.*t.*t5.*1.0e1+ddx1.*t.*t5.*1.0e1-dx0.*t.*t5.*6.0e1-dx1.*t.*t5.*6.0e1-t.*t5.*x0.*1.2e2+t.*t5.*x1.*1.2e2; 29 | end 30 | -------------------------------------------------------------------------------- /smoothing/exponentialSmoothing/README.md: -------------------------------------------------------------------------------- 1 | Function Smoothing via Exponentials 2 | 3 | ================================== 4 | 5 | Updated: October 25, 2013 6 | 7 | Written by Matthew Kelly 8 | Cornell University 9 | 10 | Functions: 11 | - SmoothAbs 12 | - SmoothAbsFancy 13 | - SmoothBnd 14 | - SmoothRamp 15 | - SmoothMax 16 | 17 | Each function is a smooth version of a simple function. 18 | In each case, the output is full continuous. 19 | 20 | In some cases, very small values of alpha can cause an 21 | overflow. If this occurs, then the functions will return 22 | the non-smooth version of the function. 23 | 24 | Each function has a testing script that shows the effect 25 | that different values of the smoothing parameter have. 26 | -------------------------------------------------------------------------------- /smoothing/exponentialSmoothing/README.txt: -------------------------------------------------------------------------------- 1 | smoothVersionExp 2 | 3 | Updated: October 25, 2013 4 | 5 | Written by Matthew Kelly 6 | Cornell University 7 | 8 | Functions: 9 | SmoothAbs 10 | SmoothAbsFancy 11 | SmoothBnd 12 | SmoothRamp 13 | SmoothMax 14 | 15 | Each function is a smooth version of a simple function. 16 | In each case, the output is full continuous. 17 | 18 | In some cases, very small values of alpha can cause an 19 | overflow. If this occurs, then the functions will return 20 | the non-smooth version of the function. 21 | 22 | Each function has a testing script that shows the effect 23 | that different values of the smoothing parameter have. -------------------------------------------------------------------------------- /smoothing/exponentialSmoothing/SmoothAbs.m: -------------------------------------------------------------------------------- 1 | function lxl = SmoothAbs(x,alpha) 2 | % 3 | %lXl = SmoothAbs(x,alpha) 4 | % 5 | %This function is a smooth version of absolute value. There are two 6 | %versions of the function. If called with two arguments it will use 7 | %hyperbolic tangent smoothing, and if called with 4 arguments it will use a 8 | %fancy smoothing that allows for the slope to be adjusted on each side. 9 | % 10 | %INPUTS: 11 | % x = a vector of inputs to smooth 12 | % alpha = smoothing parameter, alpha > 0. 13 | % Small alpha (1e-4) => no smoothing 14 | % large alpha (1e-1) => heavy smoothing 15 | % (alpha sample values assumes x on the order of 1) 16 | %OUTPUTS: 17 | % lxl = the smooth version of x 18 | % 19 | % 20 | % Written by Matthew Kelly 21 | % October 2013 22 | % Cornell University 23 | % 24 | lxl = x.*tanh(x/alpha); 25 | 26 | end -------------------------------------------------------------------------------- /smoothing/exponentialSmoothing/SmoothBnd.m: -------------------------------------------------------------------------------- 1 | function xBnd = SmoothBnd(x,alpha,Bnd) 2 | % 3 | % xBnd = SmoothBnd(x,alpha,Bnd) 4 | % 5 | % This function is used to smoothly bound the input x; 6 | % 7 | % INPUTS: 8 | % x = a vector of real numbers to apply bounds 9 | % alpha = a positive smoothing parameter. Small values correspond to 10 | % little smoothing 11 | % Bnd = the desired bounds of the function, expressed as a 2-element row 12 | % vector. If ommitted it will default to [0,1]; 13 | % 14 | % Written by Matthew Kelly 15 | % October 2013 16 | % Cornell University 17 | % 18 | 19 | if nargin==2 20 | Low = 0; 21 | Upp = 1; 22 | else 23 | Low = Bnd(1); 24 | Upp = Bnd(2); 25 | end 26 | 27 | infTest1 = exp(max(x-Low)/alpha); 28 | infTest2 = exp(max(-(x-Upp))/alpha); 29 | 30 | if isinf(infTest1) || isinf(infTest2) %Then there is a sharp transition 31 | xBnd = x; 32 | xBnd(xUpp) = Upp; 34 | else 35 | %Apply bounding to each part of the input: 36 | xLow = Low + alpha*log(exp((x-Low)/alpha)+1); 37 | xUpp = Upp - alpha*log(exp(-(x-Upp)/alpha)+1); 38 | 39 | %Combine: 40 | xBnd = xLow + xUpp - x; 41 | end 42 | 43 | end 44 | 45 | -------------------------------------------------------------------------------- /smoothing/exponentialSmoothing/SmoothMax.m: -------------------------------------------------------------------------------- 1 | function xMax = SmoothMax(x,alpha) 2 | % 3 | % xMax = SmoothMax(x, alpha) 4 | % 5 | % This function is a smooth version of the max(x) function, commonly known 6 | % as the 'soft-max' function. In the limit as the smoothing parameter goes 7 | % to zero, this function will return max(x). In the limit as the smoothing 8 | % parameter goes to infinity, this will return sum(x). 9 | % 10 | %INPUTS: 11 | % x = a vector or matrix of inputs 12 | % alpha = a smoothing parameter. Asymtotes: 0->max(x), inf->sum(x) 13 | % 14 | %OUTPUTS: 15 | % xMax = the smooth maximum of x 16 | % 17 | % 18 | % Written by Matthew Kelly 19 | % October 2013 20 | % Cornell University 21 | % 22 | 23 | if nargin==1 24 | alpha = 1; 25 | end 26 | 27 | xMax = alpha*log(sum(exp(x/alpha))); 28 | 29 | if isinf(xMax) 30 | %Then the scaling was too sharp 31 | xMax = max(x); 32 | end 33 | 34 | end -------------------------------------------------------------------------------- /smoothing/exponentialSmoothing/SmoothRamp.m: -------------------------------------------------------------------------------- 1 | function y = SmoothRamp(x,alpha) 2 | 3 | %y = SmoothRamp(x,alpha) 4 | % 5 | % This function is used to make a smooth version of the ramp function 6 | % 7 | %INPUTS: 8 | % x = a vector of real numbers to be smoothed 9 | % alpha = a positive smoothing parameter. Small values of alpha correspond 10 | % to little smoothing, large values correspond to heavy smoothing. 11 | % If alpha is too small, no smoothing will be done. 12 | % 13 | % Written by Matthew Kelly 14 | % October 2013 15 | % Cornell University 16 | % 17 | 18 | %Check for infinities 19 | infTest = exp(max(x)/alpha); 20 | 21 | if isinf(infTest) %Then there is a sharp transition 22 | y = x; 23 | y(x<0) = 0; 24 | else %do smoothing 25 | y = alpha*log(exp(x/alpha)+1); 26 | end 27 | 28 | end -------------------------------------------------------------------------------- /smoothing/exponentialSmoothing/TEST_SmoothAbs.m: -------------------------------------------------------------------------------- 1 | %TEST_SmoothAbs 2 | % 3 | % This script plots the output of SmoothAbs for a few values of alpha, over 4 | % the domain [-1,1] 5 | % 6 | % Written by Matthew Kelly 7 | % October 2013 8 | % Cornell University 9 | % 10 | 11 | t = linspace(-1,1,1000); 12 | alpha = [0.02,0.1,0.5]; 13 | N=length(alpha); 14 | 15 | figure(104); clf; hold on; 16 | for i=1:N 17 | x1 = SmoothAbs(t,alpha(i)); 18 | 19 | subplot(N,1,i); hold on; 20 | plot(t,x1,'b-','LineWidth',2) 21 | plot(t,abs(t),'k:') 22 | title(['Alpha = ' num2str(alpha(i))],'FontSize',14); 23 | end -------------------------------------------------------------------------------- /smoothing/exponentialSmoothing/TEST_SmoothAbsFancy.m: -------------------------------------------------------------------------------- 1 | %TEST_SmoothAbsFancy 2 | % 3 | % This function plots the output of SmoothAbsFancy for a few different 4 | % values of alpha (smoothin parameter) on the domain [-1,1]. See 5 | % SmoothAbsFancy for more details. 6 | % 7 | % Written by Matthew Kelly 8 | % October 2013 9 | % Cornell University 10 | % 11 | 12 | t = linspace(-1,1,1000); 13 | 14 | slope1 = 0.5; 15 | slope2 = 1; 16 | 17 | alpha = [0.001,0.01,0.1]; 18 | N=length(alpha); 19 | 20 | figure(103); clf; hold on; 21 | for i=1:N 22 | x1 = SmoothAbsFancy(t,alpha(i),slope1,slope2); 23 | 24 | subplot(N,1,i); hold on; 25 | plot(t,x1,'b-','LineWidth',2) 26 | plot(t(t<0),-slope1*t(t<0),'k:') 27 | plot(t(t>=0),slope2*t(t>=0),'k:') 28 | title(['Alpha = ' num2str(alpha(i))],'FontSize',14); 29 | end -------------------------------------------------------------------------------- /smoothing/exponentialSmoothing/TEST_SmoothBnd.m: -------------------------------------------------------------------------------- 1 | %TEST_SmoothBnd 2 | % 3 | % This function plots the output of SmoothBnd for a few sample values of 4 | % the smoothing parameter alphaover the domain [-1,1] 5 | % 6 | % Written by Matthew Kelly 7 | % October 2013 8 | % Cornell University 9 | % 10 | 11 | t = linspace(-1,2,1000); 12 | Bnd = [0,1]; 13 | alpha = [0.01,0.05,0.2]; 14 | N=length(alpha); 15 | 16 | 17 | figure(102); clf; hold on; 18 | for i=1:N 19 | x1 = SmoothBnd(t,alpha(i),[0,1]); 20 | 21 | subplot(N,1,i); hold on; 22 | plot(t,x1,'b-','LineWidth',2) 23 | c1 = t=Bnd(2); c3 = ~c1&~c2; 24 | plot(t(c1),Bnd(1),'k:') 25 | plot(t(c2),Bnd(2),'k:') 26 | plot(t(c3),t(c3),'k:') 27 | title(['Alpha = ' num2str(alpha(i))],'FontSize',14); 28 | end -------------------------------------------------------------------------------- /smoothing/exponentialSmoothing/TEST_SmoothRamp.m: -------------------------------------------------------------------------------- 1 | %TEST_SmoothRamp 2 | % 3 | % This script plots the output of SmoothRamp for a few values of alpha, on 4 | % the domain [-1,1] 5 | % 6 | % Written by Matthew Kelly 7 | % October 2013 8 | % Cornell University 9 | % 10 | 11 | t = linspace(-1,1,1000); 12 | alpha = [0.02,0.05,0.2]; 13 | 14 | figure(101); clf; hold on; 15 | for i=1:length(alpha) 16 | x1 = SmoothRamp(t,alpha(i)); 17 | 18 | subplot(N,1,i); hold on; 19 | plot(t,x1,'LineWidth',2) 20 | plot(t(t<0),0,'k:') 21 | plot(t(t>=0),t(t>=0),'k:') 22 | title(['Alpha = ' num2str(alpha(i))],'FontSize',14); 23 | end -------------------------------------------------------------------------------- /smoothing/polynomialSmoothing/README.md: -------------------------------------------------------------------------------- 1 | Function Smoothing via Polynomials 2 | 3 | =========================================== 4 | 5 | Updated: October 8, 2013 6 | 7 | Written by Matthew Kelly 8 | Cornell University 9 | 10 | Functions: 11 | - SmoothAbs 12 | - SmoothBnd 13 | - SmoothRamp 14 | 15 | Each function is a smooth version of a simple function. 16 | The user can specify the degree of smoothness, up to 8th order smooth at the transitions. The user can also specify a degree of 0, causing the function to return the original non-smooth function. 17 | 18 | Each function has a testing script that shows the effect 19 | that different values of the smoothing parameter have. 20 | 21 | There are a few other functions in this directory: 22 | 23 | - SolveCoeff -- symbolically finds the coefficients of a polynomial to satisfy a given set of constraints. It will return an error if the constraints are not well posed. It is used to find the coefficients of the polynomials that are used for smoothing. 24 | 25 | - writeSmoothRamp -- a function to write SmoothRamp 26 | 27 | - writeSmoothAbs -- a function to write SmoothAbs 28 | 29 | -------------------------------------------------------------------------------- /smoothing/polynomialSmoothing/README.txt: -------------------------------------------------------------------------------- 1 | smoothVersionPoly 2 | 3 | Updated: October 8, 2013 4 | 5 | Written by Matthew Kelly 6 | Cornell University 7 | 8 | Functions: 9 | SmoothAbs 10 | SmoothBnd 11 | SmoothRamp 12 | 13 | Each function is a smooth version of a simple function. 14 | The user can specify the degree of smoothness, up to 8th order smooth at the transitions. The user can also specify a degree of 0, causing the function to return the original non-smooth function. 15 | 16 | Each function has a testing script that shows the effect 17 | that different values of the smoothing parameter have. 18 | 19 | There are a few other functions in this directory: 20 | 21 | SolveCoeff -- symbolically finds the coefficients of a polynomial to satisfy a given set of constraints. It will return an error if the constraints are not well posed. It is used to find the coefficients of the polynomials that are used for smoothing. 22 | 23 | writeSmoothRamp -- a function to write SmoothRamp 24 | 25 | writeSmoothAbs -- a function to write SmoothAbs 26 | -------------------------------------------------------------------------------- /smoothing/polynomialSmoothing/TEST_smoothAbs.m: -------------------------------------------------------------------------------- 1 | %TEST_smoothAbs 2 | 3 | x = linspace(-1,1,1000); 4 | 5 | alpha = [0.05,0.2,0.8]; 6 | order = [1,3,8]; 7 | 8 | N = length(alpha); 9 | M = length(order); 10 | 11 | figure(111); clf; 12 | index = 0; 13 | for i=1:N 14 | for j=1:M 15 | index=index+1; 16 | subplot(N,M,index); hold on; 17 | y = smoothAbs(x,alpha(i),order(j)); 18 | plot(x,y,'b-','LineWidth',2) 19 | plot(x,abs(x),'k--','LineWidth',1); 20 | title(['alpha = ' num2str(alpha(i)) ', order = ' num2str(order(j))]... 21 | ,'FontSize',14); 22 | end 23 | end 24 | 25 | 26 | -------------------------------------------------------------------------------- /smoothing/polynomialSmoothing/TEST_smoothBnd.m: -------------------------------------------------------------------------------- 1 | %TEST_smoothBnd 2 | 3 | x = linspace(-3,4,1000); 4 | Bnd = [-1,2]; 5 | 6 | alpha = [0.2,0.5,1.5]; 7 | order = [1,3,8]; 8 | 9 | N = length(alpha); 10 | M = length(order); 11 | 12 | figure(113); clf; 13 | index = 0; 14 | for i=1:N 15 | for j=1:M 16 | index=index+1; 17 | subplot(N,M,index); hold on; 18 | y = smoothBnd(x,Bnd,alpha(i),order(j)); 19 | plot(x,y,'b-','LineWidth',2) 20 | c1 = x=Bnd(2); 22 | c2 = (~c1)&(~c3); 23 | plot(x(c1),Bnd(1)*ones(size(x(c1))),'k--','LineWidth',1); 24 | plot(x(c2),x(c2),'k--','LineWidth',1); 25 | plot(x(c3),Bnd(2)*ones(size(x(c3))),'k--','LineWidth',1); 26 | title(['alpha = ' num2str(alpha(i)) ', order = ' num2str(order(j))]... 27 | ,'FontSize',14); 28 | end 29 | end 30 | 31 | -------------------------------------------------------------------------------- /smoothing/polynomialSmoothing/TEST_smoothRamp.m: -------------------------------------------------------------------------------- 1 | %TEST_smoothRamp 2 | 3 | x = linspace(-1,1,1000); 4 | 5 | alpha = [0.05,0.2,0.8]; 6 | order = [1,3,8]; 7 | 8 | N = length(alpha); 9 | M = length(order); 10 | 11 | figure(112); clf; 12 | index = 0; 13 | for i=1:N 14 | for j=1:M 15 | index=index+1; 16 | subplot(N,M,index); hold on; 17 | y = smoothRamp(x,alpha(i),order(j)); 18 | plot(x,y,'b-','LineWidth',2) 19 | plot(x(x<0),0*x(x<0),'k--','LineWidth',1); 20 | plot(x(x>=0),x(x>=0),'k--','LineWidth',1); 21 | title(['alpha = ' num2str(alpha(i)) ', order = ' num2str(order(j))]... 22 | ,'FontSize',14); 23 | end 24 | end 25 | 26 | 27 | -------------------------------------------------------------------------------- /toppling_stick/FIGURE_PointMassTopple.m: -------------------------------------------------------------------------------- 1 | %FIGURE_PointMassTopple 2 | 3 | %This is still experimental - does not produce a meaningful plot yet 4 | 5 | P.m = 1; P.g = 1; P.L = 1; 6 | 7 | small = 1e-6; 8 | 9 | n = 50; 10 | MoI = logspace(-3,0,n); 11 | Th = linspace(0+small,pi/2-small,n); 12 | [MOMENT,THETA] = meshgrid(MoI,Th); 13 | FRICTION = zeros(size(MOMENT)); %Critical value of mu 14 | 15 | for i=1:size(MOMENT,1) 16 | for j=1:size(MOMENT,2) 17 | P.I = MOMENT(i,j); 18 | th = THETA(i,j); 19 | FRICTION(i,j) = topple_criticalMu(th,P); 20 | end 21 | end 22 | 23 | [N,M] = size(MOMENT); 24 | moi = reshape(MOMENT,N*M,1); 25 | mu = reshape(FRICTION,N*M,1); 26 | th = reshape(THETA,N*M,1); 27 | 28 | figure(700); clf; hold on; 29 | idx = mu>0; 30 | plot3(moi(idx),th(idx),mu(idx),'r.'); 31 | idx = mu<0; 32 | plot3(moi(idx),th(idx),-mu(idx),'b.'); 33 | set(gca,'Xscale','log') 34 | set(gca,'Yscale','linear') 35 | set(gca,'Zscale','log') 36 | xlabel('moi') 37 | ylabel('angle') 38 | zlabel('mu') -------------------------------------------------------------------------------- /toppling_stick/README.md: -------------------------------------------------------------------------------- 1 | Event-based simulation of a pencil falling over. 2 | 3 | ================================================ 4 | 5 | This simulation was made after reading Tad McGeer's 1989 paper: "wobbling, toppling, and forces of contact". It is designed to study the effects that moment of inertia and contaact friction have on the outcome of the simulation. 6 | 7 | --------------------------------------------------- 8 | 9 | MAIN.m -- Runs a simulation 10 | 11 | FIGURE*.m -- Batch run of simulation to generate a figure. 12 | -------------------------------------------------------------------------------- /toppling_stick/README.md~: -------------------------------------------------------------------------------- 1 | Event-based simulation of a pencil falling over. 2 | 3 | ================================================ 4 | 5 | This simulation was made after reading Tad McGeer's 1989 paper: "wobbling, toppling, and forces of contact". It is designed to study the effects that moment of inertia and contaact friction have on the outcome of the simulation. 6 | 7 | --------------------------------------------------- 8 | 9 | MAIN.m -- Runs a simulation 10 | 11 | FIGURE*.m -- Batch run of simulation to generate a figure. 12 | -------------------------------------------------------------------------------- /toppling_stick/SlideDistanceLimit.m: -------------------------------------------------------------------------------- 1 | %SlideDistanceLimit 2 | 3 | %w = sqrt(2*g / 3*L); 4 | %cos(th) = 2/3; 5 | %sin(th) = sqrt(1-cos(th)^2); 6 | % 7 | %rg = [L*sin(th); L*cos(th)]; 8 | % 9 | % Let L=1, g = 1 10 | % 11 | 12 | costh = 2/3; 13 | sinth = sqrt(1-costh^2); 14 | w = sqrt(2/3); 15 | 16 | rg = [-sinth; costh]; 17 | drg = [-w*costh; -w*sinth]; 18 | 19 | %0 = rg(2) + drg(2)*t + -0.5*t^2 20 | P = [-0.5,drg(2),rg(2)]; 21 | t = max(roots(P)); 22 | 23 | d = rg(1) + drg(1)*t; 24 | 25 | SlipDist = d+1; 26 | 27 | %%%%%%%%%%%%%% 28 | 29 | syms L g 30 | 31 | L=sym(1); 32 | g = sym(1); 33 | 34 | costh = sym(2/3); 35 | sinth = sqrt(sym(5/9)); 36 | w = sqrt((2*g)/(3*L)); 37 | 38 | rg = [-L*sinth; L*costh]; 39 | drg = -L*w*[costh; sinth]; 40 | a = -g/2; 41 | b = drg(2); 42 | c = rg(2); 43 | t = simplify((-b - sqrt(b^2-4*a*c))/(2*a)); 44 | 45 | d = simplify(rg(1) + drg(1)*t); 46 | 47 | SlipDist = simplify(d + L); 48 | 49 | pretty(SlipDist) 50 | SlipDist 51 | double(SlipDist) 52 | 53 | 54 | -------------------------------------------------------------------------------- /toppling_stick/dynamics_flight.m: -------------------------------------------------------------------------------- 1 | function [dZ, E] = dynamics_flight(~,Z,P) 2 | 3 | % DO NOT EDIT 4 | % This function was automatically generated 5 | 6 | th = Z(1,:); 7 | x = Z(2,:); 8 | y = Z(3,:); 9 | dth = Z(4,:); 10 | dx = Z(5,:); 11 | dy = Z(6,:); 12 | 13 | m = P.m; 14 | g = P.g; 15 | L = P.L; 16 | I = P.I; 17 | 18 | dZ = zeros(size(Z)); 19 | dZ(1,:) = dth; 20 | dZ(2,:) = dx; 21 | dZ(3,:) = dy; 22 | dZ(4,:) = 0; 23 | dZ(5,:) = -L.*dth.^2.*sin(th); 24 | dZ(6,:) = L.*dth.^2.*cos(th) - g; 25 | 26 | % Energy: 27 | E = zeros(2,length(th)); % [potential; kinetic] 28 | E(1,:) = g.*m.*(y + L.*cos(th)); 29 | E(2,:) = (m.*((dx - L.*dth.*cos(th)).^2 + (dy - L.*dth.*sin(th)).^2))./2 + (I.*dth.^2)./2; 30 | 31 | end 32 | -------------------------------------------------------------------------------- /toppling_stick/dynamics_hinge.m: -------------------------------------------------------------------------------- 1 | function [dZ, C, E] = dynamics_hinge(~,Z,P) 2 | 3 | % DO NOT EDIT 4 | % This function was automatically generated 5 | 6 | th = Z(1,:); 7 | dth = Z(2,:); 8 | 9 | m = P.m; 10 | g = P.g; 11 | L = P.L; 12 | I = P.I; 13 | 14 | dZ = zeros(size(Z)); 15 | dZ(1,:) = dth; 16 | dZ(2,:) = (L.*g.*m.*sin(th))./(I + L.^2.*m); 17 | 18 | % Contact Forces: 19 | C = zeros(2,length(th)); % [horizontal; vertical] 20 | C(1,:) = (L.*m.*sin(th).*(I.*dth.^2 + L.^2.*dth.^2.*m - L.*g.*m.*cos(th)))./(I + L.^2.*m); 21 | C(2,:) = (m.*(I.*g - L.^3.*dth.^2.*m.*cos(th) + L.^2.*g.*m.*cos(th).^2 - I.*L.*dth.^2.*cos(th)))./(I + L.^2.*m); 22 | 23 | % Energy: 24 | E = zeros(2,length(th)); % [potential; kinetic] 25 | E(1,:) = L.*g.*m.*cos(th); 26 | E(2,:) = (m.*(L.^2.*dth.^2.*cos(th).^2 + L.^2.*dth.^2.*sin(th).^2))./2 + (I.*dth.^2)./2; 27 | 28 | end 29 | -------------------------------------------------------------------------------- /toppling_stick/events_flight.m: -------------------------------------------------------------------------------- 1 | function [value, isterminal, direction] = events_flight(~,Z,P) 2 | 3 | LL = P.FullLength; 4 | th = Z(1,:); 5 | y0 = Z(3,:); 6 | y2 = y0 + LL*cos(th); 7 | 8 | %%%% CONSTRAINTS %%%% 9 | % 1) y0 > 0 10 | % 2) y2 > 0 11 | 12 | % As a rule, each constraint will be satisfied if it's event function value 13 | % is positive. This makes things easier at the FSM level. 14 | n = length(th); 15 | value = zeros(2,n); 16 | isterminal = true(size(value)); 17 | direction = -ones(size(value)); 18 | 19 | value(1,:) = y0; 20 | value(2,:) = y2; 21 | 22 | end -------------------------------------------------------------------------------- /toppling_stick/events_hinge.m: -------------------------------------------------------------------------------- 1 | function [value, isterminal, direction] = events_hinge(~,Z,P) 2 | 3 | [~, contacts] = dynamics_hinge([],Z,P); 4 | u = P.u; 5 | th = Z(1,:); 6 | H = contacts(1,:); 7 | V = contacts(2,:); 8 | 9 | 10 | %%%% CONSTRAINTS %%%% 11 | % 1) -pi/2 < th 12 | % 2) th < pi/2 13 | % 3) V > 0 14 | % 4) H > -u*H 15 | % 5) H < u*H 16 | 17 | % As a rule, each constraint will be satisfied if it's event function value 18 | % is positive. This makes things easier at the FSM level. 19 | n = length(th); 20 | value = zeros(5,n); 21 | isterminal = true(size(value)); 22 | direction = -ones(size(value)); 23 | 24 | value(1,:) = th + pi/2; 25 | value(2,:) = pi/2 - th; 26 | value(3,:) = V; 27 | 28 | value(4,:) = u*V - H; 29 | value(5,:) = H + u*V; 30 | 31 | end -------------------------------------------------------------------------------- /toppling_stick/events_slideNeg.m: -------------------------------------------------------------------------------- 1 | function [value, isterminal, direction] = events_slideNeg(~,Z,P) 2 | 3 | [~, contacts] = dynamics_slideNeg([],Z,P); 4 | th = Z(1,:); 5 | dx = Z(4,:); 6 | H = contacts(1,:); 7 | V = contacts(2,:); 8 | 9 | %%%% CONSTRAINTS %%%% 10 | % 1) -pi/2 < th 11 | % 2) th < pi/2 12 | % 3) dx <= 0 13 | % 4) V >= 0 14 | % 5) H >= 0 15 | 16 | % As a rule, each constraint will be satisfied if it's event function value 17 | % is positive. This makes things easier at the FSM level. 18 | n = length(th); 19 | value = zeros(5,n); 20 | isterminal = true(size(value)); 21 | direction = -ones(size(value)); 22 | 23 | %%%% HACK %%%% 24 | % avoid the singularity in the dynamics at th = pi/2 25 | th_crit = pi/2 - 1e-10; 26 | %%%% DONE %%%% 27 | 28 | value(1,:) = th + th_crit ; 29 | value(2,:) = th_crit - th; 30 | value(3,:) = -dx; 31 | value(4,:) = V; 32 | value(5,:) = H; 33 | 34 | end -------------------------------------------------------------------------------- /toppling_stick/events_slidePos.m: -------------------------------------------------------------------------------- 1 | function [value, isterminal, direction] = events_slidePos(~,Z,P) 2 | 3 | [~, contacts] = dynamics_slidePos([],Z,P); 4 | th = Z(1,:); 5 | dx = Z(4,:); 6 | H = contacts(1,:); 7 | V = contacts(2,:); 8 | 9 | %%%% CONSTRAINTS %%%% 10 | % 1) -pi/2 < th 11 | % 2) th < pi/2 12 | % 3) dx >= 0 13 | % 4) V >= 0 14 | % 5) H <= 0 15 | 16 | % As a rule, each constraint will be satisfied if it's event function value 17 | % is positive. This makes things easier at the FSM level. 18 | n = length(th); 19 | value = zeros(5,n); 20 | isterminal = true(size(value)); 21 | direction = -ones(size(value)); 22 | 23 | %%%% HACK %%%% 24 | % avoid the singularity in the dynamics at th = pi/2 25 | th_crit = pi/2 - 1e-10; 26 | %%%% DONE %%%% 27 | 28 | value(1,:) = th + th_crit ; 29 | value(2,:) = th_crit - th; 30 | value(3,:) = dx; 31 | value(4,:) = V; 32 | value(5,:) = -H; 33 | 34 | end -------------------------------------------------------------------------------- /toppling_stick/getBoundaries.m: -------------------------------------------------------------------------------- 1 | function Bnd = getBoundaries(xx,yy,zz) 2 | 3 | %This function returns a collection of lines for plotting the boundaries 4 | %between the sections in DD (a matrix of integers). 5 | 6 | [N,M] = size(zz); 7 | Bnd = []; 8 | for i=1:N 9 | x = xx(i,:); 10 | y = yy(i,:); 11 | z = zz(i,:); 12 | J = getJumps(x,y,z); 13 | Bnd = [Bnd;J]; 14 | end 15 | 16 | for j=1:M 17 | x = xx(:,j); 18 | y = yy(:,j); 19 | z = zz(:,j); 20 | J = getJumps(x,y,z); 21 | Bnd = [Bnd;J]; 22 | end 23 | 24 | end 25 | 26 | function J = getJumps(x,y,z) 27 | 28 | % in = coordinate 29 | % out = class 30 | 31 | idxJ = diff(z)~=0; 32 | 33 | low = x(1:(end-1)); low = low(idxJ); 34 | upp = x(2:end); upp = upp(idxJ); 35 | Jx = 0.5*(low+upp); 36 | 37 | low = y(1:(end-1)); low = low(idxJ); 38 | upp = y(2:end); upp = upp(idxJ); 39 | Jy = 0.5*(low+upp); 40 | 41 | J = [Jx(:), Jy(:)]; 42 | 43 | end 44 | 45 | -------------------------------------------------------------------------------- /toppling_stick/getSlipDist.m: -------------------------------------------------------------------------------- 1 | function dist = getSlipDist(D) 2 | 3 | %This function computes the amount that the stick slid in each direction 4 | 5 | N = length(D.phase); 6 | dist = zeros(N,1); 7 | 8 | for i=1:N 9 | if strcmp(D.phase{i},'SLIDE_POS') 10 | dist(i) = range(D.raw(i).state.x); 11 | elseif strcmp(D.phase{i},'SLIDE_NEG') 12 | dist(i) = -range(D.raw(i).state.x); 13 | end 14 | end 15 | 16 | end -------------------------------------------------------------------------------- /toppling_stick/topple_angularRate.m: -------------------------------------------------------------------------------- 1 | function dth = topple_angularRate(th,P) 2 | 3 | % AUTOMATICALLY GENERATED -- DO NOT EDIT 4 | % Computes angular rate for a stick toppling from rest. 5 | 6 | m = P.m; 7 | g = P.g; 8 | L = P.L; 9 | I = P.I; 10 | 11 | dth = sign(th).*(2.^(1./2).*(1 - cos(th)).^(1./2).*(L.*g.*m.*(I + L.^2.*m)).^(1./2))./(I + L.^2.*m); 12 | 13 | end 14 | -------------------------------------------------------------------------------- /toppling_stick/topple_contactHorizontal.m: -------------------------------------------------------------------------------- 1 | function H = topple_contactHorizontal(th,P) 2 | 3 | % AUTOMATICALLY GENERATED -- DO NOT EDIT 4 | % Computes horizontal component of contact force for a stick toppling from rest. 5 | 6 | m = P.m; 7 | g = P.g; 8 | L = P.L; 9 | I = P.I; 10 | 11 | H = -(L.^2.*g.*m.^2.*(3.*sin(2.*th) - 4.*sin(th)))./(2.*(I + L.^2.*m)); 12 | 13 | end 14 | -------------------------------------------------------------------------------- /toppling_stick/topple_contactVertical.m: -------------------------------------------------------------------------------- 1 | function V = topple_contactVertical(th,P) 2 | 3 | % AUTOMATICALLY GENERATED -- DO NOT EDIT 4 | % Computes vertical component of contact force for a stick toppling from rest. 5 | 6 | m = P.m; 7 | g = P.g; 8 | L = P.L; 9 | I = P.I; 10 | 11 | V = (g.*m.*(I - 2.*L.^2.*m.*cos(th) + 3.*L.^2.*m.*cos(th).^2))./(I + L.^2.*m); 12 | 13 | end 14 | -------------------------------------------------------------------------------- /toppling_stick/topple_criticalMu.m: -------------------------------------------------------------------------------- 1 | function mu = topple_criticalMu(th,P) 2 | 3 | % AUTOMATICALLY GENERATED -- DO NOT EDIT 4 | % Computes angular rate for a stick toppling from rest. 5 | 6 | m = P.m; 7 | g = P.g; 8 | L = P.L; 9 | I = P.I; 10 | 11 | mu = -(L.^2.*m.*(4.*sin(th) - 6.*cos(th).*sin(th)))./(2.*(I - 2.*L.^2.*m.*cos(th) + 3.*L.^2.*m.*cos(th).^2)); 12 | 13 | end 14 | -------------------------------------------------------------------------------- /tractorTrailer/A_Matrix.m: -------------------------------------------------------------------------------- 1 | function A = A_Matrix(phi,th,psi,v,Lt) 2 | 3 | %This is the jacobian of the system dynamics. These equations are derived 4 | %in the function Derive_EoM.m 5 | 6 | A = [... 7 | 8 | 0, 0, -v*cos(phi)*cos(psi)*cos(th), v*cos(psi)*sin(phi)*sin(th); 9 | 0, 0, -v*cos(phi)*cos(psi)*sin(th), -v*cos(psi)*cos(th)*sin(phi); 10 | 0, 0, 0, (v*cos(phi)*cos(psi))/Lt; 11 | 0, 0, 0, -(v*cos(phi)*cos(psi))/Lt]; 12 | 13 | end -------------------------------------------------------------------------------- /tractorTrailer/Actuator_Model.m: -------------------------------------------------------------------------------- 1 | function Input = Actuator_Model(Command, Params) 2 | 3 | %This function is used to saturate the actuators 4 | 5 | persistent Cmd_Prev 6 | 7 | if isempty(Cmd_Prev) 8 | Cmd_Prev = Command; 9 | end 10 | 11 | %Actuator Rate Saturation 12 | dt = Params.Ctl.dt; %Set in Simulator 13 | Rate = (Command - Cmd_Prev)/dt; 14 | Low = Rate < Params.Ctl.Rate_Limits(:,1); 15 | Upp = Rate > Params.Ctl.Rate_Limits(:,2); 16 | 17 | Command(Low) = Cmd_Prev(Low) + Params.Ctl.Rate_Limits(Low,1)*dt; 18 | Command(Upp) = Cmd_Prev(Upp) + Params.Ctl.Rate_Limits(Upp,2)*dt; 19 | 20 | % Actuator Saturation 21 | Low = Command < Params.Ctl.Actuator_Limits(:,1); 22 | Upp = Command > Params.Ctl.Actuator_Limits(:,2); 23 | 24 | Command(Low) = Params.Ctl.Actuator_Limits(Low,1); 25 | Command(Upp) = Params.Ctl.Actuator_Limits(Upp,2); 26 | 27 | Input = Command; %Store the results to return 28 | 29 | %Store persistent variables for the next function call 30 | Cmd_Prev = Command; 31 | 32 | end -------------------------------------------------------------------------------- /tractorTrailer/B_Matrix.m: -------------------------------------------------------------------------------- 1 | function B = B_Matrix(phi,th,psi,v,Lt,Lc) 2 | 3 | %This function is the jacobian of the system dynamics with respect to the 4 | %actuators. It is derived in Derive_EoM.m 5 | 6 | B = [... 7 | 8 | -cos(phi)*cos(psi)*sin(th), v*cos(phi)*sin(psi)*sin(th); 9 | cos(phi)*cos(psi)*cos(th), -v*cos(phi)*cos(th)*sin(psi); 10 | (cos(psi)*sin(phi))/Lt, -(v*sin(phi)*sin(psi))/Lt; 11 | (Lt*sin(psi) - Lc*cos(psi)*sin(phi))/(Lc*Lt), (v*(Lt*cos(psi) + Lc*sin(phi)*sin(psi)))/(Lc*Lt)]; 12 | 13 | end -------------------------------------------------------------------------------- /tractorTrailer/Controller.m: -------------------------------------------------------------------------------- 1 | function Command = Controller(State, Params, Goal_Idx, Trajectory) 2 | 3 | persistent K_Prev 4 | 5 | if strcmp(Params.Sim.Direction,'reverse') 6 | Dir = 2; 7 | else 8 | Dir = 1; 9 | end 10 | if Goal_Idx < 1 11 | %Then the at the end of the road. Command a stop 12 | Exit_Flag = true; 13 | Goal_Idx = Params.Traj.Npts; %Select the last point 14 | else 15 | Exit_Flag = false; 16 | end 17 | 18 | Goal = Trajectory.States{Dir,Goal_Idx}; 19 | K_New = Trajectory.Gains{Dir,Goal_Idx}; 20 | 21 | if isempty(K_Prev) 22 | K_Prev = K_New; 23 | end 24 | 25 | %Smooth controller switching 26 | alpha = Params.Ctl.Gain_Smoothing; 27 | K = K_New*(1-alpha) + K_Prev*(alpha); 28 | 29 | %Determine the Command value 30 | Command = -K*(State-Goal); 31 | 32 | %Command a stop if at the end of the road 33 | if Exit_Flag 34 | Command(1) = 0; 35 | end 36 | 37 | K_Prev = K; 38 | 39 | end -------------------------------------------------------------------------------- /tractorTrailer/Dot.m: -------------------------------------------------------------------------------- 1 | function Val = Dot(A,B) 2 | 3 | %2 dimensionsl dot product 4 | 5 | % A and B must both be of size (2xN); 6 | 7 | Val = A(1,:)*B(1,:) + A(2,:)*B(2,:); 8 | 9 | 10 | %NOTE - old version was not vectorized. Revert to that if errors. 11 | 12 | end -------------------------------------------------------------------------------- /tractorTrailer/Dynamics.m: -------------------------------------------------------------------------------- 1 | function dStates = Dynamics(States, Inputs, Params) 2 | 3 | %This function is used to compute the dynamics of the tractor trailer truck 4 | %system. The equations are derived by Derive_EoM.m 5 | 6 | %States is a (4xN) matrix of states 7 | %Inputs is a (2xN) matrix of inputs 8 | %Params is a structure of parameters 9 | 10 | %x = States(1,:); 11 | %y = States(2,:); 12 | th = States(3,:); 13 | phi = States(4,:); 14 | 15 | v = Inputs(1,:); 16 | psi = Inputs(2,:)'; 17 | 18 | Lt = Params.Dyn.Lt; 19 | Lc = Params.Dyn.Lc; 20 | 21 | %[dStates, A, B] = Derive_EoM() 22 | 23 | dStates =[... 24 | 25 | -v.*cos(phi).*cos(psi).*sin(th); 26 | v.*cos(phi).*cos(psi).*cos(th); 27 | (v.*cos(psi).*sin(phi))/Lt; 28 | (v.*(Lt.*sin(psi) - Lc.*cos(psi).*sin(phi)))/(Lc*Lt)]; 29 | 30 | 31 | end -------------------------------------------------------------------------------- /tractorTrailer/Estimator.m: -------------------------------------------------------------------------------- 1 | function [Xhat P] = Estimator(Z,Input,Params,Xhat,P) 2 | 3 | %This function does a single step of an extended Kalman filter 4 | 5 | %Unpack parameters 6 | dt = Params.Sim.dt; 7 | H = Params.Est.H; 8 | R = Params.Est.R; 9 | Q = Params.Est.Q; 10 | 11 | dStates = Dynamics(Xhat, Input, Params); 12 | Xbar = Xhat + dStates*dt; 13 | 14 | %Linearize about Xhat 15 | th = Xhat(3); 16 | phi = Xhat(4); 17 | v = Input(1); 18 | psi = Input(2); 19 | Lt = Params.Dyn.Lt; 20 | %Lc = Params.Dyn.Lc; 21 | F = A_Matrix(phi,th,psi,v,Lt); 22 | %G = B_Matrix(phi,th,psi,v,Lt,Lc); 23 | 24 | %Propagate covariance 25 | Pbar = F*P*F' + Q; 26 | 27 | %Measurement Update 28 | Nu = Z - H*Xbar; 29 | S = H*Pbar*H' + R; 30 | W = Pbar*H'/S; 31 | 32 | %Estimates! 33 | Xhat = Xbar + W*Nu; 34 | P = Pbar - W*S*W'; 35 | 36 | end -------------------------------------------------------------------------------- /tractorTrailer/MAIN.m: -------------------------------------------------------------------------------- 1 | clear; clc; 2 | 3 | %Get all user-defined parameters. 4 | Params = Set_Parameters(); 5 | 6 | %Create "Trajectory" which describes the road. Since the LQR trajectory 7 | %needs to know the deviation from the target state, the road is described 8 | %as a series of points in state space. 9 | Trajectory = Create_Trajectory(Params); 10 | 11 | %Run a simulation - drive the tractor trailer truck along with road, using 12 | %an EKF to do state estimation and a LQR controller to adjust the steering 13 | %angle and speed. 14 | Results = Simulator(Params, Trajectory); 15 | 16 | %Unpack the results: 17 | Time = Results.Time; 18 | Goal_Idx = Results.Goal_Idx; 19 | States = Results.States; 20 | Inputs = Results.Inputs; 21 | Trace_Cov = Results.Trace_Cov; 22 | State_Error = Results.State_Error; 23 | 24 | %Display things to the user: 25 | Render_Video = false; 26 | if Render_Video 27 | Create_Video(States,Params,Trajectory, Goal_Idx) 28 | else 29 | Animate_System(States,Params,Trajectory, Goal_Idx) 30 | end 31 | % PLOT_Map(States,Params,Trajectory) 32 | PLOT_Inputs(Time,Inputs) 33 | %PLOT_Stop_Action(States,Params,Trajectory) 34 | PLOT_Estimator(Time, Trace_Cov, State_Error); 35 | -------------------------------------------------------------------------------- /tractorTrailer/PLOT_Estimator.m: -------------------------------------------------------------------------------- 1 | function PLOT_Estimator(Time, Trace_Cov, State_Error) 2 | 3 | figure(6); 4 | plot(Time,Trace_Cov) 5 | title('Trace of the covariance matrix') 6 | xlabel('Time (s)') 7 | ylabel('Trace of Covariance') 8 | 9 | figure(7) 10 | 11 | subplot(4,1,1) 12 | plot(Time, State_Error(1,:)) 13 | title('EKF Error: X position', 'FontSize',16) 14 | xlabel('Time (s)') 15 | ylabel('Distance (m)') 16 | 17 | subplot(4,1,2) 18 | plot(Time, State_Error(2,:)) 19 | title('EKF Error: Y position', 'FontSize',16) 20 | xlabel('Time (s)') 21 | ylabel('Distance (m)') 22 | 23 | subplot(4,1,3) 24 | plot(Time, State_Error(3,:)) 25 | title('EKF Error: Orientation', 'FontSize',16) 26 | xlabel('Time (s)') 27 | ylabel('Angle (rad)') 28 | 29 | subplot(4,1,4) 30 | plot(Time, State_Error(4,:)) 31 | title('EKF Error: Cab-Trailer Angle', 'FontSize',16) 32 | xlabel('Time (s)') 33 | ylabel('Angle (rad)') 34 | 35 | end -------------------------------------------------------------------------------- /tractorTrailer/PLOT_Inputs.m: -------------------------------------------------------------------------------- 1 | function PLOT_Inputs(Time,Inputs) 2 | 3 | figure(3); 4 | clf; 5 | 6 | subplot(2,1,1) 7 | plot(Time,Inputs(1,:)) 8 | title('Speed Commands') 9 | xlabel('Time (s)') 10 | ylabel('Speed (m/s)') 11 | 12 | subplot(2,1,2) 13 | plot(Time,Inputs(2,:)) 14 | title('Steering Commands') 15 | xlabel('Time (s)') 16 | ylabel('Angle (rad)') 17 | 18 | end 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /tractorTrailer/PLOT_Map.m: -------------------------------------------------------------------------------- 1 | function PLOT_Map(States,Params, Trajectory) 2 | 3 | %This function plots an empty version of the map. 4 | 5 | x = States(1,:); 6 | y = States(2,:); 7 | th = States(3,:); 8 | phi = States(4,:); 9 | 10 | Lt = Params.Dyn.Lt; 11 | Lc = Params.Dyn.Lc; 12 | 13 | Npts = Params.Traj.Npts; 14 | 15 | %[dStates, A, B, Positions] = Derive_EoM() 16 | 17 | Pa = [... %Position of the back of the trailer 18 | x; 19 | y]; 20 | 21 | Pb = [... %Position of the back of the cab 22 | x - Lt*sin(th); 23 | y + Lt*cos(th)]; 24 | 25 | Pc = [... %Position of the front of the cab 26 | x - Lc*(cos(phi).*sin(th) + cos(th).*sin(phi)) - Lt*sin(th); 27 | y + Lc*(cos(phi).*cos(th) - sin(phi).*sin(th)) + Lt*cos(th)]; 28 | 29 | if strcmp(Params.Sim.Direction,'reverse') 30 | Dir = 2; 31 | else 32 | Dir = 1; 33 | end 34 | Road = zeros(2,Npts); 35 | for i=1:Npts 36 | Road(:,i) = Trajectory.States{Dir,i}(1:2); 37 | end 38 | 39 | figure(1) 40 | clf; hold on; 41 | plot(Pa(1,1),Pa(2,1),'r','LineWidth',5) 42 | plot(Pb(1,1),Pb(2,1),'b','LineWidth',3) 43 | plot(Pc(1,1),Pc(2,1),'g','LineWidth',3) 44 | legend('Back of Trailer','Back of Cab', 'Front of Cab') 45 | plot(Road(1,:),Road(2,:),'k','LineWidth',Params.Sim.RoadWidth) 46 | title('Map of the Road and Traces') 47 | axis equal 48 | 49 | end -------------------------------------------------------------------------------- /tractorTrailer/README.md: -------------------------------------------------------------------------------- 1 | README for Q-Exam Tractor Trailer truck control problem 2 | 3 | Problem Statement: Find a controller for a simple model of a tractor trailer truck that can drive backwards and forwards along a curving road. The number of sensors is less than the number of states, and the states are noisy. 4 | 5 | Solution: Use an extended Kalman filter to produce a state estimate as the truck is driving. At each time use a discrete linear quadratic regulator to fidn the optimal gains for the linearized system (about the current configuration). 6 | 7 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 8 | 9 | Run MAIN.M to get a simulation and plots. 10 | 11 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 12 | 13 | Equations of motion are derived using the Matlab symbolic toolbox, and this derivation can be found in Derive_EoM.m 14 | -------------------------------------------------------------------------------- /tractorTrailer/README.txt: -------------------------------------------------------------------------------- 1 | README for Q-Exam Tractor Trailer truck control problem 2 | 3 | Problem Statement: Find a controller for a simple model of a tractor trailer truck that can drive backwards and forwards along a curving road. The number of sensors is less than the number of states, and the states are noisy. 4 | 5 | Solution: Use an extended Kalman filter to produce a state estimate as the truck is driving. At each time use a discrete linear quadratic regulator to fidn the optimal gains for the linearized system (about the current configuration). 6 | 7 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 8 | 9 | Run MAIN.M to get a simulation and plots. 10 | 11 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 12 | 13 | Equations of motion are derived using the Matlab symbolic toolbox, and this derivation can be found in Derive_EoM.m -------------------------------------------------------------------------------- /tractorTrailer/Sensors.m: -------------------------------------------------------------------------------- 1 | function Z = Sensors(State,Params) 2 | 3 | %Directly measure three of the states, estimate overall orientation 4 | H = Params.Est.H; 5 | 6 | %Apply random noise 7 | w = Params.Est.Sensor_Noise_Variance.*randn(size(H,1),1); 8 | 9 | Z = H*State + w; 10 | 11 | end -------------------------------------------------------------------------------- /tractorTrailer/Set_Gain_Matrix.m: -------------------------------------------------------------------------------- 1 | function K = Set_Gain_Matrix(State, Input, Params) 2 | 3 | %This function linearizes the plant about the current configuation and then 4 | %uses a LQR to define the gain matrix. 5 | 6 | %This model assumes that the only control input is the steering angle, 7 | %because tractor trailer speed is set independently. 8 | 9 | th = State(3); 10 | phi = State(4); 11 | 12 | v = Input(1); 13 | psi = Input(2); 14 | 15 | Lt = Params.Dyn.Lt; 16 | Lc = Params.Dyn.Lc; 17 | 18 | A = A_Matrix(phi,th,psi,v,Lt); 19 | B = B_Matrix(phi,th,psi,v,Lt,Lc); 20 | 21 | K = lqr(A,B,Params.Ctl.Q,Params.Ctl.R); 22 | 23 | end --------------------------------------------------------------------------------