├── 00_Exams_Unified.pdf ├── Exams ├── 2010 │ ├── Robotics2_10.06.15.pdf │ ├── Robotics2_10.07.07.pdf │ └── Robotics2_10.09.15.pdf ├── 2011 │ ├── Robotics2_11.06.17.pdf │ └── Robotics2_11.09.12.pdf ├── 2012 │ ├── Robotics2_12.06.11.pdf │ └── Robotics2_12.07.05.pdf ├── 2013 │ ├── Robotics2_13.01.09.pdf │ ├── Robotics2_13.02.06.pdf │ └── Robotics2_13.07.15.pdf ├── 2014 │ └── Robotics2_14.10.27.pdf ├── 2016 │ ├── Robotics2_16.06.06.pdf │ ├── Robotics2_16.07.11.pdf │ ├── Robotics2_16.09.12.pdf │ ├── Robotics2_16.10.28.pdf │ ├── Robotics2_Final_Test_2015-16_16.06.01.pdf │ └── Robotics2_Midterm_Test_2015-16_16.04.13.pdf ├── 2017 │ ├── Robotics2_17.06.06.pdf │ ├── Robotics2_17.07.11.pdf │ ├── Robotics2_17.09.21.pdf │ ├── Robotics2_Final_Test_2016-17_17.05.29.pdf │ └── Robotics2_Midterm_Test_2016-17_17.03.29.pdf ├── 2018 │ ├── Robotics2_18.01.11.pdf │ ├── Robotics2_18.02.05.pdf │ ├── Robotics2_18.03.27.pdf │ ├── Robotics2_18.06.11.pdf │ ├── Robotics2_18.07.11.pdf │ └── Robotics2_Midterm_Test_2017-18_18.04.26.pdf ├── 2019 │ ├── Robotics2_19.06.17.pdf │ ├── Robotics2_19.07.11.pdf │ ├── Robotics2_19.09.11.pdf │ └── Robotics2_Midterm_Test_2018-19_19.04.29.pdf ├── 2020 │ ├── Robotics2_20.06.05.pdf │ ├── Robotics2_20.07.15.pdf │ ├── Robotics2_20.09.11.pdf │ └── Robotics2_Remote_Midterm_Test_2019-20_20.04.15.pdf ├── 2021 │ ├── Robotics2_21.01.12.pdf │ ├── Robotics2_21.02.04.pdf │ ├── Robotics2_21.06.11.pdf │ ├── Robotics2_21.07.12.pdf │ ├── Robotics2_21.09.10.pdf │ └── Robotics2_Remote_Midterm_Test_2020-21_21.04.14.pdf ├── 2022 │ ├── Robotics2_22.01.11.pdf │ ├── Robotics2_22.02.03.pdf │ ├── Robotics2_22.06.10.pdf │ ├── Robotics2_22.07.08.pdf │ ├── Robotics2_22.09.09.pdf │ ├── Robotics2_22.10.21.pdf │ └── Robotics2_Midterm_Test_2021-22_22.04.13.pdf ├── 2023 │ ├── Robotics2_23.01.25.pdf │ ├── Robotics2_23.02.13.pdf │ ├── Robotics2_23.06.12.pdf │ └── Robotics2_Midterm_Test_2022-23_23.04.19.pdf └── 2024 │ ├── Robotics2_24.06.12.pdf │ ├── Robotics2_24.07.08.pdf │ ├── Robotics2_24.09.19.pdf │ └── Robotics2_Midterm_Test_2023-24_24.04.24.pdf ├── Lectures ├── 01_Calibration.pdf ├── 02_KinematicRedundancy_1.pdf ├── 02_KinematicRedundancy_2.pdf ├── 03_LagrangianDynamics_1.pdf ├── 04_LagrangianDynamics_2.pdf ├── 05_LagrangianDynamics_3.pdf ├── 05a_LagrangianDynamics_3.pdf ├── 05b_LinearParametrizationIdentification.pdf ├── 06_NewtonEulerDynamics.pdf ├── 07_IntroControl.pdf ├── 08_Regulation.pdf ├── 09_IterativeLearning.pdf ├── 10_TrajectoryControl.pdf ├── 12_AdaptiveControl.pdf ├── 13_CartesianControl.pdf ├── 14_EnvironmentInteraction.pdf ├── 15_ImpedanceControl.pdf ├── 16_HybridControl.pdf ├── 17_VisualServoing.pdf ├── 18_ActuationFaults.pdf ├── 19_CollisionDetectionReaction.pdf └── 20_HR_CoexistenceCollaboration.pdf ├── Notes ├── 1. Robotics II - Formulary 2024.pdf └── 2. Robotics II - Notions 2021.pdf ├── README.md └── Scripts ├── 2020 ├── Boulton.m ├── DH_table.m ├── GENERAL_Posh.m ├── JBM_pseudoinverse.m ├── JBM_weightedpseudo.m ├── MovingFrame_2R.m ├── MovingFrame_3R.m ├── Saturation.m ├── dynmod_2Rrobot.m ├── dynmod_2Rrobot_motor.m ├── dynmod_3Rrobot.m ├── dynmod_PPRRrobot.m ├── dynmod_PPRrobot.m ├── dynmod_PProbot.m ├── dynmod_PRProbot.m ├── dynmod_PRProbot.mat ├── dynmod_PRR.m ├── dynmod_PRRRrobot.m ├── dynmod_PRrobot.m ├── dynmod_RPProbot.m ├── dynmod_RPRrobot.m ├── dynmod_RProbot.m ├── planar_2R.m ├── planar_3R.m ├── planar_4R.m ├── planar_PPR.m ├── planar_RP.m ├── reduced_gradient.m └── skewsymmetric.m ├── 2023 └── Flavio │ ├── basic_stuff │ ├── get_angle_rot_mat.mlx │ ├── hom_mat.mlx │ ├── inv_hom_mat.mlx │ ├── isOrthonormal.mlx │ ├── isRotation.mlx │ ├── rot_mat.mlx │ └── useful_commands.mlx │ ├── dirkin_functions │ ├── directkin.mlx │ ├── dirkin_planar3R.mlx │ ├── p_ee.mlx │ └── planar_2R.mlx │ ├── dirkin_scripts │ └── planar2R.mlx │ ├── dynamics │ ├── CoordinateTransofmration.mlx │ ├── NewtonEuler.mlx │ ├── dynmodAntropomorphic.mlx │ ├── dynmodPPR.mlx │ ├── dynmodPPRR.mlx │ ├── dynmodPRP.mlx │ ├── dynmodPRRR.mlx │ ├── dynmodPlanar2R.asv │ ├── dynmodPlanar2R.mlx │ ├── dynmodPlanar3R.mlx │ ├── dynmodPlanarPR.mlx │ ├── dynmodPlanarRP.mlx │ ├── dynmodPolar2R.mlx │ ├── dynmodSpatial3R.mlx │ └── dynmod_PRrobot.m │ ├── exams │ ├── rob2_16_04_13.mlx │ ├── rob2_17_03_29.mlx │ ├── rob2_18_04_26.mlx │ ├── rob2_19_04_29.mlx │ ├── rob2_20_04_15.mlx │ └── rob2_23_02_13.mlx │ ├── invkin_functions │ ├── invkin_RRP.mlx │ ├── invkin_RRR.mlx │ ├── invkin_planar2R.mlx │ └── invkin_planar3R.mlx │ ├── invkin_scripts │ ├── ex6_midterm_2021.mlx │ ├── test_RRP.mlx │ └── test_RRR.mlx │ ├── orientation_representations │ ├── S.m │ ├── axisangle_dir.mlx │ ├── axisangle_inv.mlx │ ├── axisangle_script.mlx │ ├── euler_dir.mlx │ ├── euler_inv.mlx │ ├── euler_script.mlx │ ├── omega_phi.mlx │ ├── rpy_dir.mlx │ └── rpy_inv.mlx │ ├── random_scripts │ └── calibration.mlx │ ├── redundancy │ ├── DynamicResolution.mlx │ ├── NullSpaceGradient.mlx │ ├── SNS.mlx │ ├── TaskPriority.mlx │ ├── lq_problem.mlx │ └── null_space.mlx │ └── templates │ ├── euler_template.mlx │ ├── invkin_template.mlx │ └── numerical_invkin_template.mlx └── 2024 ├── Gianmarco ├── DH_to_JA.m ├── DH_to_JL.m ├── DHmatrix.m ├── Exams │ ├── Exam_20_06_05.m │ ├── Exam_20_07_15.m │ ├── Exam_21_02_04.m │ ├── Exam_21_07_12.m │ ├── Exam_22_07_08.m │ ├── Exam_22_09_09.m │ ├── Exam_22_10_21.m │ ├── Exam_23_01_25.m │ ├── Exam_24_04_24.m │ ├── Exam_24_06_12.m │ ├── Exam_24_07_08.m │ └── Exam_24_09_19.m ├── SNS.m ├── christoffel_symbols.m ├── compute_christoffel_matrix.m ├── compute_g.m ├── compute_inertia_matrix.m ├── compute_potential_energy_matrix.m ├── extract_Y_v3.m ├── inertia_matrix_from_kinetic_energy.m ├── inertia_matrix_to_coriolis.m ├── moving_frames_algorithm.m ├── newton_euler_algorithm.m ├── test.m └── time_derivate.m └── Martina ├── DHMatrix.m ├── Exam_2016_3.m ├── Exam_2016_3.mlx ├── Exam_2018_n_1.m ├── Exam_2018_n_1.mlx ├── Exam_Midterm_Exercise-2.m ├── Exam_Midterm_Exercise-2.mlx ├── Homework_2_NE_Proof.m ├── Homework_2_NE_Proof.mlx ├── bound_of_g.m ├── find_singularity.m ├── g_from_potential_energy.m ├── inertia_matrix_from_kinetic_energy.m ├── inertia_matrix_to_coriolis.m ├── is_inertia_matrix.m ├── moving_frames_algorithm.m ├── newton_euler_algorithm.m ├── reduced_gradient.m ├── regressor_matrix.m ├── test_moving_frame_midterm_2024.mlx ├── untitled2.m └── untitled2.mlx /00_Exams_Unified.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/00_Exams_Unified.pdf -------------------------------------------------------------------------------- /Exams/2010/Robotics2_10.06.15.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2010/Robotics2_10.06.15.pdf -------------------------------------------------------------------------------- /Exams/2010/Robotics2_10.07.07.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2010/Robotics2_10.07.07.pdf -------------------------------------------------------------------------------- /Exams/2010/Robotics2_10.09.15.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2010/Robotics2_10.09.15.pdf -------------------------------------------------------------------------------- /Exams/2011/Robotics2_11.06.17.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2011/Robotics2_11.06.17.pdf -------------------------------------------------------------------------------- /Exams/2011/Robotics2_11.09.12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2011/Robotics2_11.09.12.pdf -------------------------------------------------------------------------------- /Exams/2012/Robotics2_12.06.11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2012/Robotics2_12.06.11.pdf -------------------------------------------------------------------------------- /Exams/2012/Robotics2_12.07.05.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2012/Robotics2_12.07.05.pdf -------------------------------------------------------------------------------- /Exams/2013/Robotics2_13.01.09.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2013/Robotics2_13.01.09.pdf -------------------------------------------------------------------------------- /Exams/2013/Robotics2_13.02.06.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2013/Robotics2_13.02.06.pdf -------------------------------------------------------------------------------- /Exams/2013/Robotics2_13.07.15.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2013/Robotics2_13.07.15.pdf -------------------------------------------------------------------------------- /Exams/2014/Robotics2_14.10.27.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2014/Robotics2_14.10.27.pdf -------------------------------------------------------------------------------- /Exams/2016/Robotics2_16.06.06.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2016/Robotics2_16.06.06.pdf -------------------------------------------------------------------------------- /Exams/2016/Robotics2_16.07.11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2016/Robotics2_16.07.11.pdf -------------------------------------------------------------------------------- /Exams/2016/Robotics2_16.09.12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2016/Robotics2_16.09.12.pdf -------------------------------------------------------------------------------- /Exams/2016/Robotics2_16.10.28.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2016/Robotics2_16.10.28.pdf -------------------------------------------------------------------------------- /Exams/2016/Robotics2_Final_Test_2015-16_16.06.01.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2016/Robotics2_Final_Test_2015-16_16.06.01.pdf -------------------------------------------------------------------------------- /Exams/2016/Robotics2_Midterm_Test_2015-16_16.04.13.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2016/Robotics2_Midterm_Test_2015-16_16.04.13.pdf -------------------------------------------------------------------------------- /Exams/2017/Robotics2_17.06.06.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2017/Robotics2_17.06.06.pdf -------------------------------------------------------------------------------- /Exams/2017/Robotics2_17.07.11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2017/Robotics2_17.07.11.pdf -------------------------------------------------------------------------------- /Exams/2017/Robotics2_17.09.21.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2017/Robotics2_17.09.21.pdf -------------------------------------------------------------------------------- /Exams/2017/Robotics2_Final_Test_2016-17_17.05.29.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2017/Robotics2_Final_Test_2016-17_17.05.29.pdf -------------------------------------------------------------------------------- /Exams/2017/Robotics2_Midterm_Test_2016-17_17.03.29.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2017/Robotics2_Midterm_Test_2016-17_17.03.29.pdf -------------------------------------------------------------------------------- /Exams/2018/Robotics2_18.01.11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2018/Robotics2_18.01.11.pdf -------------------------------------------------------------------------------- /Exams/2018/Robotics2_18.02.05.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2018/Robotics2_18.02.05.pdf -------------------------------------------------------------------------------- /Exams/2018/Robotics2_18.03.27.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2018/Robotics2_18.03.27.pdf -------------------------------------------------------------------------------- /Exams/2018/Robotics2_18.06.11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2018/Robotics2_18.06.11.pdf -------------------------------------------------------------------------------- /Exams/2018/Robotics2_18.07.11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2018/Robotics2_18.07.11.pdf -------------------------------------------------------------------------------- /Exams/2018/Robotics2_Midterm_Test_2017-18_18.04.26.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2018/Robotics2_Midterm_Test_2017-18_18.04.26.pdf -------------------------------------------------------------------------------- /Exams/2019/Robotics2_19.06.17.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2019/Robotics2_19.06.17.pdf -------------------------------------------------------------------------------- /Exams/2019/Robotics2_19.07.11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2019/Robotics2_19.07.11.pdf -------------------------------------------------------------------------------- /Exams/2019/Robotics2_19.09.11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2019/Robotics2_19.09.11.pdf -------------------------------------------------------------------------------- /Exams/2019/Robotics2_Midterm_Test_2018-19_19.04.29.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2019/Robotics2_Midterm_Test_2018-19_19.04.29.pdf -------------------------------------------------------------------------------- /Exams/2020/Robotics2_20.06.05.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2020/Robotics2_20.06.05.pdf -------------------------------------------------------------------------------- /Exams/2020/Robotics2_20.07.15.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2020/Robotics2_20.07.15.pdf -------------------------------------------------------------------------------- /Exams/2020/Robotics2_20.09.11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2020/Robotics2_20.09.11.pdf -------------------------------------------------------------------------------- /Exams/2020/Robotics2_Remote_Midterm_Test_2019-20_20.04.15.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2020/Robotics2_Remote_Midterm_Test_2019-20_20.04.15.pdf -------------------------------------------------------------------------------- /Exams/2021/Robotics2_21.01.12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2021/Robotics2_21.01.12.pdf -------------------------------------------------------------------------------- /Exams/2021/Robotics2_21.02.04.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2021/Robotics2_21.02.04.pdf -------------------------------------------------------------------------------- /Exams/2021/Robotics2_21.06.11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2021/Robotics2_21.06.11.pdf -------------------------------------------------------------------------------- /Exams/2021/Robotics2_21.07.12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2021/Robotics2_21.07.12.pdf -------------------------------------------------------------------------------- /Exams/2021/Robotics2_21.09.10.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2021/Robotics2_21.09.10.pdf -------------------------------------------------------------------------------- /Exams/2021/Robotics2_Remote_Midterm_Test_2020-21_21.04.14.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2021/Robotics2_Remote_Midterm_Test_2020-21_21.04.14.pdf -------------------------------------------------------------------------------- /Exams/2022/Robotics2_22.01.11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2022/Robotics2_22.01.11.pdf -------------------------------------------------------------------------------- /Exams/2022/Robotics2_22.02.03.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2022/Robotics2_22.02.03.pdf -------------------------------------------------------------------------------- /Exams/2022/Robotics2_22.06.10.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2022/Robotics2_22.06.10.pdf -------------------------------------------------------------------------------- /Exams/2022/Robotics2_22.07.08.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2022/Robotics2_22.07.08.pdf -------------------------------------------------------------------------------- /Exams/2022/Robotics2_22.09.09.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2022/Robotics2_22.09.09.pdf -------------------------------------------------------------------------------- /Exams/2022/Robotics2_22.10.21.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2022/Robotics2_22.10.21.pdf -------------------------------------------------------------------------------- /Exams/2022/Robotics2_Midterm_Test_2021-22_22.04.13.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2022/Robotics2_Midterm_Test_2021-22_22.04.13.pdf -------------------------------------------------------------------------------- /Exams/2023/Robotics2_23.01.25.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2023/Robotics2_23.01.25.pdf -------------------------------------------------------------------------------- /Exams/2023/Robotics2_23.02.13.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2023/Robotics2_23.02.13.pdf -------------------------------------------------------------------------------- /Exams/2023/Robotics2_23.06.12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2023/Robotics2_23.06.12.pdf -------------------------------------------------------------------------------- /Exams/2023/Robotics2_Midterm_Test_2022-23_23.04.19.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2023/Robotics2_Midterm_Test_2022-23_23.04.19.pdf -------------------------------------------------------------------------------- /Exams/2024/Robotics2_24.06.12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2024/Robotics2_24.06.12.pdf -------------------------------------------------------------------------------- /Exams/2024/Robotics2_24.07.08.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2024/Robotics2_24.07.08.pdf -------------------------------------------------------------------------------- /Exams/2024/Robotics2_24.09.19.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2024/Robotics2_24.09.19.pdf -------------------------------------------------------------------------------- /Exams/2024/Robotics2_Midterm_Test_2023-24_24.04.24.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Exams/2024/Robotics2_Midterm_Test_2023-24_24.04.24.pdf -------------------------------------------------------------------------------- /Lectures/01_Calibration.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/01_Calibration.pdf -------------------------------------------------------------------------------- /Lectures/02_KinematicRedundancy_1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/02_KinematicRedundancy_1.pdf -------------------------------------------------------------------------------- /Lectures/02_KinematicRedundancy_2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/02_KinematicRedundancy_2.pdf -------------------------------------------------------------------------------- /Lectures/03_LagrangianDynamics_1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/03_LagrangianDynamics_1.pdf -------------------------------------------------------------------------------- /Lectures/04_LagrangianDynamics_2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/04_LagrangianDynamics_2.pdf -------------------------------------------------------------------------------- /Lectures/05_LagrangianDynamics_3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/05_LagrangianDynamics_3.pdf -------------------------------------------------------------------------------- /Lectures/05a_LagrangianDynamics_3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/05a_LagrangianDynamics_3.pdf -------------------------------------------------------------------------------- /Lectures/05b_LinearParametrizationIdentification.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/05b_LinearParametrizationIdentification.pdf -------------------------------------------------------------------------------- /Lectures/06_NewtonEulerDynamics.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/06_NewtonEulerDynamics.pdf -------------------------------------------------------------------------------- /Lectures/07_IntroControl.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/07_IntroControl.pdf -------------------------------------------------------------------------------- /Lectures/08_Regulation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/08_Regulation.pdf -------------------------------------------------------------------------------- /Lectures/09_IterativeLearning.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/09_IterativeLearning.pdf -------------------------------------------------------------------------------- /Lectures/10_TrajectoryControl.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/10_TrajectoryControl.pdf -------------------------------------------------------------------------------- /Lectures/12_AdaptiveControl.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/12_AdaptiveControl.pdf -------------------------------------------------------------------------------- /Lectures/13_CartesianControl.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/13_CartesianControl.pdf -------------------------------------------------------------------------------- /Lectures/14_EnvironmentInteraction.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/14_EnvironmentInteraction.pdf -------------------------------------------------------------------------------- /Lectures/15_ImpedanceControl.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/15_ImpedanceControl.pdf -------------------------------------------------------------------------------- /Lectures/16_HybridControl.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/16_HybridControl.pdf -------------------------------------------------------------------------------- /Lectures/17_VisualServoing.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/17_VisualServoing.pdf -------------------------------------------------------------------------------- /Lectures/18_ActuationFaults.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/18_ActuationFaults.pdf -------------------------------------------------------------------------------- /Lectures/19_CollisionDetectionReaction.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Lectures/19_CollisionDetectionReaction.pdf -------------------------------------------------------------------------------- /Lectures/20_HR_CoexistenceCollaboration.pdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 403 Forbidden 4 | 5 |

Forbidden

6 |

You don't have permission to access /~deluca/rob2_en/20_HR_CoexistenceCollaboration.pdf 7 | on this server.
8 |

9 |
10 |
Apache/2.4.18 (Ubuntu) Server at www.diag.uniroma1.it Port 80
11 | 12 | -------------------------------------------------------------------------------- /Notes/1. Robotics II - Formulary 2024.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Notes/1. Robotics II - Formulary 2024.pdf -------------------------------------------------------------------------------- /Notes/2. Robotics II - Notions 2021.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Notes/2. Robotics II - Notions 2021.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robotics 2 2 | 3 | | | Available | Last Update | 4 | | --------------- | --------- | ----------- | 5 | | Exams | Yes | 2024 Dec | 6 | | Homework | No | --- | 7 | | Notes | Yes | 2024 Sep | 8 | | Scripts | Yes | 2024 Sep | 9 | | Slides | Yes | 2024 Aug | 10 | 11 | ## Website of the course 12 | 13 | | Course Site | Last Update | 14 | | -------------------------------------------------------------------------------------------------------------------------------------- | ----------- | 15 | | [http://www.diag.uniroma1.it/~deluca/rob2_en/material_rob2_en.html](http://www.diag.uniroma1.it/~deluca/rob2_en/material_rob2_en.html) | 2024 | 16 | 17 | > :bulb:_TIP: For studying this exam, it could be sufficient to have a general overview of the topics through the slides and to solve Exams using the 2024 Formulary._ 18 | 19 | > _**NOTE n.1:**_ Several useful scripts for Robotics 2, such as SNS implementation, are available in `Scripts > 2023 > Flavio > redundancy` folder. 20 | 21 | > :exclamation:_**NOTE n.2**_: Exams (only those with solutions), Notes, Scripts and Slides have been updated for Robotics 2 (A.Y. 23-24). Also, a unified Exams PDF is available for download. 22 | 23 | ### Requirements 24 | 25 | MATLAB for students 26 | 27 | * [Link to have MATLAB](https://it.mathworks.com/academia/tah-portal/sapienza-universita-di-roma-40576534.html) 28 | 29 | ------------------------ 30 | 31 | ### Acknowledgements 32 | 33 | _Remember to always thank who shares their work for free!_ 34 | | Name | Contribution | 35 | | ------------------------------------------------------ | ----------- | 36 | | **[Sveva Pepe](pepes97)** | Original 2020-2021 contribution | 37 | | **[Francesco Scotti](https://github.com/FrancescoScotti)** | Original 2020-2021 contribution | 38 | | **[Flavio Maiorana](https://github.com/neverorfrog)** | [Script updates](https://github.com/neverorfrog/robotics-toolbox-airo) for A.Y. 2022-2023 | 39 | | **[Martina Doku](https://github.com/MartinaDoku2001)** | [Theory & Scripts updates](https://github.com/MartinaDoku2001/robotics1_scripts) for A.Y. 2023-2024 | 40 | | **[Giuseppina Iannotti](https://github.com/Giuseian)** | Contributions in _`Notes > 1. Robotics II - Formulary 2024.pdf`_ | 41 | | **[Jacopo Tedeschi](https://github.com/jacocoptdsc)** | Contributions in _`Scripts > 2024 > Gianmarco`_ | 42 | | **[Camilla Iorio](https://github.com/camillaiorio)** | Contributions in _`Scripts > 2024 > Gianmarco`_ | 43 | | **[Gianmarco Scarano](https://github.com/SlimShadys)** | Current Repo maintainer | -------------------------------------------------------------------------------- /Scripts/2020/Boulton.m: -------------------------------------------------------------------------------- 1 | %% dynamic model of Boulton-Watt under gravity 2 | %% using a Lagrangian formulation in symbolic form 3 | 4 | clear all 5 | close all 6 | clc 7 | 8 | %% define symbolic variables 9 | syms m real 10 | syms d1 d2 d3 real 11 | syms L h real 12 | syms Isxx Isyy Iszz real %symbolic variables explicitly defined as real 13 | syms I2xx I2yy I2zz real %symbolic variables explicitly defined as real 14 | syms theta phi real 15 | syms dtheta dphi real 16 | syms ddq1 ddq2 real 17 | syms u1 u2 g0 real 18 | 19 | disp(' ') 20 | disp('*kinetic energy of link 1*') 21 | %% compute linear part of linear kinetic energy of joint 1 22 | pct = [0;0;h]; 23 | vct=diff(pct,theta)*dtheta; 24 | Tlt = simplify(1/2*m*vct'*vct); 25 | 26 | %% compute angular part of linear kinetic energy of joint 1 27 | omt = [0 0 dtheta]'; 28 | Tat = simplify(1/2*omt'*diag([Isxx Isyy Iszz])*omt); 29 | 30 | Tt = simplify(Tlt+Tat) 31 | pause 32 | 33 | disp('*kinetic energy of link 2') 34 | 35 | %% compute the linear part of kinetic energy of joint 2 36 | %pcp=[L*cos(phi)*-theta L*sin(phi) 0]'; 37 | %vcp=diff(pcp,theta)*dtheta+diff(pcp,phi)*dphi; 38 | %vcp = dtheta*sin(phi)+phi; 39 | Tlp= simplify((1/2)*m*L^2*(dphi^2+dtheta^2*sin(phi)^2)); 40 | 41 | %% compute the angular part of kinetic energy of joint 2 42 | omp = [0 0 0]'; 43 | Tap = simplify(1/2*omp'*diag([Isxx Isyy Iszz])*omp); 44 | 45 | Tp= simplify(Tlp+Tap) 46 | pause 47 | 48 | disp('***robot kinetic energy***') 49 | 50 | %% total kinetic energy 51 | T=simplify(Tt+2*Tp); 52 | T=collect(T,dtheta^2); 53 | T=collect(T,dphi^2); 54 | 55 | %% inertia matrix 56 | 57 | M(1,1)=diff(T,dtheta,2); 58 | M(2,2)=diff(T,dphi,2); 59 | 60 | TempB12=diff(T,dtheta); 61 | M(1,2)=diff(TempB12,dphi); 62 | M(2,1)=M(1,2); 63 | M=simplify(M) 64 | pause 65 | %% compute the Christoffel Matrix 66 | q=[theta;phi]; 67 | 68 | % consider the first column of matrix M, so you have M1 69 | % do the same for M2 70 | M1=M(:,1); 71 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,theta)) 72 | M2=M(:,2); 73 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,phi)) 74 | 75 | pause 76 | 77 | disp('***robot centrifugal terms (no Coriolis here!)***') 78 | 79 | dq=[dtheta;dphi]; 80 | c1=dq'*C1*dq; 81 | c2=dq'*C2*dq; 82 | c=[c1;c2] 83 | 84 | pause 85 | 86 | %% gavity term 87 | g=-g0; 88 | 89 | 90 | U = 2*m*g*L*cos(phi); 91 | 92 | G=jacobian(U,q)' 93 | pause 94 | 95 | 96 | -------------------------------------------------------------------------------- /Scripts/2020/DH_table.m: -------------------------------------------------------------------------------- 1 | clear all 2 | %% Define symbolic variables 3 | syms alpha a d C D E F theta 4 | syms q1 q2 q3 q4 q5 q6 real 5 | syms d1 d2 d3 a2 a3 d4 L1 L2 L3 L4 r2 real 6 | 7 | %% number of joints 8 | N=6; 9 | 10 | %% Insert DH table of parameters of SCARA 11 | 12 | %% ALPHA A D THETA 13 | DHTABLE = [-pi/2 0 L1 q1; 14 | 0 L2 0 q2; 15 | 0 L3 0 q3; 16 | -pi/2 0 0 q4; 17 | 0 L4 0 q5; 18 | 0 0 0 q6] 19 | 20 | %% Build the general Denavit-Hartenberg trasformation matrix 21 | TDH = [cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta); 22 | sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta); 23 | 0 sin(alpha) cos(alpha) d; 24 | 0 0 0 1]; 25 | 26 | %% Build transformation matrices for each link 27 | 28 | % empty cell array, we will put homogenous matrices 29 | A = cell(1,N); 30 | 31 | % For every row in 'DHTABLE' we substitute the right value inside 32 | % the general DH matrix 33 | for i = 1:N 34 | alpha = DHTABLE(i,1); 35 | a = DHTABLE(i,2); 36 | d = DHTABLE(i,3); 37 | theta = DHTABLE(i,4); 38 | A{i} = subs(TDH); 39 | end 40 | 41 | % we compute the final result matrix 42 | % homogenous matrix of last frame respect to the first frame 43 | tot=A{1}; 44 | for i = 2:N 45 | ris=A{i}; 46 | tot=tot*ris; 47 | end 48 | 49 | %% Compute the point of last frame respect to the first frame 50 | p=simplify(tot(1:3,4)) 51 | pause; 52 | 53 | %% Compute matrices R from homogenous A 54 | R = cell(1,N); 55 | 56 | for i = 1:N 57 | R{i}=A{i}(1:3,1:3); 58 | fprintf('R{%d} \n',i); 59 | R{i} 60 | pause; 61 | end 62 | -------------------------------------------------------------------------------- /Scripts/2020/GENERAL_Posh.m: -------------------------------------------------------------------------------- 1 | %% PRP-planar robot (Midterm 2018-2019) 2 | 3 | % 1) N.B: --> each column refers to a sigle joint's parameters 4 | % 2) GAME STARTS 5 | 6 | % number of joints 7 | N = 3; 8 | 9 | % 'q' and 'dq' are respectively vectors of joint's variables and their derivatives 10 | q = sym(['q1';'q2';'q3']); assume(q, 'real'); 11 | dq = sym(['dq1';'dq2';'dq3']); assume(dq, 'real'); 12 | 13 | % gravity vector 14 | g0 = [0; 0; 0]; % Horizontal plane otherwise g0=[0; -sym('g0'); 0]; 15 | 16 | % constants 17 | k1=sym('k1'); assume(k1, 'real'); 18 | D2=sym('D2'); assume(D2, 'real'); 19 | L2=sym('L2'); assume(L2, 'real'); 20 | 21 | % masses 22 | m = sym(['m1'; 'm2'; 'm3']); assume(m, 'real'); 23 | 24 | 25 | % Inertia 26 | Iz = [sym('Iz1'); sym('Iz2'); sym('Iz3')]; assume(Iz, 'real'); 27 | I = cell(1,N); 28 | 29 | for i=1:N 30 | I{i} = [0 0 0; 31 | 0 0 0; 32 | 0 0 Iz(i)]; 33 | end 34 | 35 | % positions of CoM's w.r.t. the base frame 36 | rc = [0 D2*cos(q(2)) L2*cos(q(2))-q(3)*sin(q(2)); 37 | q(1) k1+q(1)+D2*sin(q(2)) q(1)+k1+L2*sin(q(2))+q(3)*cos(q(2)); 38 | 0 0 0]; 39 | 40 | % angular velocities 41 | w = [0 0 0; 42 | 0 0 0; 43 | 0 dq(2) dq(2)]; 44 | 45 | % traslational velocities 46 | Vc = [ 0 -D2*dq(2)*sin(q(2)) -L2*dq(2)*sin(q(2))-(dq(3)*sin(q(2))+q(3)*dq(2)*cos(q(2))); 47 | dq(1) dq(1)+D2*dq(2)*cos(q(2)) dq(1)+L2*dq(2)*cos(q(2))+(dq(3)*cos(q(2))-q(3)*dq(2)*sin(q(2))); 48 | 0 0 0]; 49 | 50 | %% Kinetic Energy and Inertia Matrix 51 | T = sym(zeros(N,1)); 52 | Tot_Kin_En = 0; 53 | 54 | for i=1:N 55 | T(i) = 0.5*m(i)*(Vc(:,i)'*Vc(:,i))+ 0.5*w(:,i)'*I{i}*w(:,i); 56 | T(i) = simplify(T(i),'IgnoreAnalyticConstraints',true); 57 | Tot_Kin_En = Tot_Kin_En + T(i); 58 | end 59 | 60 | Tot_Kin_En = simplify(collect(Tot_Kin_En,dq)); 61 | 62 | M = sym(zeros(N,N)); 63 | for i=1:N 64 | for j=1:N 65 | if i==j 66 | Tot_Kin_En = collect(Tot_Kin_En, dq(i)^2); 67 | M(i,i) = simplify(diff(Tot_Kin_En, dq(i), 2)); 68 | 69 | else 70 | Tot_Kin_En = collect(Tot_Kin_En, dq(i)*dq(j)); 71 | tmp = simplify(diff(Tot_Kin_En, dq(i))); 72 | M(i,j) = simplify(diff(tmp, dq(j))); 73 | M(j,i) = M(i,j); 74 | end 75 | end 76 | end 77 | M=simplify(M); 78 | 79 | %% Calculate Christoffel matrix 80 | c = sym(zeros(N,1)); 81 | C = cell(1,N); 82 | 83 | for j=1:N 84 | C{j} = 0.5*simplify((jacobian(M(:,j),q) + transpose(jacobian(M(:,j),q)) - diff(M,q(j)))); 85 | c(j) = dq'*C{j}*dq; 86 | c(j) = simplify(collect(c(j), dq)); 87 | end 88 | 89 | %% Calculate Gravitational term 90 | U = 0; 91 | aux = sym(zeros(N,1)); 92 | 93 | for i=1:N 94 | aux(i) = simplify(-m(i)*g0'*rc(:,i)); 95 | U = U + aux(i); 96 | end 97 | 98 | G = jacobian(U,q)'; 99 | 100 | % GAME OVER 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | -------------------------------------------------------------------------------- /Scripts/2020/JBM_pseudoinverse.m: -------------------------------------------------------------------------------- 1 | %% Jacobian Based Method - Pseudoinverse 2 | function [qd, Jpse] = JBM_pseudoinverse(J,pd) 3 | % takes as inputs: 4 | % - J = the jacobian matrix 5 | % - pd = the cartesian velocity 6 | % 7 | % output: 8 | % - qd = q dot 9 | % - Jpse = pseudoinverse 10 | 11 | %% Control if J is full row rank 12 | if rank(J) == size(J,1) 13 | % compute Pseudoinverse 14 | Jpse = simplify(transpose(J)*(inv(J*(transpose(J))))) 15 | detJ= simplify(det(J*(transpose(J)))) 16 | else 17 | Jpse = pinv(J) 18 | end 19 | 20 | %% compute joint velocity 21 | qd = simplify(Jpse * pd) 22 | -------------------------------------------------------------------------------- /Scripts/2020/JBM_weightedpseudo.m: -------------------------------------------------------------------------------- 1 | %% Jacobian Based Method - Weighted Pseudoinverse 2 | function [qwd, JWpse] = JBM_weightedpseudo(J,pd) 3 | % takes as inputs: 4 | % - J = the jacobian matrix 5 | % - pd = the cartesian velocity 6 | % 7 | % output: 8 | % - qwd = weighted q dot 9 | % - JWpse = weighted pseudoinverse 10 | 11 | 12 | %% define W, weighted matrix 13 | syms w 14 | W=[1 0 0; 0 1 0; 0 0 w]; 15 | 16 | %% Control if J is full row rank 17 | if rank(J) == size(J,1) 18 | % compute Weighted Pseudoinverse 19 | secondPart = simplify(inv(J*inv(W)*transpose(J))); 20 | JWpse = simplify(inv(W)*transpose(J)*secondPart); 21 | pause 22 | detJW = simplify(det(J*inv(W)*transpose(J))); 23 | pause 24 | else 25 | disp('Oh my friend, there is a problem') 26 | end 27 | 28 | %% general solution to minimize objective function and understand 29 | %% which value we can choose for w 30 | vecdot= [sym('qd1'); sym('qd2'); sym('qd3');]; 31 | 32 | H = (1/2)*transpose(vecdot)*W*vecdot 33 | pause 34 | 35 | %% compute joint velocity 36 | qwd = simplify(JWpse * pd); 37 | 38 | -------------------------------------------------------------------------------- /Scripts/2020/MovingFrame_2R.m: -------------------------------------------------------------------------------- 1 | %% dynamic model of 2R planar robot under gravity 2 | %% using a Lagrangian formulation in symbolic form 3 | 4 | clear all 5 | close all 6 | clc 7 | 8 | %% define symbolic variables 9 | syms m1 m2 real 10 | syms I1xx I1yy I1zz real 11 | syms I2xx I2yy I2zz real %symbolic variables explicitly defined as real 12 | syms d1 d2 l1 l2 13 | syms q1 q2 real 14 | syms dq1 dq2 real 15 | syms ddq1 ddq2 real 16 | syms u1 u2 g0 real 17 | 18 | disp('**** dynamic model of 2R planar robot in a vertical plane ****') 19 | disp(' ') 20 | disp('[press return to proceed at every pause]') 21 | 22 | pause 23 | 24 | %% initialization 25 | v0=0; 26 | w0=0; 27 | %% Rotation Matrix R 28 | R1 = [cos(q1) -sin(q1) 0; 29 | sin(q1) cos(q1) 0; 30 | 0 0 1] 31 | 32 | R2 = [cos(q2) -sin(q2) 0; 33 | sin(q2) cos(q2) 0; 34 | 0 0 1] 35 | 36 | %% position of center of mass of each link 37 | rc1 = [-l1+d1; 0; 0;]; 38 | rc2 = [-l2+d2; 0; 0;]; 39 | 40 | %% position 41 | pc1=[d1*cos(q1);d1*sin(q1); 0]; 42 | pc2=[l1*cos(q1) + d2*cos(q1+q2); l1*sin(q1)+d2*sin(q1+q2);0]; 43 | 44 | 45 | disp(' ') 46 | disp('*START RECURSION OF ALGORITHM*') 47 | disp(' ') 48 | disp('*angular velocity of link 1 with respect to frame 1*') 49 | 50 | %% compute angular velocity w of link 1 51 | w1 = transpose(R1)*(w0+dq1*[0;0;1]) 52 | 53 | disp(' ') 54 | disp('*linear velocity of link 1 with respect to frame 1*') 55 | 56 | %% compute linear velocity v of link 1 57 | v1 = transpose(R1)*(v0+cross(w1,[l1*cos(q1);l1*sin(q1);0])) 58 | 59 | disp(' ') 60 | disp('*linear velocity of center od mass of link 1 with respect to frame 1*') 61 | 62 | %% compute velocity of center of mass of link 1 63 | vc1 = v1 + cross(w1,rc1) 64 | 65 | disp(' ') 66 | disp('*kinetic energy of link 1*') 67 | 68 | %% compute kinetic energy of joint 1 69 | T1=(1/2)*(I1zz + m1*d1^2)*dq1^2 70 | 71 | pause 72 | 73 | disp(' ') 74 | disp('*angular velocity of link 2 with respect to frame 2*') 75 | 76 | %% compute angular velocity w of link 1 77 | w2 = transpose(R2)*(w1+dq2*[0;0;1]) 78 | 79 | disp(' ') 80 | disp('*linear velocity of link 2 with respect to frame 2*') 81 | 82 | %% compute linear velocity v of link 1 83 | v2 = transpose(R2)*(v1+cross(w2,[l2*cos(q2);l2*sin(q2);0])) 84 | 85 | disp(' ') 86 | disp('*linear velocity of center od mass of link 1 with respect to frame 1*') 87 | 88 | %% compute velocity of center of mass of link 1 89 | vc2 = v2 + cross(w2,rc2) 90 | 91 | disp('*kinetic energy of link 2 *') 92 | 93 | %% kinetic energy of joint 2 94 | T2=1/2*m2*(l1^2*dq1^2+d1^2*(dq1+dq2)^2+2*l1*d2*cos(q2)*dq1*(dq1+dq2))+ 1/2*I2zz*(dq1+dq2)^2; 95 | pause 96 | 97 | disp('***robot kinetic energy***') 98 | 99 | %% total kinetic energy 100 | T=T1+T2; 101 | 102 | disp('*simplifying*') 103 | 104 | T=simplify(T1+T2) 105 | pause 106 | 107 | % collect in base of term that you pass it in this case, collect terms that has dq1^2 and 108 | % do the same for dq2^2 because you need them to compute M 109 | T=collect(T,dq1^2) 110 | T=collect(T,dq2^2) 111 | 112 | pause 113 | 114 | disp('***robot inertia matrix***') 115 | 116 | %% compute robot matrix M, inertia matrix 117 | 118 | % we extract the element M(1,1) from the total kinetic energy by doing twice the derivative 119 | % respect qd1 because this will be the factor that multiplyin the square of velocity of joint 1 120 | % we do the same also for joint 2 121 | % the factor 1/2 disappear automatically in this differentation 122 | 123 | a1 = I1zz + m1*d1^2 +I2zz+m2*d2^2+m2*l1^2; 124 | a2 = m2*l1*d2; 125 | a3 = I2zz+m2*d2^2; 126 | 127 | M(1,1) = a1+2*a2*cos(q2) 128 | M(1,2)= a3+a2*cos(q2); 129 | M(2,1) = a3+a2*cos(q2); 130 | M(2,2)=a3; 131 | pause 132 | 133 | disp('*Christoffel matrices*') 134 | 135 | %% compute the Christoffel Matrix 136 | q=[q1;q2]; 137 | 138 | % consider the first column of matrix M, so you have M1 139 | % do the same for M2 140 | M1=M(:,1); 141 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 142 | M2=M(:,2); 143 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 144 | 145 | pause 146 | 147 | disp('***robot centrifugal terms (no Coriolis here!)***') 148 | 149 | dq=[dq1;dq2]; 150 | c1=dq'*C1*dq; 151 | c2=dq'*C2*dq; 152 | c=[c1;c2] 153 | 154 | pause 155 | 156 | disp('*potential energy of link 1*') 157 | 158 | %% vector gravity acceleration 159 | g=[0;-g0;0]; 160 | 161 | %% compute the potential energy of link 1 162 | U1=-m1*transpose(g)*pc1 163 | 164 | pause 165 | 166 | disp('*potential energy of link 2*') 167 | 168 | %% compute the potential energy of link 2 169 | U2=-m2*transpose(g)*pc2 170 | 171 | pause 172 | 173 | disp('***robot potential energy (due to gravity)***') 174 | 175 | %% total potential energy 176 | U=simplify(U1+U2) 177 | 178 | pause 179 | 180 | disp('***robot gravity term***') 181 | 182 | %% compute G 183 | G=jacobian(U,q)' 184 | 185 | pause 186 | 187 | disp('***complete dynamic equations***') 188 | 189 | %% show complete dynamic equations 190 | M*[ddq1; ddq2]+c+G==[u1 u2]' 191 | 192 | disp('***end***') 193 | 194 | % end 195 | -------------------------------------------------------------------------------- /Scripts/2020/MovingFrame_3R.m: -------------------------------------------------------------------------------- 1 | %% dynamic model of 3R planar robot under gravity 2 | %% using a Lagrangian formulation in symbolic form 3 | 4 | clear all 5 | close all 6 | clc 7 | 8 | %% define symbolic variables 9 | syms m1 m2 m3 d 10 | syms I1xx I1yy I1zz real 11 | syms I2xx I2yy I2zz real 12 | syms I3xx I3yy I3zz real 13 | syms d1 d2 L1 L2 L3 real 14 | syms q1 q2 q3 real 15 | syms dq1 dq2 dq3 real 16 | syms ddq1 ddq2 ddq3 real 17 | syms u1 u2 u3 g0 real 18 | 19 | disp('**** dynamic model of 2R planar robot in a vertical plane ****') 20 | disp(' ') 21 | disp('[press return to proceed at every pause]') 22 | 23 | pause 24 | 25 | %% initialization 26 | w0=[0;0;0]; 27 | v0=[0;0;0]; 28 | %% R matrix 29 | 30 | R1 = [cos(q1) 0 sin(q1); 31 | sin(q1) 0 -cos(q1); 32 | 0 1 0]; 33 | 34 | R2 = [cos(q2) -sin(q2) 0; 35 | 36 | sin(q2) cos(q2) 0; 37 | 0 0 1;]; 38 | 39 | R3 = [cos(q3) -sin(q3) 0; 40 | sin(q3) cos(q3) 0; 41 | 0 0 1;]; 42 | 43 | %% position of center of mass of each link 44 | syms A F C D E real 45 | 46 | rc1 = [A;-F;0;]; 47 | rc2 = [-C;0;0]; 48 | rc3 = [-D;0;E]; 49 | 50 | %% positions of link 51 | pc1 = R1'*[0 0 L1]' 52 | pc2 = simplify(R2'*[L2*cos(q2) L2*sin(q2) 0]') 53 | pc3 = simplify(R3'*[L3*cos(q3) L3*sin(q3) 0]') 54 | 55 | pause 56 | disp('*START RECURSION OF ALGORITHM*') 57 | disp(' ') 58 | 59 | %% compute angular velocity w of link 1 60 | w1 = transpose(R1)*(w0+dq1*[0;0;1]); 61 | 62 | %% compute linear velocity v of link 1 63 | v1 = transpose(R1)*v0 + cross(w1, pc1); 64 | 65 | %% compute velocity of center of mass of link 1 66 | vc1 = v1 + cross(w1,rc1) 67 | 68 | disp(' ') 69 | disp('*kinetic energy of link 1*') 70 | 71 | %% compute kinetic energy of joint 1 72 | Tl1=(1/2)*m1*vc1'*vc1; 73 | Ta1 = 1/2*w1'*diag([I1xx I1yy I1zz])*w1; 74 | 75 | T1 = simplify(Tl1+Ta1) 76 | pause 77 | 78 | disp('*angular velocity of link 1 with respect to frame 2*') 79 | 80 | %% compute angular velocity w of link 2 81 | w2 = transpose(R2)*(w1+dq2*[0;0;1]); 82 | 83 | %% compute linear velocity v of link 2 84 | v2 = transpose(R2)*v1 +cross(w2, pc2); 85 | 86 | %% compute velocity of center of mass of link 2 87 | vc2 = v2 + cross(w2,rc2) 88 | pause 89 | disp('*kinetic energy of link 2*') 90 | 91 | %% compute kinetic energy of joint 2 92 | Tl2=(1/2)*m2*vc2'*vc2; 93 | Ta2 = 1/2*w2'*diag([I2xx I2yy I2zz])*w2; 94 | 95 | T2=simplify(Tl2+Ta2) 96 | pause 97 | 98 | disp('*angular velocity of link 1 with respect to frame 3*') 99 | 100 | %% compute angular velocity w of link 3 101 | w3 = transpose(R3)*(w2+dq3*[0;0;1]); 102 | 103 | %% compute linear velocity v of link 2 104 | v3 = transpose(R3)*v2+cross(w3, pc3); 105 | 106 | %% compute velocity of center of mass of link 2 107 | vc3 = v3 + cross(w3,rc3) 108 | pause 109 | disp('*kinetic energy of link 3*') 110 | 111 | %% compute kinetic energy of joint 2 112 | Tl3=(1/2)*m3*vc3'*vc3; 113 | Ta3 = 1/2*w3'*diag([I3xx I3yy I3zz])*w3; 114 | 115 | T3=simplify(Tl3+Ta3) 116 | pause 117 | 118 | T=simplify(T1+T2+T3) 119 | 120 | T=collect(T,dq1^2); 121 | T=collect(T,dq2^2); 122 | T=collect(T,dq3^2); 123 | 124 | 125 | %% compute M 126 | 127 | M(1,1)=diff(T,dq1,2); 128 | M(2,2)=diff(T,dq2,2); 129 | M(3,3)=diff(T,dq3,2); 130 | 131 | TempB12=diff(T,dq1); 132 | M(1,2)=diff(TempB12,dq2); 133 | M(2,1)=M(1,2); 134 | 135 | M(1,3)=diff(TempB12,dq3); 136 | M(3,1)=M(1,3); 137 | 138 | TempB2=diff(T,dq2); 139 | M(2,3)=diff(TempB2,dq3); 140 | M(3,2)=M(2,3); 141 | M=simplify(M) -------------------------------------------------------------------------------- /Scripts/2020/Saturation.m: -------------------------------------------------------------------------------- 1 | %% Saturation for 3R 2 | 3 | %% Value of qd 4 | qd=[0; 0; 0;]; 5 | 6 | %% Bounds 7 | b1 = 2.8; 8 | b2 = 3.6; 9 | b3 = 4; 10 | 11 | %% Subs value 12 | J = simplify(subs(J,{q1,q2,q3},{pi/6,pi/6,pi/6})) 13 | pause 14 | Jd_3R = simplify(subs(Jd_3R,{q1,q2,q3,qd1,qd2,qd3},{pi/6,pi/6,pi/6,qd(1),qd(2),qd(3)})) 15 | pause 16 | rho = rank(J); 17 | 18 | %% Compute qdd_ps 19 | qdd_ps= simplify(pinv(J))* (pdd - Jd_3R*qd); 20 | 21 | %% Adjust bound limit 22 | pdd_new = pdd - J(1:2,3)*(-b3); 23 | 24 | %% Compute q_new,where it contains it contains the variables 25 | %% that we have not adjusted, but that we need to recalculate 26 | % use pinv because in this way if J is not square but it has full 27 | % row rank, this function compute pseudoinverse 28 | q_new = simplify(pinv(J(1:2,1:2))*pdd_new); 29 | 30 | %% Final value qdd and norm 31 | qdd = [-b3; q_new] 32 | norm_qdd = simplify(norm(qdd)) 33 | -------------------------------------------------------------------------------- /Scripts/2020/dynmod_2Rrobot.m: -------------------------------------------------------------------------------- 1 | 2 | %% dynamic model of 2R planar robot under gravity 3 | %% using a Lagrangian formulation in symbolic form 4 | 5 | clear all 6 | close all 7 | clc 8 | 9 | %% define symbolic variables 10 | syms m1 m2 real 11 | syms L1 L2 L real 12 | syms d1 d2 real 13 | syms I1xx I1yy I1zz real %symbolic variables explicitly defined as real 14 | syms I2xx I2yy I2zz real %symbolic variables explicitly defined as real 15 | syms q1 q2 real 16 | syms dq1 dq2 real 17 | syms ddq1 ddq2 real 18 | syms u1 u2 g0 real 19 | 20 | disp('**** dynamic model of 2R planar robot in a vertical plane ****') 21 | disp(' ') 22 | disp('[press return to proceed at every pause]') 23 | 24 | pause 25 | 26 | disp(' ') 27 | disp('*kinetic energy of link 1 - linear part*') 28 | 29 | %% compute linear part of kinetic energy of joint 1 30 | pc1 =[(L/2)*cos(q1) (L/2)*sin(q1) 0]'; 31 | vc1 = diff(pc1,q1)*dq1; 32 | Tl1 = (1/2)*m1*vc1'*vc1; 33 | 34 | disp('*kinetic energy of link 1 - angular part*') 35 | 36 | %% compute the angular part of kinetic energy of joint 2 37 | %om1 = [0 dq1 0]'; 38 | om1 = [0 0 dq1]'; 39 | Ta1 = (1/2)*om1'*diag([I1xx I1yy I1zz])*om1; 40 | 41 | T1= simplify(Tl1+Ta1) 42 | pause 43 | 44 | disp('*kinetic energy of link 2 - linear part*') 45 | 46 | %% compute the linear part of kinetic energy of joint 2 47 | %pc2 = [d2*cos(q1)*cos(q2) d2*cos(q2)*sin(q1) L1+d2*sin(q2)]'; 48 | pc2 = [L*cos(q1)+(L/2)*cos(q1+q2) L*sin(q1)+(L/2)*sin(q1+q2) 0]'; 49 | vc2 = simplify(diff(pc2,q1)*dq1+diff(pc2,q2)*dq2); 50 | T2l = simplify((1/2)*m2*vc2'*vc2); 51 | 52 | disp('*kinetic energy of link 2 - angular part*') 53 | 54 | %% compute the angular part of kinetic energy of joint 2 55 | %om2=[dq1*sin(q2) dq1*cos(q2) dq2]'; 56 | om2=[0 0 dq1+dq2]'; 57 | T2a=simplify((1/2)*om2'*diag([I2xx I2yy I2zz])*om2); 58 | 59 | %% kinetic energy of joint 2 60 | T2=simplify(T2l+T2a) 61 | pause 62 | 63 | disp('***robot kinetic energy***') 64 | 65 | %% total kinetic energy 66 | T=T1+T2; 67 | 68 | disp('*simplifying*') 69 | 70 | T=simplify(T1+T2); 71 | 72 | % collect in base of term that you pass it in this case, collect terms that has dq1^2 and 73 | % do the same for dq2^2 because you need them to compute M 74 | T=collect(T,dq1^2); 75 | T=collect(T,dq2^2) 76 | pause 77 | 78 | disp('***robot inertia matrix***') 79 | 80 | %% compute robot matrix M, inertia matrix 81 | 82 | % we extract the element M(1,1) from the total kinetic energy by doing twice the derivative 83 | % respect qd1 because this will be the factor that multiplyin the square of velocity of joint 1 84 | % we do the same also for joint 2 85 | % the factor 1/2 disappear automatically in this differentation 86 | 87 | M(1,1)=diff(T,dq1,2); 88 | M(2,2)=diff(T,dq2,2); 89 | 90 | TempB1=diff(T,dq1); 91 | M(1,2)=diff(TempB1,dq2); 92 | M(2,1)=M(1,2); 93 | 94 | 95 | M=simplify(M) 96 | pause 97 | 98 | %% parametrization 99 | syms a1 a2 a3 a4 real 100 | % M=[ a1 a3*cos(q1-q2); 101 | % a3*cos(q1-q2) a2;] 102 | disp('*Christoffel matrices*') 103 | 104 | %% compute the Christoffel Matrix 105 | q=[q1;q2;]; 106 | 107 | % consider the first column of matrix M, so you have M1 108 | % do the same for M2 109 | M1=M(:,1); 110 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 111 | M2=M(:,2); 112 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 113 | 114 | 115 | pause 116 | 117 | disp('***robot centrifugal terms (no Coriolis here!)***') 118 | 119 | dq=[dq1;dq2]; 120 | c1=dq'*C1*dq; 121 | c2=dq'*C2*dq; 122 | c=simplify([c1;c2]) 123 | 124 | pause 125 | 126 | disp('*potential energy of link 1*') 127 | 128 | 129 | %% vector gravity acceleration 130 | g=[0;-g0;0]; 131 | 132 | %% compute the potential energy of link 1 133 | U1=m1*g'*[0;d1;0] 134 | 135 | pause 136 | 137 | disp('*potential energy of link 2*') 138 | 139 | %% compute the potential energy of link 2 140 | U2=m2*g'*[0;d2*sin(q2);0] 141 | pause 142 | 143 | disp('***robot potential energy (due to gravity)***') 144 | 145 | %% total potential energy 146 | U=U1+U2 147 | 148 | pause 149 | 150 | disp('***robot gravity term***') 151 | 152 | %% compute G 153 | G=jacobian(U,q)' 154 | 155 | pause 156 | 157 | disp('***complete dynamic equations***') 158 | 159 | %% show complete dynamic equations 160 | M*[ddq1; ddq2]+c+G==[u1 u2]' 161 | 162 | disp('***end***') 163 | 164 | % end 165 | -------------------------------------------------------------------------------- /Scripts/2020/dynmod_2Rrobot_motor.m: -------------------------------------------------------------------------------- 1 | %% dynamic model of 2R motor planar robot under gravity 2 | %% using a Lagrangian formulation in symbolic form 3 | 4 | clear all 5 | close all 6 | clc 7 | 8 | %% define symbolic variables 9 | syms m1 m2 real 10 | syms mm1 mm2 real 11 | syms L1 L2 real 12 | syms d1 d2 real 13 | syms I1xx I1yy I1zz real %symbolic variables explicitly defined as real 14 | syms Im1xx Im1yy Im1zz real %symbolic variables explicitly defined as real 15 | syms I2xx I2yy I2zz real %symbolic variables explicitly defined as real 16 | syms Im2xx Im2yy Im2zz real %symbolic variables explicitly defined as real 17 | syms t1 t2 real 18 | syms tm1 tm2 real 19 | syms dt1 dt2 real 20 | syms dtm1 dtm2 real 21 | syms ddt1 ddt2 real 22 | syms ddtm1 ddtm2 real 23 | syms u1 u2 u3 u4 g0 real 24 | 25 | disp('**** dynamic model of 2R planar robot in a vertical plane ****') 26 | disp(' ') 27 | disp('[press return to proceed at every pause]') 28 | 29 | pause 30 | 31 | disp(' ') 32 | disp('*kinetic energy of link 1 - linear part*') 33 | 34 | %% compute linear part of kinetic energy of joint 1 35 | pc1 =[d1*cos(t1) d1*sin(t1) 0]'; 36 | vc1 = diff(pc1,t1)*dt1; 37 | Tl1 = (1/2)*m1*vc1'*vc1; 38 | 39 | disp('*kinetic energy of link 1 - angular part*') 40 | 41 | %% compute the angular part of kinetic energy of joint 2 42 | %om1 = [0 dq1 0]'; 43 | om1 = [0 0 dt1]'; 44 | Ta1 = (1/2)*om1'*diag([I1xx I1yy I1zz])*om1; 45 | 46 | T1= simplify(Tl1+Ta1) 47 | pause 48 | 49 | disp('*kinetic energy of link 2 - linear part*') 50 | 51 | %% compute the linear part of kinetic energy of joint 2 52 | %pc2 = [d2*cos(q1)*cos(q2) d2*cos(q2)*sin(q1) L1+d2*sin(q2)]'; 53 | pc2 = [L1*cos(t1)+d2*cos(t2) L1*sin(t1)+d2*sin(t2) 0]'; 54 | vc2 = simplify(diff(pc2,t1)*dt1+diff(pc2,t2)*dt2); 55 | T2l = simplify((1/2)*m2*vc2'*vc2); 56 | 57 | disp('*kinetic energy of link 2 - angular part*') 58 | 59 | %% compute the angular part of kinetic energy of joint 2 60 | %om2=[dq1*sin(q2) dq1*cos(q2) dq2]'; 61 | om2=[0 0 dt2]'; 62 | T2a=simplify((1/2)*om2'*diag([I2xx I2yy I2zz])*om2); 63 | 64 | %% kinetic energy of joint 2 65 | T2=simplify(T2l+T2a) 66 | pause 67 | 68 | disp('***robot motor energy***') 69 | omm1=[0 0 dtm1]'; 70 | Tm1 = simplify((1/2)*omm1'*diag([Im1xx Im1yy Im1zz])*omm1) 71 | pause 72 | 73 | omm2=[0 0 dtm2+dt1]'; 74 | Tm2 = simplify((1/2)*omm2'*diag([Im2xx Im2yy Im2zz])*omm2) + (1/2)*mm2*L1^2*dt1^2 75 | pause 76 | 77 | disp('***robot kinetic energy***') 78 | 79 | %% 80 | %% total kinetic energy 81 | T=T1+T2+Tm1+Tm2; 82 | 83 | disp('*simplifying*') 84 | 85 | T=simplify(T); 86 | 87 | % collect in base of term that you pass it in this case, collect terms that has dq1^2 and 88 | % do the same for dq2^2 because you need them to compute M 89 | T=collect(T,dt1^2); 90 | T=collect(T,dt2^2); 91 | T=collect(T,dtm1^2); 92 | T=collect(T,dtm2^2) 93 | pause 94 | 95 | disp('***robot inertia matrix***') 96 | 97 | %% compute robot matrix M, inertia matrix 98 | 99 | % we extract the element M(1,1) from the total kinetic energy by doing twice the derivative 100 | % respect qd1 because this will be the factor that multiplyin the square of velocity of joint 1 101 | % we do the same also for joint 2 102 | % the factor 1/2 disappear automatically in this differentation 103 | M(1,1)=diff(T,dt1,2); 104 | M(2,2)=diff(T,dt2,2); 105 | M(3,3)= diff(T,dtm1,2); 106 | M(4,4)=diff(T,dtm2,2); 107 | 108 | TempB1=diff(T,dt1); 109 | M(1,2)=diff(TempB1,dt2); 110 | M(2,1)=M(1,2); 111 | 112 | M(1,3)=diff(TempB1,dtm1); 113 | M(3,1)=M(1,3); 114 | 115 | M(1,4)=diff(TempB1,dtm2); 116 | M(4,1)=M(1,4); 117 | 118 | TempB2=diff(T,dt2); 119 | M(2,3) = diff(TempB2,dtm1); 120 | M(3,2)=M(2,3); 121 | 122 | M(2,4)=diff(TempB2,dtm2); 123 | M(4,2)=M(2,4); 124 | 125 | TempB3=diff(T,dtm1); 126 | M(3,4)=diff(TempB3,dtm2); 127 | M(4,3)=M(3,4); 128 | 129 | M=simplify(M) 130 | 131 | pause 132 | 133 | %% parametrization 134 | syms a1 a2 a3 a4 real 135 | % M=[ a1 a3*cos(q1-q2); 136 | % a3*cos(q1-q2) a2;] 137 | disp('*Christoffel matrices*') 138 | 139 | %% compute the Christoffel Matrix 140 | q=[t1;t2;tm1;tm2]; 141 | 142 | % consider the first column of matrix M, so you have M1 143 | % do the same for M2 144 | M1=M(:,1); 145 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,t1)) 146 | M2=M(:,2); 147 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,t2)) 148 | M3=M(:,1); 149 | C3=(1/2)*(jacobian(M3,q)+jacobian(M3,q)'-diff(M,tm1)) 150 | M4=M(:,2); 151 | C4=(1/2)*(jacobian(M4,q)+jacobian(M4,q)'-diff(M,tm2)) 152 | pause 153 | 154 | disp('***robot centrifugal terms (no Coriolis here!)***') 155 | 156 | dq=[dt1;dt2;dtm1;dtm2]; 157 | c1=dq'*C1*dq; 158 | c2=dq'*C2*dq; 159 | c3=dq'*C3*dq; 160 | c4=dq'*C4*dq; 161 | c=[c1;c2;c3;c4] 162 | 163 | pause 164 | 165 | disp('*potential energy of link 1*') 166 | 167 | 168 | %% vector gravity acceleration 169 | g=[0;-g0;0]; 170 | 171 | %% compute the potential energy of link 1 172 | U1=m1*g'*[0;d1;0] 173 | 174 | pause 175 | 176 | disp('*potential energy of link 2*') 177 | 178 | %% compute the potential energy of link 2 179 | U2=m2*g'*[0;d2*sin(q2);0] 180 | pause 181 | 182 | disp('***robot potential energy (due to gravity)***') 183 | 184 | %% total potential energy 185 | U=U1+U2 186 | 187 | pause 188 | 189 | disp('***robot gravity term***') 190 | 191 | %% compute G 192 | G=jacobian(U,q)' 193 | 194 | pause 195 | 196 | disp('***complete dynamic equations***') 197 | 198 | %% show complete dynamic equations 199 | M*[ddq1; ddq2]+c+G==[u1 u2]' 200 | 201 | disp('***end***') 202 | 203 | % end 204 | -------------------------------------------------------------------------------- /Scripts/2020/dynmod_3Rrobot.m: -------------------------------------------------------------------------------- 1 | %% dynamic model of 2R planar robot under gravity 2 | %% using a Lagrangian formulation in symbolic form 3 | 4 | clear all 5 | close all 6 | clc 7 | 8 | %% define symbolic variables 9 | syms m1 m2 m3 m real 10 | syms L1 L2 L3 L real 11 | syms d1 d2 d3 real 12 | syms I1xx I1yy I1zz real %symbolic variables explicitly defined as real 13 | syms I2xx I2yy I2zz real %symbolic variables explicitly defined as real 14 | syms I3xx I3yy I3zz real %symbolic variables explicitly defined as real 15 | syms Il real 16 | syms q1 q2 q3 real 17 | syms dq1 dq2 dq3 real 18 | syms ddq1 ddq2 ddq3 real 19 | syms u1 u2 u3 g0 real 20 | 21 | disp('**** dynamic model of 2R planar robot in a vertical plane ****') 22 | disp(' ') 23 | disp('[press return to proceed at every pause]') 24 | 25 | pause 26 | 27 | disp(' ') 28 | disp('*kinetic energy of link 1 - linear part*') 29 | 30 | %% compute linear part of kinetic energy of joint 1 31 | %pc1 =[(L/2)*cos(q1) (L/2)*sin(q1) 0]'; 32 | vc1 = diff(pc1,q1)*dq1; 33 | Tl1 = (1/2)*m1*vc1'*vc1; 34 | 35 | disp('*kinetic energy of link 1 - angular part*') 36 | 37 | %% compute the angular part of kinetic energy of joint 2 38 | om1 = [0 0 dq1]'; 39 | Ta1 = (1/2)*om1'*diag([I1xx I1yy I1zz])*om1; 40 | 41 | T1= simplify(Tl1+Ta1) 42 | pause 43 | 44 | disp('*kinetic energy of link 2 - linear part*') 45 | 46 | %% compute the linear part of kinetic energy of joint 2 47 | pc2 = [(L/2)*cos(q2+q1)+L*cos(q1) (L/2)*sin(q2+q1)+L*sin(q1) 0]'; 48 | vc2 = simplify(diff(pc2,q1)*dq1+diff(pc2,q2)*dq2); 49 | T2l = simplify((1/2)*m2*vc2'*vc2); 50 | 51 | disp('*kinetic energy of link 2 - angular part*') 52 | 53 | %% compute the angular part of kinetic energy of joint 2 54 | om2=[0 0 dq2+dq1]'; 55 | T2a=simplify((1/2)*om2'*diag([I2xx I2yy I2zz])*om2); 56 | 57 | %% kinetic energy of joint 2 58 | T2=simplify(T2l+T2a) 59 | pause 60 | 61 | disp('*kinetic energy of link 3 - linear part*') 62 | 63 | %% compute the linear part of kinetic energy of joint 2 64 | pc3 = [L*cos(q2+q1)+L*cos(q1)+(L/2)*cos(q2+q1+q3) L*sin(q2+q1)+L*sin(q1)+(L/2)*cos(q1+q2+q3) 0]'; 65 | vc3 = simplify(diff(pc3,q1)*dq1+diff(pc3,q2)*dq2)+diff(pc3,q2)*dq3; 66 | T3l = simplify((1/2)*m3*vc3'*vc3); 67 | 68 | disp('*kinetic energy of link 3 - angular part*') 69 | 70 | %% compute the angular part of kinetic energy of joint 2 71 | om3=[0 0 dq2+dq1+dq3]'; 72 | T3a=simplify((1/2)*om3'*diag([I3xx I3yy I3zz])*om3); 73 | 74 | T3 = T3l+T3a 75 | pause 76 | 77 | %% total kinetic energy 78 | T=T1+T2+T3; 79 | 80 | disp('*simplifying*') 81 | 82 | T=simplify(T1+T2+T3); 83 | 84 | % collect in base of term that you pass it in this case, collect terms that has dq1^2 and 85 | % do the same for dq2^2 because you need them to compute M 86 | T=collect(T,dq1^2); 87 | T=collect(T,dq2^2); 88 | T=collect(T,dq3^2) 89 | pause 90 | 91 | disp('***robot inertia matrix***') 92 | 93 | %% compute robot matrix M, inertia matrix 94 | 95 | % we extract the element M(1,1) from the total kinetic energy by doing twice the derivative 96 | % respect qd1 because this will be the factor that multiplyin the square of velocity of joint 1 97 | % we do the same also for joint 2 98 | % the factor 1/2 disappear automatically in this differentation 99 | M(1,1)=diff(T,dq1,2); 100 | M(2,2)=diff(T,dq2,2); 101 | M(3,3)=diff(T,dq3,2); 102 | 103 | TempB12=diff(T,dq1); 104 | M(1,2)=diff(TempB12,dq2); 105 | M(2,1)=M(1,2); 106 | M(1,3)=diff(TempB12,dq3); 107 | M(3,1)=M(1,3); 108 | 109 | TempB23 = diff(T,dq2); 110 | M(2,3)=diff(TempB23,dq3); 111 | M(3,2)=M(2,3); 112 | 113 | M = simplify(M) 114 | 115 | pause 116 | 117 | %% parametrization 118 | % syms a1 a2 a3 a4 real 119 | % M=[ a1 a3*cos(q1-q2); 120 | % a3*cos(q1-q2) a2;] 121 | % disp('*Christoffel matrices*') 122 | 123 | %% compute the Christoffel Matrix 124 | q=[q1;q2;q3]; 125 | 126 | % consider the first column of matrix M, so you have M1 127 | % do the same for M2 128 | M1=M(:,1); 129 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 130 | M2=M(:,2); 131 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 132 | M3=M(:,3); 133 | C3=(1/2)*(jacobian(M3,q)+jacobian(M3,q)'-diff(M,q3)) 134 | pause 135 | 136 | disp('***robot centrifugal terms (no Coriolis here!)***') 137 | 138 | dq=[dq1;dq2;dq3]; 139 | c1=dq'*C1*dq; 140 | c2=dq'*C2*dq; 141 | c3=dq'*C3*dq; 142 | 143 | c=[c1;c2;c3] 144 | 145 | pause 146 | 147 | disp('*potential energy of link 1*') 148 | 149 | 150 | %% vector gravity acceleration 151 | g=[0;-g0;0]; 152 | 153 | %% compute the potential energy of link 1 154 | U1=m1*g'*[0;d1;0] 155 | 156 | pause 157 | 158 | disp('*potential energy of link 2*') 159 | 160 | %% compute the potential energy of link 2 161 | U2=m2*g'*[0;d2*sin(q2);0] 162 | pause 163 | 164 | disp('***robot potential energy (due to gravity)***') 165 | 166 | %% total potential energy 167 | U=U1+U2 168 | 169 | pause 170 | 171 | disp('***robot gravity term***') 172 | 173 | %% compute G 174 | G=jacobian(U,q)' 175 | 176 | pause 177 | 178 | disp('***complete dynamic equations***') 179 | 180 | %% show complete dynamic equations 181 | M*[ddq1; ddq2]+c+G==[u1 u2]' 182 | 183 | disp('***end***') 184 | 185 | % end 186 | -------------------------------------------------------------------------------- /Scripts/2020/dynmod_PPRRrobot.m: -------------------------------------------------------------------------------- 1 | %% dynamic model of PPRR planar robot under gravity 2 | %% using a Lagrangian formulation in symbolic form 3 | 4 | clear all 5 | close all 6 | clc 7 | 8 | %% define symbolic variables 9 | syms m1 m2 m3 m4 real 10 | syms d1 d2 d3 d4 real 11 | syms L1 L2 L3 L4 real 12 | syms I1xx I1yy I1zz real %symbolic variables explicitly defined as real 13 | syms I2xx I2yy I2zz real %symbolic variables explicitly defined as real 14 | syms I3xx I3yy I3zz real %symbolic variables explicitly defined as real 15 | syms I4xx I4yy I4zz real %symbolic variables explicitly defined as real 16 | syms q1 q2 q3 q4 real 17 | syms dq1 dq2 dq3 dq4 real 18 | syms ddq1 ddq2 ddq3 ddq4 real 19 | syms u1 u2 u3 u4 g0 real 20 | 21 | disp('**** dynamic model of RRRR planar robot in a vertical plane ****') 22 | disp(' ') 23 | disp('[press return to proceed at every pause]') 24 | 25 | pause 26 | 27 | disp(' ') 28 | disp('*kinetic energy of link 1*') 29 | 30 | %% compute kinetic energy of joint 1 31 | pc1 = [-(d1-q1); 0;0]; 32 | vc1=diff(pc1,q1)*dq1; 33 | T1 = 1/2*m1*vc1'*vc1 34 | pause 35 | disp('*kinetic energy of link 2') 36 | 37 | %% compute kinetic energy of joint 2 38 | pc2 = [q1; q2-d2;0]; 39 | vc2=diff(pc2,q1)*dq1+diff(pc2,q2)*dq2; 40 | T2 = 1/2*m2*vc2'*vc2 41 | pause 42 | 43 | disp('*kinetic energy of link 3') 44 | 45 | %% compute linear part of linear kinetic energy of joint 3 46 | pc3 = [q1+d3*cos(q3); q2+d3*sin(q3);0]; 47 | vc3=diff(pc3,q1)*dq1+diff(pc3,q2)*dq2++diff(pc3,q3)*dq3; 48 | Tl3 = 1/2*m3*vc3'*vc3; 49 | 50 | 51 | %% compute the angular part of kinetic energy of joint 3 52 | om3 = [0 0 dq3]'; 53 | Ta3 = 1/2*om3'*diag([I3xx I3yy I3zz])*om3; 54 | 55 | T3= simplify(Tl3+Ta3) 56 | pause 57 | disp('*kinetic energy of link 4') 58 | 59 | %% compute linear part of linear kinetic energy of joint 4 60 | pc4 = [q1+L3*cos(q3)+d4*cos(q4+q3); q2+L3*sin(q3)+d4*sin(q4+q3);0]; 61 | vc4=diff(pc4,q1)*dq1+diff(pc4,q2)*dq2+diff(pc4,q3)*dq3+diff(pc4,q4)*dq4; 62 | Tl4 = 1/2*m4*vc4'*vc4; 63 | 64 | %% compute the angular part of kinetic energy of joint 3 65 | om4 = [0 0 dq4+dq3]'; 66 | Ta4 = 1/2*om4'*diag([I4xx I4yy I4zz])*om4; 67 | 68 | T4= simplify(Tl4+Ta4) 69 | pause 70 | 71 | T=simplify(T1+T2+T3+T4) 72 | pause 73 | 74 | T=collect(T,dq1^2); 75 | T=collect(T,dq2^2); 76 | T=collect(T,dq3^2); 77 | T=collect(T,dq4^2); 78 | disp('***robot inertia matrix***') 79 | 80 | %% compute robot matrix M, inertia matrix 81 | 82 | M(1,1)=diff(T,dq1,2); 83 | M(2,2)=diff(T,dq2,2); 84 | M(3,3)= diff(T,dq3,2); 85 | M(4,4)=diff(T,dq4,2); 86 | 87 | TempB1=diff(T,dq1); 88 | M(1,2)=diff(TempB1,dq2); 89 | M(2,1)=M(1,2); 90 | 91 | M(1,3)=diff(TempB1,dq3); 92 | M(3,1)=M(1,3); 93 | 94 | M(1,4)=diff(TempB1,dq4); 95 | M(4,1)=M(1,4); 96 | 97 | TempB2=diff(T,dq2); 98 | M(2,3) = diff(TempB2,dq3); 99 | M(3,2)=M(2,3); 100 | 101 | M(2,4)=diff(TempB2,dq4); 102 | M(4,2)=M(2,4); 103 | 104 | TempB3=diff(T,dq3); 105 | M(3,4)=diff(TempB3,dq4); 106 | M(4,3)=M(3,4); 107 | 108 | M=simplify(M) 109 | pause 110 | disp('*parametrized*') 111 | 112 | %% M in parametrized form 113 | 114 | syms a1 a2 a3 a4 a5 a6 a7 real 115 | 116 | M=[a1 0 -a6*sin(q3+q4)-a5*sin(q3) -a6*sin(q3+q4); 117 | 0 a2 a6*cos(q3+q4)+a5*cos(q3) a6*cos(q3+q4); 118 | -a6*sin(q3+q4)-a5*sin(q3) a6*cos(q3+q4)+a5*cos(q3) a3+2*a6*L3*cos(q4) a4+a6*L3*cos(q4); 119 | -a6*sin(q3+q4) a6*cos(q3+q4) a4+a6*L3*cos(q4) a4;] 120 | 121 | disp('*Christoffel matrices*') 122 | 123 | 124 | %% compute the Christoffel Matrix 125 | q=[q1;q2;q3;q4]; 126 | 127 | 128 | M1=M(:,1); 129 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 130 | pause 131 | 132 | M2=M(:,2); 133 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 134 | pause 135 | 136 | M3=M(:,3); 137 | C3=(1/2)*(jacobian(M3,q)+jacobian(M3,q)'-diff(M,q3)) 138 | pause 139 | 140 | M4=M(:,4); 141 | C4=(1/2)*(jacobian(M4,q)+jacobian(M4,q)'-diff(M,q4)) 142 | pause 143 | 144 | disp('***robot centrifugal terms (no Coriolis here!)***') 145 | 146 | dq=[dq1;dq2;dq3;dq4]; 147 | c1=dq'*C1*dq; 148 | c2=dq'*C2*dq; 149 | c3=dq'*C3*dq; 150 | c4=dq'*C4*dq; 151 | c=[c1;c2;c3;c4] 152 | pause 153 | 154 | disp('*potential energy of link 1*') 155 | %% vector gravity acceleration 156 | g=[0;-g0;0]; 157 | 158 | %% compute the potential energy of link 1 159 | U1=-m1*g'*pc1; 160 | 161 | disp('*potential energy of link 2*') 162 | 163 | 164 | %% compute the potential energy of link 2 165 | U2=-m2*g'*pc2; 166 | 167 | disp('*potential energy of link 3*') 168 | %% compute the potential energy of link 3 169 | U3=-m3*g'*pc3; 170 | 171 | disp('*potential energy of link 4*') 172 | 173 | %% compute the potential energy of link 4 174 | U4=-m4*g'*pc4; 175 | 176 | U=simplify(U1+U2+U3+U4) 177 | pause 178 | 179 | disp('***robot gravity term***') 180 | 181 | G=jacobian(U,q)' 182 | pause 183 | 184 | %Gp = [0; a2*g0; g0*(a6*cos(q3)+a5*cos(q3+q4)); a5*g0] 185 | 186 | 187 | disp('***complete dynamic equations***') 188 | 189 | %% show complete dynamic equations 190 | M*[ddq1; ddq2; ddq3; ddq4]+c+G==[u1 u2 u3 u4]' 191 | 192 | ris = collect(M*[ddq1; ddq2; ddq3; ddq4]+c+G,a1); 193 | ris = collect(ris,a2); 194 | ris = collect(ris,a3); 195 | ris = collect(ris,a4); 196 | ris = collect(ris,a5); 197 | ris = collect(ris,a6) 198 | 199 | pause 200 | 201 | 202 | -------------------------------------------------------------------------------- /Scripts/2020/dynmod_PPRrobot.m: -------------------------------------------------------------------------------- 1 | %% dynamic model of PPR planar robot under gravity 2 | %% using a Lagrangian formulation in symbolic form 3 | 4 | clear all 5 | close all 6 | clc 7 | 8 | %% define symbolic variables 9 | syms m1 m2 m3 real 10 | syms d1 d2 d3 real 11 | syms L1 L2 L3 real 12 | syms I1xx I1yy I1zz real 13 | syms I2xx I2yy I2zz real 14 | syms I3xx I3yy I3zz real 15 | syms q1 q2 q3 real 16 | syms dq1 dq2 dq3 real 17 | syms ddq1 ddq2 ddq3 real 18 | syms u1 u2 u3 g0 real 19 | 20 | disp('**** dynamic model of PPR planar robot in a vertical plane ****') 21 | disp(' ') 22 | disp('[press return to proceed at every pause]') 23 | 24 | pause 25 | 26 | disp(' ') 27 | disp('*kinetic energy of link 1*') 28 | 29 | %% compute kinetic energy of joint 1 30 | pc1 = [0 q1-d1 0]'; 31 | vc1 = diff(pc1,q1)*dq1; 32 | 33 | T1 = simplify((1/2)*m1*vc1'*vc1) 34 | pause 35 | 36 | disp('*kinetic energy of link 2*') 37 | 38 | %% compute kinetic energy of joint 2 39 | pc2 = [q2-d2 q1 0]'; 40 | vc2 = diff(pc2,q1)*dq1 + diff(pc2,q2)*dq2; 41 | 42 | T2 = simplify((1/2)*m2*vc2'*vc2) 43 | pause 44 | 45 | disp('*kinetic energy of link 3*') 46 | 47 | %% compute linear part of kinetic energy of joint 3 48 | pc3 = [q2+d3*cos(q3) q1+d3*sin(q3) 0]'; 49 | vc3 = simplify(diff(pc3, q1)*dq1+diff(pc3,q2)*dq2 + diff(pc3,q3)*dq3); 50 | Tl3 = simplify((1/2)*m3*vc3'*vc3); 51 | 52 | %% compute angular part of kinetic energy of joint 3 53 | om3 = [0 0 dq3]'; 54 | Ta3 = simplify((1/2)*om3'*diag([I3xx I3yy I3zz])*om3); 55 | 56 | T3 = simplify(Tl3+Ta3) 57 | pause 58 | 59 | T=simplify(T1+T2+T3); 60 | 61 | T = collect(T,dq1^2); 62 | T = collect(T,dq2^2); 63 | T = collect(T,dq3^2) 64 | pause 65 | 66 | %% compute inertia matrix 67 | 68 | M(1,1)=diff(T,dq1,2); 69 | M(2,2)=diff(T,dq2,2); 70 | M(3,3)=diff(T,dq3,2); 71 | 72 | TempB12=diff(T,dq1); 73 | M(1,2)=diff(TempB12,dq2); 74 | M(2,1)=M(1,2); 75 | M(1,3)=diff(TempB12,dq3); 76 | M(3,1)=M(1,3); 77 | 78 | TempB23 = diff(T,dq2); 79 | M(2,3)=diff(TempB23,dq3); 80 | M(3,2)=M(2,3); 81 | 82 | M = simplify(M) 83 | pause 84 | 85 | %% parametrization 86 | syms a1 a2 a3 a4 real 87 | 88 | M=[a1 0 a4*cos(q3); 89 | 0 a2 -a4*sin(q3); 90 | a4*cos(q3) -a4*sin(q3) a3;] 91 | 92 | %% compute the Christoffel Matrix 93 | q=[q1;q2;q3]; 94 | 95 | M1=M(:,1); 96 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 97 | M2=M(:,2); 98 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 99 | M3=M(:,3); 100 | C3=(1/2)*(jacobian(M3,q)+jacobian(M3,q)'-diff(M,q3)) 101 | 102 | pause 103 | 104 | disp('*robot centrifugal terms*') 105 | 106 | dq=[dq1;dq2;dq3]; 107 | c1=dq'*C1*dq; 108 | c2=dq'*C2*dq; 109 | c3=dq'*C3*dq; 110 | c=[c1;c2;c3] 111 | 112 | pause 113 | 114 | disp('*potential energy of link 1*') 115 | %% gravity vector 116 | g= [0;g0;0]; 117 | 118 | %% compute the potential energy of link 1 119 | U1=0 120 | 121 | disp('*potential energy of link 2*') 122 | 123 | %% compute the potential energy of link 2 124 | U2=0; 125 | 126 | disp('* total potential energy*') 127 | 128 | %% total potential energy 129 | U=U1+U2 130 | 131 | %% G 132 | G= jacobian(U,q)' 133 | 134 | %% show complete dynamic equations 135 | M*[ddq1; ddq2; ddq3]+c+G==[u1 u2 u3]' 136 | 137 | syms V0 alpha real 138 | 139 | qdd =[((L3*sin(q3)*dq3^2 + V0*cos(alpha))*(L3^2*sin(q3)^2 + 1))/(L3^2 + 1) - (L3^2*cos(q3)*sin(q3)*(- L3*cos(q3)*dq3^2 + V0*sin(alpha)))/(L3^2 + 1); 140 | (L3^2*cos(q3)*sin(q3)*(L3*sin(q3)*dq3^2 + V0*cos(alpha)))/(L3^2 + 1) - ((- L3*cos(q3)*dq3^2 + V0*sin(alpha))*(L3^2 - L3^2*sin(q3)^2 + 1))/(L3^2 + 1); 141 | L3*V0*cos(alpha-q3)] 142 | 143 | 144 | disp('***end***') 145 | -------------------------------------------------------------------------------- /Scripts/2020/dynmod_PProbot.m: -------------------------------------------------------------------------------- 1 | %% dynamic model of PP planar robot under gravity 2 | %% using a Lagrangian formulation in symbolic form 3 | 4 | clear all 5 | close all 6 | clc 7 | 8 | %% define symbolic variables 9 | syms m1 m2 d2 I2xx I2yy I2zz real %symbolic variables explicitly defined as real 10 | syms q1 q2 dq1 dq2 ddq1 ddq2 u1 u2 g0 real 11 | 12 | disp('**** dynamic model of PP planar robot in a vertical plane ****') 13 | disp(' ') 14 | disp('[press return to proceed at every pause]') 15 | 16 | pause 17 | 18 | disp(' ') 19 | disp('*kinetic energy of link 1*') 20 | 21 | %% compute kinetic energy of joint 1 22 | T1=(1/2)*m1*dq1^2 23 | 24 | pause 25 | 26 | disp('*kinetic energy of link 2') 27 | 28 | %% compute the kinetic energy of joint 2 29 | pc2=[q1+q2*cos(deg2rad(120)) q2*sin(deg2rad(120)) 0]'; 30 | vc2=diff(pc2,q1)*dq1+diff(pc2,q2)*dq2; 31 | 32 | T2= (1/2)*m2*vc2'*vc2 33 | pause 34 | 35 | disp('***robot kinetic energy***') 36 | 37 | %% total kinetic energy 38 | T=T1+T2; 39 | 40 | pause 41 | 42 | disp('*simplifying*') 43 | 44 | T=simplify(T1+T2) 45 | pause 46 | 47 | % collect in base of term that you pass it in this case, collect terms that has dq1^2 and 48 | % do the same for dq2^2 because you need them to compute M 49 | T=collect(T,dq1^2); 50 | T=collect(T,dq2^2); 51 | 52 | disp('***robot inertia matrix***') 53 | 54 | %% compute robot matrix M, inertia matrix 55 | 56 | % we extract the element M(1,1) from the total kinetic energy by doing twice the derivative 57 | % respect qd1 because this will be the factor that multiplyin the square of velocity of joint 1 58 | % we do the same also for joint 2 59 | % the factor 1/2 disappear automatically in this differentation 60 | M(1,1)=diff(T,dq1,2); 61 | M(2,2)=diff(T,dq2,2); 62 | TempB12=diff(T,dq1); 63 | M(1,2)=diff(TempB12,dq2); 64 | M(2,1)=M(1,2); 65 | M=simplify(M) 66 | 67 | pause 68 | 69 | disp('*Christoffel matrices*') 70 | 71 | %% compute the Christoffel Matrix 72 | q=[q1;q2]; 73 | 74 | % consider the first column of matrix M, so you have M1 75 | % do the same for M2 76 | M1=M(:,1); 77 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 78 | M2=M(:,2); 79 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 80 | 81 | pause 82 | 83 | disp('***robot centrifugal terms (no Coriolis here!)***') 84 | 85 | dq=[dq1;dq2]; 86 | c1=dq'*C1*dq; 87 | c2=dq'*C2*dq; 88 | c=[c1;c2] 89 | 90 | pause 91 | 92 | disp('*potential energy of link 1*') 93 | 94 | %% vector gravity acceleration 95 | g=[0;-g0;0]; 96 | 97 | %% compute the potential energy of link 1 98 | U1=-m1*g'*[q1;0;0]; 99 | 100 | pause 101 | 102 | disp('*potential energy of link 2*') 103 | 104 | 105 | %% compute the potential energy of link 2 106 | U2=-m2*g'*pc2; 107 | pause 108 | 109 | disp('***robot potential energy (due to gravity)***') 110 | 111 | %% total potential energy 112 | U=U1+U2 113 | 114 | pause 115 | 116 | disp('***robot gravity term***') 117 | 118 | %% compute G 119 | G=jacobian(U,q) 120 | 121 | pause 122 | 123 | disp('***complete dynamic equations***') 124 | 125 | %% show complete dynamic equations 126 | M*[ddq1; ddq2]+c+G==[u1 u2]' 127 | 128 | disp('***end***') 129 | 130 | % end 131 | -------------------------------------------------------------------------------- /Scripts/2020/dynmod_PRProbot.m: -------------------------------------------------------------------------------- 1 | 2 | %% dynamic model of PRP planar robot under gravity 3 | %% using a Lagrangian formulation in symbolic form 4 | 5 | clear all 6 | close all 7 | clc 8 | 9 | %% define symbolic variables 10 | syms m1 m2 m3 real 11 | syms d1 d2 d3 real 12 | syms L1 L2 L3 real 13 | syms I2xx I2yy I2zz real 14 | syms I3xx I3yy I3zz real 15 | syms q1 q2 q3 real 16 | syms dq1 dq2 dq3 real 17 | syms ddq1 ddq2 ddq3 real 18 | syms u1 u2 u3 g0 real 19 | 20 | disp('**** dynamic model of PRP planar robot in a vertical plane ****') 21 | disp(' ') 22 | disp('[press return to proceed at every pause]') 23 | 24 | pause 25 | 26 | disp(' ') 27 | disp('*kinetic energy of link 1*') 28 | 29 | %% compute kinetic energy of joint 1 30 | %pc1 = [q1 0 0]'; 31 | pc1=[0 q1 0]'; 32 | vc1 = diff(pc1,q1)*dq1; 33 | T1=(1/2)*m1*vc1'*vc1 34 | pause 35 | 36 | disp('*kinetic energy of link 2 - linear part') 37 | 38 | %% compute the linear part of kinetic energy of joint 2 39 | %pc2=[q1+d1+d2*cos(q2) d2*sin(q2) 0]'; 40 | pc2=[d2*cos(q2) q1+d1+d2*sin(q2) 0]' 41 | vc2=simplify(diff(pc2,q1)*dq1+diff(pc2,q2)*dq2); 42 | Tl2= simplify((1/2)*m2*vc2'*vc2); 43 | 44 | disp('*kinetic energy of link 2 - angular part*') 45 | 46 | %% compute the angular part of kinetic energy of joint 2 47 | om2=[0 0 dq2]'; 48 | Ta2=(1/2)*om2'*diag([I2xx I2yy I2zz])*om2; 49 | 50 | T2=simplify(Tl2+Ta2) 51 | pause 52 | disp('*kinetic energy of link 3*') 53 | 54 | %% compute the linear part of kinetic energy of joint 3 55 | pc3 = [q3*cos(q2+pi/2)+L2*cos(q2) q1+d1+L2*sin(q2)+q3*sin(q2+pi/2) 0]'; 56 | vc3=simplify(diff(pc3,q1)*dq1+diff(pc3,q2)*dq2+diff(pc3,q3)*dq3); 57 | %vc3 = [dq1; dq2*(q3-d3); dq3] 58 | Tl3 = simplify(1/2*m3*vc3'*vc3); 59 | 60 | %% compute the angular part of kinetic energy of joint 3 61 | Ta3 = simplify((1/2)*om2'*diag([I3xx I3yy I3zz])*om2); 62 | 63 | T3=simplify(Tl3+Ta3) 64 | pause 65 | 66 | disp('***robot kinetic energy***') 67 | 68 | 69 | %% total kinetic energy 70 | T=T1+T2+T3; 71 | 72 | disp('*simplifying*') 73 | 74 | T=simplify(T1+T2+T3) 75 | pause 76 | % collect in base of term that you pass it in this case, collect terms that has dq1^2 and 77 | % do the same for dq2^2 because you need them to compute M 78 | T=collect(T,dq1^2); 79 | T=collect(T,dq2^2); 80 | T=collect(T,dq3^2); 81 | disp('***robot inertia matrix***') 82 | 83 | %% compute robot matrix M, inertia matrix 84 | 85 | % we extract the element M(1,1) from the total kinetic energy by doing twice the derivative 86 | % respect qd1 because this will be the factor that multiplyin the square of velocity of joint 1 87 | % we do the same also for joint 2 88 | % the factor 1/2 disappear automatically in this differentation 89 | M(1,1)=diff(T,dq1,2); 90 | M(2,2)=diff(T,dq2,2); 91 | M(3,3)=diff(T,dq3,2); 92 | 93 | TempB1=diff(T,dq1); 94 | M(1,2)=diff(TempB1,dq2); 95 | M(2,1)=M(1,2); 96 | 97 | M(1,3)=diff(TempB1,dq3); 98 | M(3,1)=M(1,3); 99 | 100 | TempB2= diff(T,dq2); 101 | M(2,3)=diff(TempB2,dq3); 102 | M(3,2)=M(2,3); 103 | 104 | M=simplify(M) 105 | 106 | pause 107 | 108 | %% parametrization 109 | syms a1 a2 a3 a4 a5 real 110 | 111 | M=[a1 a4*cos(q2)-a3*q3*sin(q2) a3*cos(q2); 112 | a4*cos( 113 | q2)-a3*q3*sin(q2) a2+a3*q3^2 a3*L2; 114 | a3*cos(q2) a3*L2 a3;] 115 | 116 | disp('*Christoffel matrices*') 117 | 118 | %% compute the Christoffel Matrix 119 | q=[q1;q2;q3]; 120 | 121 | % consider the first column of matrix M, so you have M1 122 | % do the same for M2 123 | M1=M(:,1); 124 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 125 | M2=M(:,2); 126 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 127 | M3 = M(:,3); 128 | C3=(1/2)*(jacobian(M3,q)+jacobian(M3,q)'-diff(M,q3)) 129 | 130 | 131 | pause 132 | 133 | disp('***robot centrifugal terms (no Coriolis here!)***') 134 | 135 | dq=[dq1;dq2;dq3]; 136 | c1=simplify(dq'*C1*dq); 137 | c2=simplify(dq'*C2*dq); 138 | c3=simplify(dq'*C3*dq); 139 | c=[c1;c2;c3] 140 | 141 | pause 142 | 143 | disp('*potential energy of link 1*') 144 | 145 | %% compute the potential energy of link 1 146 | U1=0 147 | 148 | pause 149 | 150 | disp('*potential energy of link 2*') 151 | 152 | %% vector gravity acceleration 153 | g=[0;-g0;0]; 154 | 155 | %% compute the potential energy of link 2 156 | U2=-m2*g'*pc2; 157 | 158 | %% compute the potential energy of link 3 159 | U3=-m3*g'*pc3; 160 | 161 | disp('***robot potential energy (due to gravity)***') 162 | 163 | %% total potential energy 164 | U=U1+U2+U3 165 | 166 | pause 167 | 168 | disp('***robot gravity term***') 169 | 170 | %% compute G 171 | G= jacobian(U,q)' 172 | 173 | pause 174 | 175 | disp('***complete dynamic equations***') 176 | 177 | %% show complete dynamic equations 178 | M*[ddq1; ddq2; ddq3;]+c+G==[u1 u2 u3]' 179 | 180 | disp('***end***') 181 | 182 | % end 183 | -------------------------------------------------------------------------------- /Scripts/2020/dynmod_PRProbot.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2020/dynmod_PRProbot.mat -------------------------------------------------------------------------------- /Scripts/2020/dynmod_PRR.m: -------------------------------------------------------------------------------- 1 | %% dynamic model of PRR planar robot under gravity 2 | %% using a Lagrangian formulation in symbolic form 3 | 4 | clear all 5 | close all 6 | clc 7 | 8 | %% define symbolic variables 9 | syms m1 m2 m3 real 10 | syms I1xx I1yy I1zz real 11 | syms I2xx I2yy I2zz real %symbolic variables explicitly defined as real 12 | syms I3xx I3yy I3zz real 13 | syms d1 d2 d3 L1 L2 L3 real 14 | syms q1 q2 q3 real 15 | syms dq1 dq2 dq3 real 16 | syms ddq1 ddq2 ddq3 real 17 | syms u1 u2 u3 g0 real 18 | 19 | disp('**** dynamic model of PRR planar robot in a vertical plane ****') 20 | disp(' ') 21 | disp('[press return to proceed at every pause]') 22 | 23 | pause 24 | 25 | 26 | %% link 1 27 | pc1 = [q1-d1;0;0]; 28 | vc1 = diff(pc1,q1)*dq1; 29 | T1= (1/2)*m1*vc1'*vc1 30 | pause 31 | 32 | %% link 2 33 | pc2 = [q1+d2*cos(q2);d2*sin(q2);0]; 34 | vc2 = diff(pc2,q1)*dq1 + diff(pc2,q2)*dq2; 35 | Tl2= simplify((1/2)*m2*vc2'*vc2); 36 | 37 | om2= [0;0;dq2;]; 38 | Ta2= (1/2)*om2'*diag([I2xx I2yy I2zz])*om2; 39 | 40 | T2=simplify(Ta2+Tl2); 41 | T2=collect(T2,dq2^2) 42 | pause 43 | 44 | %% link 3 45 | pc3 = [q1+L2*cos(q2)+d3*cos(q2+q3); L2*sin(q2)+d3*sin(q2+q3);0]; 46 | vc3 = diff(pc3,q1)*dq1 + diff(pc3,q2)*dq2 + diff(pc3,q3)*dq3; 47 | Tl3 = simplify((1/2)*m3*vc3'*vc3); 48 | 49 | om3= [0;0;dq2+dq3;]; 50 | Ta3= simplify((1/2)*om3'*diag([I3xx I3yy I3zz])*om3); 51 | 52 | T3=simplify(Tl3+Ta3) 53 | pause 54 | %% total kinetic energy 55 | disp('*simplifying*') 56 | 57 | T=simplify(T1+T2+T3); 58 | 59 | % collect in base of term that you pass it in this case, collect terms that has dq1^2 and 60 | % do the same for dq2^2 because you need them to compute M 61 | 62 | T=collect(T,dq1^2); 63 | T=collect(T,dq2^2); 64 | T=collect(T,dq3^2) 65 | pause 66 | 67 | disp('***robot inertia matrix***') 68 | 69 | %% compute robot matrix M, inertia matrix 70 | 71 | % we extract the element M(1,1) from the total kinetic energy by doing twice the derivative 72 | % respect qd1 because this will be the factor that multiplying the square of velocity of + 73 | % joint 1 we do the same also for joint 2 74 | % the factor 1/2 disappear automatically in this differentation 75 | 76 | 77 | M(1,1) = simplify(diff(T,dq1,2)); 78 | M(2,2) = simplify(diff(T,dq2,2)); 79 | M(3,3) = simplify(diff(T,dq3,2)); 80 | 81 | TempB1 = simplify(diff(T,dq1)); 82 | M(1,2)= simplify(diff(TempB1,dq2)); 83 | M(2,1) = M(1,2); 84 | M(1,3) = simplify(diff(TempB1,dq3)); 85 | M(3,1)=M(1,3); 86 | 87 | TempB2=simplify(diff(T,dq2)); 88 | M(2,3) = simplify(diff(TempB2,dq3)); 89 | M(3,2)=M(2,3); 90 | M=simplify(M) 91 | pause 92 | 93 | 94 | %% parametrization 95 | syms a1 a2 a3 a4 a5 a6 real 96 | 97 | % M=[a1 -a4*sin(q2+q3)-a5*sin(q2) -a4*sin(q2+q3); 98 | % -a4*sin(q2+q3)-a5*sin(q2) a2+2*a6*cos(q3) a3+a6*cos(q3); 99 | % -a4*sin(q2+q3) a3+a6*cos(q3) a3;]; 100 | 101 | disp('*Christoffel matrices*') 102 | 103 | %% compute the Christoffel Matrix 104 | q=[q1;q2;q3]; 105 | 106 | % consider the first column of matrix M, so you have M1 107 | % do the same for M2 108 | M1=M(:,1); 109 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 110 | M2=M(:,2); 111 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 112 | M3=M(:,3); 113 | C3=(1/2)*(jacobian(M3,q)+jacobian(M3,q)'-diff(M,q3)) 114 | 115 | pause 116 | 117 | disp('***robot centrifugal terms (no Coriolis here!)***') 118 | 119 | dq=[dq1;dq2;dq3;]; 120 | c1=dq'*C1*dq; 121 | c2=dq'*C2*dq; 122 | c3=dq'*C3*dq; 123 | c=simplify([c1;c2;c3]) 124 | 125 | pause 126 | 127 | disp('*potential energy of link 1*') 128 | 129 | %% vector gravity acceleration 130 | g=[0;0;-g0;]; 131 | 132 | %% compute the potential energy of link 1 133 | U1=-m1*transpose(g)*pc1 134 | 135 | pause 136 | 137 | disp('*potential energy of link 2*') 138 | 139 | %% compute the potential energy of link 2 140 | U2=-m2*transpose(g)*pc2 141 | 142 | pause 143 | 144 | %% potential energy of link 3 145 | U3=-m3*transpose(g)*pc3 146 | 147 | disp('***robot potential energy (due to gravity)***') 148 | 149 | %% total potential energy 150 | U=simplify(U1+U2+U3) 151 | 152 | pause 153 | 154 | disp('***robot gravity term***') 155 | 156 | %% compute G 157 | G=jacobian(U,q)' 158 | 159 | Gp=[a1*g0; a4*g0*cos(q2)*cos(q3);a4*g0*sin(q2)*sin(q3);] 160 | pause 161 | 162 | disp('***complete dynamic equations***') 163 | 164 | %% show complete dynamic equations 165 | Mp*[ddq1; ddq2; ddq3]+c+Gp==[u1 u2 u3]' 166 | 167 | disp('***end***') 168 | 169 | % end 170 | -------------------------------------------------------------------------------- /Scripts/2020/dynmod_PRRRrobot.m: -------------------------------------------------------------------------------- 1 | 2 | clear all 3 | close all 4 | clc 5 | 6 | %% define symbolic variables 7 | syms m1 m2 m3 m4 real 8 | syms d1 d2 d3 d4 real 9 | syms L1 L2 L3 L4 L h real 10 | syms I1xx I1yy I1zz real %symbolic variables explicitly defined as real 11 | syms I2xx I2yy I2zz real %symbolic variables explicitly defined as real 12 | syms I3xx I3yy I3zz real %symbolic variables explicitly defined as real 13 | syms I4xx I4yy I4zz real %symbolic variables explicitly defined as real 14 | syms q1 q2 q3 q4 real 15 | syms dq1 dq2 dq3 dq4 real 16 | syms ddq1 ddq2 ddq3 ddq4 real 17 | syms u1 u2 u3 u4 g0 real 18 | 19 | %% 1 20 | pc1 = [q1; 0;0]; 21 | vc1=diff(pc1,q1)*dq1; 22 | T1 = 1/2*m1*vc1'*vc1 23 | pause 24 | 25 | 26 | %% 2 27 | pc2 = [q1+d2*cos(q2); h+d2*sin(q2);0]; 28 | vc2=diff(pc2,q1)*dq1+diff(pc2,q2)*dq2; 29 | Tl2 = 1/2*m2*vc2'*vc2; 30 | 31 | om2 = [0 0 dq2]'; 32 | Ta2 = 1/2*om2'*diag([I2xx I2yy I2zz])*om2; 33 | 34 | T2=Tl2+Ta2 35 | pause 36 | 37 | 38 | %% 3 39 | pc3 = [q1+L*cos(q2)+d3*cos(q3); h+L*sin(q2)+d3*sin(q3);0]; 40 | vc3=diff(pc3,q1)*dq1+diff(pc3,q2)*dq2++diff(pc3,q3)*dq3; 41 | Tl3 = 1/2*m3*vc3'*vc3; 42 | 43 | om3 = [0 0 dq3]'; 44 | Ta3 = 1/2*om3'*diag([I3xx I3yy I3zz])*om3; 45 | 46 | T3=Tl3+Ta3 47 | pause 48 | 49 | 50 | %% 4 51 | 52 | pc4 = [q1+L*cos(q2)+L*cos(q3)+d4*cos(q4); h+L*sin(q2)+L*sin(q3)+d4*sin(q4);0]; 53 | vc4=diff(pc4,q1)*dq1+diff(pc4,q2)*dq2+diff(pc4,q3)*dq3+diff(pc4,q4)*dq4; 54 | Tl4 = 1/2*m4*vc4'*vc4 55 | 56 | om4 = [0 0 dq4]'; 57 | Ta4 = 1/2*om4'*diag([I4xx I4yy I4zz])*om4; 58 | 59 | T4= simplify(Tl4+Ta4) 60 | pause 61 | 62 | T=simplify(T1+T2+T3+T4) 63 | pause 64 | 65 | T=collect(T,dq1^2); 66 | T=collect(T,dq2^2); 67 | T=collect(T,dq3^2); 68 | T=collect(T,dq4^2); 69 | 70 | 71 | %% inertia 72 | 73 | M(1,1)=diff(T,dq1,2); 74 | M(2,2)=diff(T,dq2,2); 75 | M(3,3)= diff(T,dq3,2); 76 | M(4,4)=diff(T,dq4,2); 77 | 78 | TempB1=diff(T,dq1); 79 | M(1,2)=diff(TempB1,dq2); 80 | M(2,1)=M(1,2); 81 | 82 | M(1,3)=diff(TempB1,dq3); 83 | M(3,1)=M(1,3); 84 | 85 | M(1,4)=diff(TempB1,dq4); 86 | M(4,1)=M(1,4); 87 | 88 | TempB2=diff(T,dq2); 89 | M(2,3) = diff(TempB2,dq3); 90 | M(3,2)=M(2,3); 91 | 92 | M(2,4)=diff(TempB2,dq4); 93 | M(4,2)=M(2,4); 94 | 95 | TempB3=diff(T,dq3); 96 | M(3,4)=diff(TempB3,dq4); 97 | M(4,3)=M(3,4); 98 | 99 | M=simplify(M) 100 | pause 101 | disp('*parametrized*') 102 | -------------------------------------------------------------------------------- /Scripts/2020/dynmod_PRrobot.m: -------------------------------------------------------------------------------- 1 | %% dynamic model of PR planar robot under gravity 2 | %% using a Lagrangian formulation in symbolic form 3 | % 4 | % A. De Luca 5 | % distributed on March 14, 2020 6 | 7 | clear all 8 | close all 9 | clc 10 | 11 | %% define symbolic variables 12 | syms m1 m2 d2 I2xx I2yy I2zz real %symbolic variables explicitly defined as real 13 | syms q1 q2 dq1 dq2 ddq1 ddq2 u1 u2 g0 real 14 | 15 | disp('**** dynamic model of PR planar robot in a vertical plane ****') 16 | disp(' ') 17 | disp('[press return to proceed at every pause]') 18 | 19 | pause 20 | 21 | disp(' ') 22 | disp('*kinetic energy of link 1*') 23 | 24 | %% compute kinetic energy of joint 1 25 | T1=(1/2)*m1*dq1^2 26 | 27 | pause 28 | 29 | disp('*kinetic energy of link 2 - linear part*') 30 | 31 | %% compute the linear part of kinetic energy of joint 2 32 | pc2=[q1+d2*cos(q2) d2*sin(q2) 0]' 33 | vc2=diff(pc2,q1)*dq1+diff(pc2,q2)*dq2 34 | T2c= (1/2)*m2*vc2'*vc2; 35 | 36 | pause 37 | 38 | disp('*kinetic energy of link 2 - angular part*') 39 | 40 | %% compute the angular part of kinetic energy of joint 2 41 | om2=[0 0 dq2]' 42 | T2a=(1/2)*om2'*diag([I2xx I2yy I2zz])*om2; 43 | 44 | %% kinetic energy of joint 2 45 | T2=T2c+T2a 46 | pause 47 | 48 | disp('***robot kinetic energy***') 49 | 50 | %% total kinetic energy 51 | T=T1+T2 52 | 53 | pause 54 | 55 | disp('*simplifying*') 56 | 57 | T=simplify(T1+T2) 58 | pause 59 | 60 | % collect in base of term that you pass it in this case, collect terms that has dq1^2 and 61 | % do the same for dq2^2 because you need them to compute M 62 | T=collect(T,dq1^2) 63 | T=collect(T,dq2^2) 64 | 65 | pause 66 | 67 | disp('***robot inertia matrix***') 68 | 69 | %% compute robot matrix M, inertia matrix 70 | 71 | % we extract the element M(1,1) from the total kinetic energy by doing twice the derivative 72 | % respect qd1 because this will be the factor that multiplyin the square of velocity of joint 1 73 | % we do the same also for joint 2 74 | % the factor 1/2 disappear automatically in this differentation 75 | M(1,1)=diff(T,dq1,2); 76 | M(2,2)=diff(T,dq2,2); 77 | TempB12=diff(T,dq1); 78 | M(1,2)=diff(TempB12,dq2); 79 | M(2,1)=M(1,2); 80 | M=simplify(M) 81 | 82 | pause 83 | 84 | disp('*Christoffel matrices*') 85 | 86 | %% compute the Christoffel Matrix 87 | q=[q1;q2]; 88 | 89 | % consider the first column of matrix M, so you have M1 90 | % do the same for M2 91 | M1=M(:,1); 92 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 93 | M2=M(:,2); 94 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 95 | 96 | pause 97 | 98 | disp('***robot centrifugal terms (no Coriolis here!)***') 99 | 100 | dq=[dq1;dq2]; 101 | c1=dq'*C1*dq; 102 | c2=dq'*C2*dq; 103 | c=[c1;c2] 104 | 105 | pause 106 | 107 | disp('*potential energy of link 1*') 108 | 109 | %% compute the potential energy of link 1 110 | U1=0 111 | 112 | pause 113 | 114 | disp('*potential energy of link 2*') 115 | 116 | %% vector gravity acceleration 117 | g=[0;-g0;0]; 118 | 119 | %% compute the potential energy of link 2 120 | U2=-m2*g'*pc2 121 | 122 | pause 123 | 124 | disp('***robot potential energy (due to gravity)***') 125 | 126 | %% total potential energy 127 | U=simplify(U1+U2) 128 | 129 | pause 130 | 131 | disp('***robot gravity term***') 132 | 133 | %% compute G 134 | G=jacobian(U,q)' 135 | 136 | pause 137 | 138 | disp('***complete dynamic equations***') 139 | 140 | %% show complete dynamic equations 141 | M*[ddq1; ddq2]+c+G==[u1 u2]' 142 | 143 | disp('***end***') 144 | 145 | % end 146 | -------------------------------------------------------------------------------- /Scripts/2020/dynmod_RPProbot.m: -------------------------------------------------------------------------------- 1 | %% dynamic model of RPP planar robot under gravity 2 | %% using a Lagrangian formulation in symbolic form 3 | 4 | clear all 5 | close all 6 | clc 7 | 8 | %% define symbolic variables 9 | syms m1 m2 m3 real 10 | syms d1 d2 d3 real 11 | syms r2 a2 L1 real 12 | syms I1xx I1yy I1zz real 13 | syms I2xx I2yy I2zz real 14 | syms I3xx I3yy I3zz real 15 | syms q1 q2 q3 real 16 | syms dq1 dq2 dq3 real 17 | syms ddq1 ddq2 ddq3 real 18 | syms u1 u2 u3 g0 real 19 | 20 | disp('**** dynamic model of RPP planar robot in a vertical plane ****') 21 | disp(' ') 22 | disp('[press return to proceed at every pause]') 23 | 24 | pause 25 | 26 | disp(' ') 27 | disp('*kinetic energy of link 1*') 28 | 29 | %% compute kinetic energy of joint 1 30 | om1 = [0 0 dq1]'; 31 | T1=(1/2)*om1'*diag([I1xx I1yy I1zz])*om1 32 | 33 | pause 34 | 35 | disp('*kinetic energy of link 2*') 36 | 37 | %% compute linear part kinetic energy of joint 2 38 | pc2 = [r2*cos(q1) r2*sin(q1) L1+q2]'; 39 | vc2 = diff(pc2,q1)*dq1+diff(pc2,q2)*dq2; 40 | Tl2 = (1/2)*m2*vc2'*vc2; 41 | 42 | %% compute kinetic energy of joint 2 43 | om2 = [0 0 dq1]'; 44 | Ta2=(1/2)*om2'*diag([I2xx I2yy I2zz])*om2; 45 | 46 | T2 = simplify(Tl2+Ta2) 47 | pause 48 | 49 | disp('*kinetic energy of link 3*') 50 | 51 | %% compute linear part kinetic energy of joint 3 52 | pc3 = [cos(q1)*(a2+q3) sin(q1)*(a2+q3) L1+q2+r2]' 53 | %pc3 = [cos(q1)*(a2 + q3 + r2); sin(q1)*(a2 + q3 + r2); -q2] 54 | 55 | 56 | vc3 = diff(pc3,q1)*dq1+diff(pc3,q2)*dq2+diff(pc3,q3)*dq3; 57 | Tl3 = (1/2)*m3*vc3'*vc3; 58 | 59 | %% compute kinetic energy of joint 2 60 | om3 = [0 0 dq1]'; 61 | Ta3=(1/2)*om3'*diag([I3xx I3yy I3zz])*om3; 62 | 63 | T3 = simplify(Tl3+Ta3) 64 | pause 65 | 66 | T=simplify(T1+T2+T3); 67 | 68 | T=collect(T,dq1^2); 69 | T=collect(T,dq2^2); 70 | T=collect(T,dq3^2) 71 | pause 72 | 73 | disp('***robot inertia matrix***') 74 | 75 | %% compute robot matrix M, inertia matrix 76 | 77 | M(1,1)=diff(T,dq1,2); 78 | M(2,2)=diff(T,dq2,2); 79 | M(3,3)=diff(T,dq3,2); 80 | 81 | TempB12=diff(T,dq1); 82 | M(1,2)=diff(TempB12,dq2); 83 | M(2,1)=M(1,2); 84 | M(1,3)=diff(TempB12,dq3); 85 | M(3,1)=M(1,3); 86 | 87 | TempB23 = diff(T,dq2); 88 | M(2,3)=diff(TempB23,dq3); 89 | M(3,2)=M(2,3); 90 | 91 | M=simplify(M) 92 | pause 93 | 94 | %% parametrization 95 | syms b1 b2 b3 b4 real 96 | 97 | M=[b1+b3*(d3-q3)^2 0 b4; 98 | 0 b2 0; 99 | b4 0 b3] 100 | 101 | %% compute the Christoffel Matrix 102 | q=[q1;q2;q3]; 103 | 104 | M1=M(:,1); 105 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 106 | M2=M(:,2); 107 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 108 | M3=M(:,3); 109 | C3=(1/2)*(jacobian(M3,q)+jacobian(M3,q)'-diff(M,q3)) 110 | 111 | pause 112 | 113 | disp('***robot centrifugal terms (no Coriolis here!)***') 114 | 115 | dq=[dq1;dq2;dq3]; 116 | c1=dq'*C1*dq; 117 | c2=dq'*C2*dq; 118 | c3=dq'*C3*dq; 119 | c=[c1;c2;c3] 120 | 121 | pause 122 | 123 | disp('*potential energy of link 1*') 124 | 125 | %% compute the potential energy of link 1 126 | U1=0; 127 | 128 | disp('*potential energy of link 2*') 129 | 130 | %% vector gravity acceleration 131 | g=[0;0;-g0]; 132 | 133 | %% compute the potential energy of link 2 134 | U2=-m2*g'*pc2; 135 | 136 | disp('*potential energy of link 3*') 137 | 138 | %% compute the potential energy of link 2 139 | U3=-m3*g'*pc3; 140 | 141 | U=U1+U2+U3 142 | pause 143 | 144 | G= jacobian(U,q)' 145 | pause -------------------------------------------------------------------------------- /Scripts/2020/dynmod_RPRrobot.m: -------------------------------------------------------------------------------- 1 | 2 | %% dynamic model of RPR planar robot under gravity 3 | %% using a Lagrangian formulation in symbolic form 4 | 5 | clear all 6 | close all 7 | clc 8 | 9 | %% define symbolic variables 10 | syms m1 m2 m3 real 11 | syms d1 d2 d3 real 12 | syms L1 L2 L3 real 13 | syms I1xx I1yy I1zz real 14 | syms I2xx I2yy I2zz real 15 | syms I3xx I3yy I3zz real 16 | syms q1 q2 q3 real 17 | syms dq1 dq2 dq3 real 18 | syms ddq1 ddq2 ddq3 real 19 | syms u1 u2 u3 g0 real 20 | 21 | disp('**** dynamic model of RPR planar robot in a vertical plane ****') 22 | disp(' ') 23 | disp('[press return to proceed at every pause]') 24 | 25 | pause 26 | 27 | disp(' ') 28 | disp('*kinetic energy of link 1*') 29 | 30 | %% compute linear part of kinetic energy of joint 1 31 | pc1=[0; 0;0] 32 | vc1=diff(pc1,q1)*dq1; 33 | Tl1=simplify((1/2)*m1*vc1'*vc1); 34 | 35 | %% compute angular part of kinetic energy of joint 1 36 | om1 = [0 0 dq1]'; 37 | Ta1=simplify((1/2)*om1'*diag([I1xx I1yy I1zz])*om1); 38 | 39 | T1=simplify(Tl1+Ta1) 40 | pause 41 | 42 | disp('*kinetic energy of link 2*') 43 | 44 | %% compute linear part kinetic energy of joint 2 45 | pc2 = [(q2-d2)*cos(q1) (q2-d2)*sin(q1) 0]'; 46 | vc2 = diff(pc2,q1)*dq1+diff(pc2,q2)*dq2; 47 | Tl2 = (1/2)*m2*vc2'*vc2; 48 | 49 | %% compute kinetic energy of joint 2 50 | om2 = [0 0 dq1]'; 51 | Ta2=(1/2)*om2'*diag([I2xx I2yy I2zz])*om2; 52 | 53 | T2 = simplify(Tl2+Ta2) 54 | pause 55 | 56 | 57 | disp('*kinetic energy of link 3*') 58 | 59 | %% compute linear part kinetic energy of joint 3 60 | pc3 = [q2*cos(q1)+(L3-d3)*cos(q3); q2*sin(q1)+(L3-d3)*sin(q3);0]; 61 | vc3 = diff(pc3,q1)*dq1 + diff(pc3,q2)*dq2 + diff(pc3,q3)*dq3; 62 | Tl3 = (1/2)*m3*vc3'*vc3; 63 | 64 | %% compute kinetic energy of joint 2 65 | om3 = [0 0 dq3+dq1]'; 66 | Ta3=(1/2)*om3'*diag([I3xx I3yy I3zz])*om3; 67 | 68 | T3 = simplify(Tl3+Ta3) 69 | pause 70 | 71 | T=simplify(T1+T2+T3); 72 | 73 | T=collect(T,dq1^2); 74 | T=collect(T,dq2^2); 75 | T=collect(T,dq3^2) 76 | pause 77 | 78 | disp('***robot inertia matrix***') 79 | 80 | %% compute robot matrix M, inertia matrix 81 | 82 | M(1,1)=diff(T,dq1,2); 83 | M(2,2)=diff(T,dq2,2); 84 | M(3,3)=diff(T,dq3,2); 85 | 86 | TempB12=diff(T,dq1); 87 | M(1,2)=diff(TempB12,dq2); 88 | M(2,1)=M(1,2); 89 | M(1,3)=diff(TempB12,dq3); 90 | M(3,1)=M(1,3); 91 | 92 | TempB23 = diff(T,dq2); 93 | M(2,3)=diff(TempB23,dq3); 94 | M(3,2)=M(2,3); 95 | 96 | M=simplify(M) 97 | pause 98 | %% parametrization 99 | syms a1 a2 a3 a4 a5 a6 real 100 | 101 | % M=[a1+a2*q2^2-2*a5*q2-2*a4*L1*sin(q3)+2*a4*cos(q3)*q2 a2*L1-a4*sin(q3) a3-a4*L1*sin(q3)+a4*cos(q3)*q2; 102 | % a2*L1-a4*sin(q3) a2 -a4*sin(q3); 103 | % a3-a4*L1*sin(q3)+a4*cos(q3)*q2 -a4*sin(q3) a3] 104 | 105 | M=[a1+a2*q2^2-2*a5*q2+2*a4*q2*cos(q3) -a4*sin(q3) a3+a4*q2*cos(q3); 106 | -a4*sin(q3) a2 -a4*sin(q3); 107 | a3+a4*q2*cos(q3) -a4*sin(q3) a3;] 108 | 109 | %% compute the Christoffel Matrix 110 | q=[q1;q2;q3]; 111 | 112 | M1=M(:,1); 113 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 114 | M2=M(:,2); 115 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 116 | M3=M(:,3); 117 | C3=(1/2)*(jacobian(M3,q)+jacobian(M3,q)'-diff(M,q3)) 118 | 119 | pause 120 | 121 | disp('***robot centrifugal terms (no Coriolis here!)***') 122 | 123 | dq=[dq1;dq2;dq3]; 124 | c1=dq'*C1*dq; 125 | c2=dq'*C2*dq; 126 | c3=dq'*C3*dq; 127 | c=simplify([c1;c2;c3]) 128 | 129 | pause 130 | 131 | disp('*potential energy of link 1*') 132 | 133 | %% vector gravity acceleration 134 | g=[g0;0;0]; 135 | 136 | %% compute the potential energy of link 1 137 | U1=-m1*g'*pc1; 138 | 139 | disp('*potential energy of link 2*') 140 | 141 | %% compute the potential energy of link 2 142 | U2=-m2*g'*pc2; 143 | 144 | disp('*potential energy of link 3*') 145 | 146 | %% compute the potential energy of link 2 147 | U3=-m3*g'*pc3; 148 | 149 | U=U1+U2+U3 150 | pause 151 | 152 | G= jacobian(U,q)' 153 | pause 154 | 155 | disp('***complete dynamic equations***') 156 | 157 | %% show complete dynamic equations 158 | M*[ddq1; ddq2; ddq3]+c+G==[u1 u2 u3]' 159 | 160 | disp('***end***') -------------------------------------------------------------------------------- /Scripts/2020/dynmod_RProbot.m: -------------------------------------------------------------------------------- 1 | %% dynamic model of RP planar robot under gravity 2 | %% using a Lagrangian formulation in symbolic form 3 | 4 | clear all 5 | close all 6 | clc 7 | 8 | %% define symbolic variables 9 | syms m1 m2 real 10 | syms d1 d2 d3 real 11 | syms L1 L2 L3 real 12 | syms I1xx I1yy I1zz real %symbolic variables explicitly defined as real 13 | syms I2xx I2yy I2zz real %symbolic variables explicitly defined as real 14 | syms q1 q2 real 15 | syms dq1 dq2 real 16 | syms ddq1 ddq2 real 17 | syms u1 u2 g0 real 18 | 19 | disp('**** dynamic model of RP planar robot in a vertical plane ****') 20 | disp(' ') 21 | disp('[press return to proceed at every pause]') 22 | 23 | pause 24 | 25 | disp(' ') 26 | disp('*kinetic energy of link 1*') 27 | %% compute linear part of linear kinetic energy of joint 1 28 | pc1 = [0;0;0]; 29 | vc1=diff(pc1,q1)*dq1; 30 | Tl1 = simplify(1/2*m1*vc1'*vc1); 31 | 32 | 33 | %% compute angular part of linear kinetic energy of joint 1 34 | om1 = [0 0 dq1]'; 35 | Ta1 = simplify(1/2*om1'*diag([I1xx I1yy I1zz])*om1); 36 | 37 | T1 = simplify(Tl1+Ta1) 38 | pause 39 | 40 | disp('*kinetic energy of link 2') 41 | 42 | %% compute the linear part of kinetic energy of joint 2 43 | pc2=[(q2-d2)*sin(q1) (q2-d2)*cos(q1) 0]'; 44 | vc2=diff(pc2,q1)*dq1+diff(pc2,q2)*dq2; 45 | Tl2= simplify((1/2)*m2*vc2'*vc2); 46 | 47 | %% compute the angular part of kinetic energy of joint 2 48 | om2 = [0 0 dq1]'; 49 | Ta2 = simplify(1/2*om2'*diag([I2xx I2yy I2zz])*om2); 50 | 51 | T2= simplify(Tl2+Ta2) 52 | pause 53 | 54 | disp('***robot kinetic energy***') 55 | 56 | %% total kinetic energy 57 | T=T1+T2; 58 | 59 | disp('*simplifying*') 60 | 61 | T=simplify(T1+T2); 62 | 63 | % collect in base of term that you pass it in this case, collect terms that has dq1^2 and 64 | % do the same for dq2^2 because you need them to compute M 65 | T=collect(T,dq1^2); 66 | T=collect(T,dq2^2) 67 | pause 68 | disp('***robot inertia matrix***') 69 | 70 | %% compute robot matrix M, inertia matrix 71 | 72 | % we extract the element M(1,1) from the total kinetic energy by doing twice the derivative 73 | % respect qd1 because this will be the factor that multiplyin the square of velocity of joint 1 74 | % we do the same also for joint 2 75 | % the factor 1/2 disappear automatically in this differentation 76 | M(1,1)=diff(T,dq1,2); 77 | M(2,2)=diff(T,dq2,2); 78 | 79 | TempB12=diff(T,dq1); 80 | M(1,2)=diff(TempB12,dq2); 81 | M(2,1)=M(1,2); 82 | M=simplify(M) 83 | 84 | pause 85 | 86 | %% parametrization 87 | syms a1 a2 a3 a4 real 88 | 89 | % M=[a1+a2*q2^2-2*a3*q2 0; 90 | % 0 a2] 91 | 92 | disp('*Christoffel matrices*') 93 | 94 | %% compute the Christoffel Matrix 95 | q=[q1;q2]; 96 | 97 | % consider the first column of matrix M, so you have M1 98 | % do the same for M2 99 | M1=M(:,1); 100 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 101 | M2=M(:,2); 102 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 103 | 104 | pause 105 | 106 | disp('***robot centrifugal terms (no Coriolis here!)***') 107 | 108 | dq=[dq1;dq2]; 109 | c1=dq'*C1*dq; 110 | c2=dq'*C2*dq; 111 | c=[c1;c2] 112 | 113 | pause 114 | 115 | disp('*potential energy of link 1*') 116 | 117 | %% vector gravity acceleration 118 | syms alpha real 119 | g=[0;g0;0]; 120 | 121 | %% compute the potential energy of link 1 122 | U1=-m1*g'*pc1; 123 | 124 | pause 125 | 126 | disp('*potential energy of link 2*') 127 | 128 | %% compute the potential energy of link 2 129 | U2=-m2*g'*pc2 130 | 131 | pause 132 | 133 | disp('***robot potential energy (due to gravity)***') 134 | 135 | %% total potential energy 136 | U=U1+U2 137 | 138 | pause 139 | 140 | disp('***robot gravity term***') 141 | 142 | %% compute G 143 | G=jacobian(U,q)' 144 | 145 | pause 146 | 147 | disp('***complete dynamic equations***') 148 | 149 | %% show complete dynamic equations 150 | M*[ddq1; ddq2]+c+G==[u1 u2]' 151 | 152 | disp('***end***') 153 | 154 | % end 155 | -------------------------------------------------------------------------------- /Scripts/2020/planar_2R.m: -------------------------------------------------------------------------------- 1 | %% 2R planar 2 | clear all 3 | 4 | syms q1 q2 real 5 | syms dq1 dq2 real 6 | syms ddq1 ddq2 real 7 | syms l1 l2 l3 real 8 | 9 | 10 | vec =[q1 q2]; 11 | %% define length of joints 12 | l1 = 1; 13 | l2= 1; 14 | %% define point 15 | p=[l1*cos(q1)+l2*cos(q1+q2); 16 | l1*sin(q1)+l2*sin(q1+q2);]; 17 | 18 | %% Analytic Jacobian 19 | J=simplify(jacobian(p,vec)) 20 | pause 21 | 22 | %% define velocity in Cartesian Space 23 | pd= [1; 1]; 24 | 25 | %% define acceleration in Cartesian Space 26 | pdd=[4;2]; 27 | 28 | %% time derivative of the Jacobian 3R 29 | dJ=simplify(diff(J,q1)*dq1 + diff(J,q2)*dq2) 30 | 31 | pause 32 | 33 | ddJ = simplify(diff(dJ,q1)*dq1 + diff(dJ,q2)*dq2 + diff(dJ,dq1)*ddq1 + diff(dJ,dq2)*ddq2) 34 | 35 | 36 | -------------------------------------------------------------------------------- /Scripts/2020/planar_3R.m: -------------------------------------------------------------------------------- 1 | %% 3R planar 2 | clear all 3 | clc 4 | syms q1 q2 q3 real 5 | syms dq1 dq2 dq3 real 6 | syms ddq1 ddq2 ddq3 real 7 | syms l1 l2 l3 real 8 | syms l real 9 | 10 | vec =[q1 q2 q3]; 11 | %% define length of joints 12 | l1 = 1; 13 | l2= 1; 14 | l3 =1; 15 | 16 | %% define point 17 | p=[l1*cos(q1)+l2*cos(q1+q2)+l3*cos(q1+q2+q3); 18 | l1*sin(q1)+l2*sin(q1+q2)+l3*sin(q1+q2+q3);]; 19 | 20 | %% Analytic Jacobian 21 | J=simplify(jacobian(p,vec)) 22 | pause 23 | 24 | %% define velocity in Cartesian Space 25 | pd= [1; 1]; 26 | 27 | %% define acceleration in Cartesian Space 28 | pdd=[4;2]; 29 | 30 | %% time derivative of the Jacobian 3R 31 | dJ = simplify(diff(J,q1)*dq1 + diff(J,q2)*dq2 + diff(J,q3)*dq3) 32 | pause 33 | 34 | ddJ = simplify(diff(dJ,q1)*dq1 + diff(dJ,q2)*dq2 + diff(dJ,q3)*dq3 + diff(dJ,dq1)*ddq1 + diff(dJ,dq2)*ddq2 + diff(dJ,dq3)*ddq3) 35 | -------------------------------------------------------------------------------- /Scripts/2020/planar_4R.m: -------------------------------------------------------------------------------- 1 | %% 4R planar 2 | clear all 3 | clc 4 | syms q1 q2 q3 q4 real 5 | syms dq1 dq2 dq3 dq4 real 6 | syms l1 l2 l3 l4 real 7 | 8 | 9 | vec =[q1 q2 q3 q4]; 10 | %% define length of joints 11 | % l1 = 0.2; 12 | % l2 = 0.2; 13 | % l3 = 0.2; 14 | % l4 = 0.2; 15 | 16 | %% define point 17 | p=[l1*cos(q1)+l2*cos(q1+q2)+l3*cos(q1+q2+q3)+ l4*cos(q1+q2+q3+q4); 18 | l1*sin(q1)+l2*sin(q1+q2)+l3*sin(q1+q2+q3)+l4*sin(q1+q2+q3+q4);]; 19 | 20 | %% Analytic Jacobian 21 | J=simplify(jacobian(p,vec)) 22 | 23 | pause 24 | 25 | %% define velocity in Cartesian Space 26 | pd= []; 27 | 28 | %% define acceleration in Cartesian Space 29 | pdd=[5;2]; 30 | 31 | %% time derivative of the Jacobian 4R 32 | dJ = diff(J,q1)*dq1 + diff(J,q2)*dq2 + diff(J,q3)*dq3 + diff(J,q4)*dq4 33 | -------------------------------------------------------------------------------- /Scripts/2020/planar_PPR.m: -------------------------------------------------------------------------------- 1 | %% PPR planar 2 | clear all 3 | 4 | syms q1 q2 q3 5 | syms qd3 6 | 7 | vec =[q1 q2 q3]; 8 | %% define length of joints 9 | syms l 10 | 11 | %% define point 12 | p=[q1+l*cos(q3); 13 | q2+l*sin(q3)]; 14 | 15 | %% Analytic Jacobian 16 | J=simplify(jacobian(p,vec)) 17 | pause 18 | 19 | %% define velocity in Cartesian Space 20 | pd= [-1;1]; 21 | 22 | %% define acceleration in Cartesian Space 23 | pdd=[]; 24 | 25 | %% time derivative of the Jacobian PPR 26 | Jd_PPR = [0 0 -l*cos(q3)*qd3; 27 | 0 0 -l*sin(q3)*qd3]; 28 | -------------------------------------------------------------------------------- /Scripts/2020/planar_RP.m: -------------------------------------------------------------------------------- 1 | %% RP planar 2 | clear all 3 | 4 | syms q1 q2 5 | syms qd2 qd3 6 | 7 | vec =[q1 q2]; 8 | 9 | %% define point 10 | p=[q2*cos(q1); 11 | q2*sin(q1)]; 12 | 13 | %% Analytic Jacobian 14 | J=simplify(jacobian(p,vec)) 15 | pause 16 | 17 | %% define velocity in Cartesian Space 18 | pd= []; 19 | 20 | %% define acceleration in Cartesian Space 21 | pdd=[]; 22 | 23 | %% time derivative of the Jacobian PPR 24 | Jd_RP =[cos(q1)*(qd2)-sin(q1)*qd1*q2; 25 | sin(q1)*(qd2)+cos(q1)*qd1*q2]; 26 | -------------------------------------------------------------------------------- /Scripts/2020/reduced_gradient.m: -------------------------------------------------------------------------------- 1 | %% Reduced Gradient 2 | function qd_red = reduced_gradient(JA,JB,T,pd) 3 | % takes as inputs: 4 | % - JA = the jacobian matrix JA of decomposion of J 5 | % of size MxM, non singular 6 | % - JB = the jacobian matrix JB of decomposion of J 7 | % of size Mx(N-M) 8 | % - T = identity matrix, corresponding to the q 9 | % variables of the Jacobean JA and JB. 10 | % For example J has dimension 2x3 = [JA JB]. 11 | % JA =[J1 J3] and JB = [J2]. 12 | % T will be = [1 0 0; 0 0 1; 0 1 0], first column 13 | % [1;0;0;] because we have q1, second column is 14 | % [0;0;1;] because we have q3 and the last column is 15 | % [0;1;0;] because we have q2 16 | % - pd = the cartesian velocity 17 | % 18 | % output: 19 | % - qd_red = q dot obtain by reduced gradient 20 | 21 | syms q1 q2 q3 22 | %% JA and JB two matrices give by decomposion that compose J 23 | J = [JA JB]; 24 | 25 | %% control if JA is full row rank 26 | if rank(JA) == size(JA,1) 27 | invJA=inv(JA); 28 | else 29 | disp("my friend, you have a problem") 30 | end 31 | 32 | %% Compute gradient of the objective function given by the problem 33 | % for example: H(q) = sin^2(q2)+sin^2(q3) 34 | % in this case fist row is zero because we don't have variable q1 35 | gradq_H= [0; 2*sin(q2)*cos(q2); 2*sin(q3)*cos(q3)]; 36 | 37 | %% Compute reduced gradient 38 | first_part = invJA*JB; 39 | transpose_first = -transpose(first_part); 40 | redgrad_qb_H = [transpose_first 1]*(T*gradq_H); 41 | 42 | %% q dot reduced gradient 43 | qd_red = transpose(T)*[invJA*(pd - JB * redgrad_qb_H); redgrad_qb_H] 44 | -------------------------------------------------------------------------------- /Scripts/2020/skewsymmetric.m: -------------------------------------------------------------------------------- 1 | 2 | %% Skewsymmetric 3 | syms a1 a2 a3 a4 real 4 | syms q1 q2 q3 real 5 | syms dq1 dq2 dq3 real 6 | %% inertia matrix 7 | q = [q1;q2] 8 | 9 | M = [ a1+a2*sin(q2)^2+a3*cos(q2)^2 0; 10 | 0 a4] 11 | 12 | %% coriolis 13 | M1=M(:,1); 14 | C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1)) 15 | M2=M(:,2); 16 | C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2)) 17 | 18 | pause 19 | 20 | dq=[dq1;dq2]; 21 | c1=dq'*C1*dq; 22 | c2=dq'*C2*dq; 23 | c=simplify([c1;c2]) 24 | 25 | pause 26 | %% M dot 27 | 28 | dM = diff(M,q1)*dq1+diff(M,q2)*dq2 29 | 30 | pause 31 | %% skewsymm 32 | 33 | S1 = [dq'*C1; 34 | dq'*C2;] 35 | 36 | pause 37 | disp("skewsymmetric matrix") 38 | 39 | skew= simplify(dM-2*S1) 40 | pause 41 | %% not skewsymm 42 | 43 | S2= [dq2*sin(2*q2)*(a2-a3) 0; 44 | -dq1*sin(2*q2)*(a2-a3) 0] 45 | 46 | disp(" not skewsymmetric matrix") 47 | not_skew=simplify(dM-2*S2) 48 | -------------------------------------------------------------------------------- /Scripts/2023/Flavio/basic_stuff/get_angle_rot_mat.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/basic_stuff/get_angle_rot_mat.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/basic_stuff/hom_mat.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/basic_stuff/hom_mat.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/basic_stuff/inv_hom_mat.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/basic_stuff/inv_hom_mat.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/basic_stuff/isOrthonormal.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/basic_stuff/isOrthonormal.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/basic_stuff/isRotation.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/basic_stuff/isRotation.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/basic_stuff/rot_mat.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/basic_stuff/rot_mat.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/basic_stuff/useful_commands.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/basic_stuff/useful_commands.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dirkin_functions/directkin.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dirkin_functions/directkin.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dirkin_functions/dirkin_planar3R.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dirkin_functions/dirkin_planar3R.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dirkin_functions/p_ee.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dirkin_functions/p_ee.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dirkin_functions/planar_2R.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dirkin_functions/planar_2R.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dirkin_scripts/planar2R.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dirkin_scripts/planar2R.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/CoordinateTransofmration.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/CoordinateTransofmration.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/NewtonEuler.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/NewtonEuler.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmodAntropomorphic.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/dynmodAntropomorphic.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmodPPR.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/dynmodPPR.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmodPPRR.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/dynmodPPRR.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmodPRP.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/dynmodPRP.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmodPRRR.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/dynmodPRRR.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmodPlanar2R.asv: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/dynmodPlanar2R.asv -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmodPlanar2R.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/dynmodPlanar2R.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmodPlanar3R.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/dynmodPlanar3R.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmodPlanarPR.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/dynmodPlanarPR.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmodPlanarRP.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/dynmodPlanarRP.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmodPolar2R.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/dynmodPolar2R.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmodSpatial3R.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/dynamics/dynmodSpatial3R.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/dynamics/dynmod_PRrobot.m: -------------------------------------------------------------------------------- 1 | % dynamic model of PR planar robot under gravity 2 | % Lagrangian formulation in symbolic form 3 | % A. De Luca 4 | % 23 March 2010 5 | 6 | clear all 7 | clc 8 | 9 | syms m1 m2 d I2xx I2yy I2zz real %symbolic variables explicitly defined as real 10 | syms q1 q2 dq1 dq2 g0 real 11 | 12 | disp('**** dynamic model of PR planar robot in the vertical plane ****') 13 | disp(' ') 14 | 15 | pause 16 | 17 | disp('*kinetic energy of link 1*') 18 | 19 | T1=(1/2)*m1*dq1^2 20 | 21 | pause 22 | 23 | disp('*kinetic energy of link 2 - linear part*') 24 | 25 | pc2=[q1+d*cos(q2) d*sin(q2) 0]' 26 | vc2=diff(pc2,q1)*dq1+diff(pc2,q2)*dq2 27 | T2c= (1/2)*m2*vc2'*vc2 28 | 29 | pause 30 | 31 | disp('*kinetic energy of link 2 - angular part*') 32 | 33 | om2=[0 0 dq2]' 34 | T2a=(1/2)*om2'*diag([I2xx I2yy I2zz])*om2 35 | 36 | T2=T2c+T2a; 37 | pause 38 | 39 | disp('***robot kinetic energy***') 40 | 41 | T=T1+T2 42 | 43 | pause 44 | 45 | disp('*simplifying*') 46 | 47 | T=simplify(T1+T2) 48 | T=collect(T,dq1^2) 49 | T=collect(T,dq2^2) 50 | 51 | pause 52 | 53 | disp('***robot inertia matrix***') 54 | 55 | B(1,1)=diff(T,dq1,2); 56 | B(2,2)=diff(T,dq2,2); 57 | TempB12=diff(T,dq1); 58 | B(1,2)=diff(TempB12,dq2); 59 | B(2,1)=B(1,2); 60 | B=simplify(B) 61 | 62 | pause 63 | 64 | disp('*Christoffel matrices*') 65 | 66 | q=[q1;q2]; 67 | b1=B(:,1); 68 | C1=(1/2)*(jacobian(b1,q)+jacobian(b1,q)'-diff(B,q1)) 69 | b2=B(:,2); 70 | C2=(1/2)*(jacobian(b2,q)+jacobian(b2,q)'-diff(B,q2)) 71 | 72 | pause 73 | 74 | disp('***robot Coriolis and centrifugal term***') 75 | 76 | dq=[dq1;dq2]; 77 | c1=dq'*C1*dq; 78 | c2=dq'*C2*dq; 79 | c=[c1;c2] 80 | 81 | pause 82 | 83 | disp('*potential energy of link 1*') 84 | 85 | U1=0 86 | 87 | pause 88 | 89 | disp('*potential energy of link 2*') 90 | 91 | g=[0;-g0;0] 92 | 93 | U2=-m2*g'*pc2 94 | 95 | pause 96 | 97 | disp('***robot potential energy (due to gravity)***') 98 | 99 | U=simplify(U1+U2) 100 | 101 | pause 102 | 103 | disp('***robot gravity term***') 104 | 105 | G=jacobian(U,q)' 106 | 107 | disp('***end***') 108 | 109 | % end 110 | -------------------------------------------------------------------------------- /Scripts/2023/Flavio/exams/rob2_16_04_13.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/exams/rob2_16_04_13.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/exams/rob2_17_03_29.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/exams/rob2_17_03_29.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/exams/rob2_18_04_26.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/exams/rob2_18_04_26.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/exams/rob2_19_04_29.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/exams/rob2_19_04_29.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/exams/rob2_20_04_15.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/exams/rob2_20_04_15.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/exams/rob2_23_02_13.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/exams/rob2_23_02_13.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/invkin_functions/invkin_RRP.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/invkin_functions/invkin_RRP.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/invkin_functions/invkin_RRR.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/invkin_functions/invkin_RRR.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/invkin_functions/invkin_planar2R.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/invkin_functions/invkin_planar2R.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/invkin_functions/invkin_planar3R.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/invkin_functions/invkin_planar3R.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/invkin_scripts/ex6_midterm_2021.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/invkin_scripts/ex6_midterm_2021.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/invkin_scripts/test_RRP.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/invkin_scripts/test_RRP.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/invkin_scripts/test_RRR.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/invkin_scripts/test_RRR.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/orientation_representations/S.m: -------------------------------------------------------------------------------- 1 | function skew_symmetric_matrix = S(vector) 2 | x = vector(1); 3 | y = vector(2); 4 | z = vector(3); 5 | skew_symmetric_matrix = [0 -z y; z 0 -x; -y x 0]; 6 | end 7 | 8 | -------------------------------------------------------------------------------- /Scripts/2023/Flavio/orientation_representations/axisangle_dir.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/orientation_representations/axisangle_dir.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/orientation_representations/axisangle_inv.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/orientation_representations/axisangle_inv.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/orientation_representations/axisangle_script.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/orientation_representations/axisangle_script.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/orientation_representations/euler_dir.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/orientation_representations/euler_dir.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/orientation_representations/euler_inv.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/orientation_representations/euler_inv.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/orientation_representations/euler_script.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/orientation_representations/euler_script.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/orientation_representations/omega_phi.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/orientation_representations/omega_phi.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/orientation_representations/rpy_dir.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/orientation_representations/rpy_dir.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/orientation_representations/rpy_inv.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/orientation_representations/rpy_inv.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/random_scripts/calibration.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/random_scripts/calibration.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/redundancy/DynamicResolution.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/redundancy/DynamicResolution.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/redundancy/NullSpaceGradient.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/redundancy/NullSpaceGradient.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/redundancy/SNS.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/redundancy/SNS.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/redundancy/TaskPriority.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/redundancy/TaskPriority.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/redundancy/lq_problem.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/redundancy/lq_problem.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/redundancy/null_space.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/redundancy/null_space.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/templates/euler_template.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/templates/euler_template.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/templates/invkin_template.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/templates/invkin_template.mlx -------------------------------------------------------------------------------- /Scripts/2023/Flavio/templates/numerical_invkin_template.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2023/Flavio/templates/numerical_invkin_template.mlx -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/DH_to_JA.m: -------------------------------------------------------------------------------- 1 | function [Ja] = DH_to_JA(DHTABLE, prismatic_indices) 2 | %Function that find the angular jacobian for a robot 3 | % 4 | %parameters: 5 | %-DH in the order alpha a d theta, ex: 6 | % DHTABLE = [ pi/2 0 sym('d1') q1; 7 | % pi/2 0 0 q2; 8 | % pi/2 0 q3 0; 9 | % 0 sym('a4') 0 q4]; 10 | %-prismatic_indices: a list of indices of prismatic Joints ex: [2,4] 11 | % if there is no prismatic joint put [] 12 | syms alpha_ d a_ theta_ 13 | 14 | N = size(DHTABLE, 1); 15 | 16 | TDH = [cos(theta_) -sin(theta_)*cos(alpha_) sin(theta_)*sin(alpha_) a_*cos(theta_); 17 | sin(theta_) cos(theta_)*cos(alpha_) -cos(theta_)*sin(alpha_) a_*sin(theta_); 18 | 0 sin(alpha_) cos(alpha_) d; 19 | 0 0 0 1]; 20 | 21 | A = cell(1, N); 22 | 23 | 24 | for i = 1:N 25 | A{i} = subs(TDH, [alpha_, a_, d, theta_], DHTABLE(i, :)); 26 | end 27 | 28 | T = eye(4); 29 | rotation_0_to_i = cell(1, N); 30 | 31 | for i = 1:N 32 | T = T * A{i}; 33 | rotation_0_to_i{i} = simplify(T); 34 | end 35 | 36 | % Initialize the Jacobian matrix as symbolic 37 | Ja = sym(zeros(3, N)); 38 | 39 | % Unit vector along the z-axis 40 | z = [0; 0; 1]; 41 | Ja(:, 1) = z; 42 | % Compute the column vectors for the Jacobian matrix 43 | for i = 1:N-1 44 | R = rotation_0_to_i{i}(1:3, 1:3); 45 | zi = R * z; 46 | Ja(:, i+1) = zi; 47 | end 48 | 49 | % Substitute columns with zeros for prismatic joints 50 | for prismatic_index = prismatic_indices 51 | if prismatic_index ~= N 52 | Ja(:, prismatic_index) = zeros(3, 1); 53 | end 54 | end 55 | 56 | % Display or return the Jacobian matrix as needed 57 | disp('Angular Jacobian Matrix:'); 58 | disp(Ja); 59 | end 60 | 61 | -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/DH_to_JL.m: -------------------------------------------------------------------------------- 1 | function [JL] = DH_to_JL(DHTABLE, variables) 2 | %Function that find the linear jacobian for a robot 3 | % 4 | %parameters: 5 | %-DH in the order alpha a d theta, ex: 6 | % DHTABLE = [ pi/2 0 sym('d1') q1; 7 | % pi/2 0 0 q2; 8 | % pi/2 0 q3 0; 9 | % 0 sym('a4') 0 q4]; 10 | % 11 | %-variables: array containing the joint variables(i.e.: [q1,...,qn], where n is the number of the last joint). 12 | % Leave them as literals even if in the DH you defined the numerical value 13 | syms alpha_ d a_ theta_ 14 | 15 | N = size(DHTABLE, 1); 16 | 17 | TDH = [cos(theta_) -sin(theta_)*cos(alpha_) sin(theta_)*sin(alpha_) a_*cos(theta_); 18 | sin(theta_) cos(theta_)*cos(alpha_) -cos(theta_)*sin(alpha_) a_*sin(theta_); 19 | 0 sin(alpha_) cos(alpha_) d; 20 | 0 0 0 1]; 21 | 22 | A = cell(1, N); 23 | 24 | for i = 1:N 25 | A{i} = subs(TDH, [alpha_, a_, d, theta_], DHTABLE(i, :)); 26 | end 27 | 28 | T = eye(4); 29 | position_0_to_i = cell(1, N); 30 | 31 | for i = 1:N 32 | T = T * A{i}; 33 | position_0_to_i{i} = T(1:3,4); 34 | end 35 | 36 | p_0_e=position_0_to_i{N}; 37 | 38 | % compute the jacobian in one shot 39 | JL = simplify(jacobian(p_0_e, variables)); 40 | 41 | % Display or return the Jacobian matrix as needed 42 | disp('Linear Jacobian Matrix: '); 43 | disp(JL); 44 | end 45 | 46 | -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/DHmatrix.m: -------------------------------------------------------------------------------- 1 | function matrix = DHmatrix(alpha, a, d, theta) 2 | 3 | syms alpha a d theta; 4 | 5 | matrix = [cos(theta) -cos(alpha)*sin(theta) sin(alpha)*sin(theta) a*cos(theta); 6 | sin(theta) cos(alpha)*cos(theta) -sin(alpha)*cos(theta) a*sin(theta); 7 | 0 sin(alpha) cos(alpha) d; 8 | 0 0 0 1]; 9 | end -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/Exams/Exam_20_06_05.m: -------------------------------------------------------------------------------- 1 | num_joints = 3; % Number of joints 2 | m = 2; % Task dimension 3 | V = [-1 , -1.5 , -2; 4 | 1 , 1.5 , 2]; % First row lower limits, second row upper limits 5 | J = [-1 , -1 , -0.5 ; 6 | -0.366, -0.866, -0.866]; % Jacobian 7 | 8 | task = [2; 1]; % Task 9 | 10 | % SNS (task, J_array, constraint_min, constraint_max, velocity_initial_condition) 11 | [new_values, old_values] = SNS(task, {J}, V(1,:)', V(2,:)', zeros(num_joints, 1)); -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/Exams/Exam_20_07_15.m: -------------------------------------------------------------------------------- 1 | % @ Exam 20/07/15 2 | % -------- EXERCISE 1 -------- 3 | % Natural constraints 4 | vz = 0; 5 | wx = 0; 6 | wy = 0; 7 | fx = 0; 8 | fy = 0; 9 | tq_z = 0; 10 | 11 | % Artificial constraints 12 | vx = vx_d; 13 | vy = 0; 14 | wz = wz_d; 15 | fz = fz_d; 16 | tq_x = 0; 17 | tq_y = 0; -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/Exams/Exam_21_02_04.m: -------------------------------------------------------------------------------- 1 | % @ Exam 21/02/04 2 | % -------- EXERCISE 1 -------- 3 | syms q1 q2 q3 q4 real 4 | syms dq1 dq2 dq3 dq4 real 5 | syms ddq1 ddq2 ddq3 ddq4 real 6 | syms m1 m2 m3 m4 real 7 | syms I1 I2 I3 I4 real 8 | syms l1 real 9 | syms g 10 | syms dc1 dc2 dc3 dc4 real 11 | 12 | q = [q1; q2; q3; q4]; 13 | dq = [dq1; dq2; dq3; dq4]; 14 | ddq = [ddq1; ddq2; ddq3; ddq4]; 15 | 16 | num_joints = 4; 17 | 18 | % T computations 19 | 20 | % -------------------- T1 -------------------- 21 | % For computing px and py for q1, we need the DH Table (2R Planar) 22 | % dc_i = Distanza dall'inizio del joint i-1 e il centro di massa del link i 23 | % We use dc_i instead of L, because we are considering the distance from the 24 | % joint i-1 to the center of mass of the link i 25 | px1 = simplify(dc1*cos(q1)); 26 | py1 = simplify(dc1*sin(q1)); 27 | 28 | dpx1 = simplify(time_derivate(px1, q, dq)); 29 | dpy1 = simplify(time_derivate(py1, q, dq)); 30 | 31 | w1 = dq1; % Angular velocity of link 1 32 | v1 = simplify(dpx1^2 + dpy1^2); % Linear velocity of link 1 (already squared) 33 | 34 | T1 = simplify(0.5 * m1 * v1 + 0.5 * I1 * w1^2); 35 | 36 | % -------------------- T2 -------------------- 37 | px2 = simplify(l1 * cos(q1)); 38 | py2 = simplify(l1 * sin(q1)); 39 | 40 | dpx2 = simplify(time_derivate(px2, q, dq)); 41 | dpy2 = simplify(time_derivate(py2, q, dq)); 42 | 43 | w2 = dq1 + dq2; % Angular velocity of link 2 44 | v2 = simplify(dpx2^2 + dpy2^2); % Linear velocity of link 2 (already squared) 45 | 46 | T2 = simplify(0.5 * m2 * v2 + 0.5 * I2 * w2^2); 47 | 48 | % -------------------- T3 -------------------- 49 | % This is a prismatic joint, so no angular velocity is present 50 | px3 = simplify(l1 * cos(q1) + (q3 - dc3) * cos(q1 + q2)); 51 | py3 = simplify(l1 * sin(q1) + (q3 - dc3) * sin(q1 + q2)); 52 | 53 | dpx3 = simplify(time_derivate(px3, q, dq)); 54 | dpy3 = simplify(time_derivate(py3, q, dq)); 55 | 56 | v3 = simplify(dpx3^2 + dpy3^2); % Linear velocity of link 3 (already squared) 57 | 58 | T3 = simplify(0.5 * m3 * v3); 59 | 60 | % -------------------- T4 -------------------- 61 | px4 = simplify(l1 * cos(q1) + q3 * cos(q1 + q2) + dc4 * cos(q1 + q2 + q4)); 62 | py4 = simplify(l1 * sin(q1) + q3 * sin(q1 + q2) + dc4 * sin(q1 + q2 + q4)); 63 | 64 | dpx4 = simplify(time_derivate(px4, q, dq)); 65 | dpy4 = simplify(time_derivate(py4, q, dq)); 66 | 67 | w4 = simplify(dq1 + dq2 + dq4); % Angular velocity of link 4 68 | v4 = simplify(dpx4^2 + dpy4^2); % Linear velocity of link 4 (already squared) 69 | 70 | T4 = simplify(0.5 * m4 * v4 + 0.5 * I4 * w4^2); 71 | 72 | % ------------------------------------------- 73 | 74 | % Computation of T 75 | T = simplify(T1 + T2 + T3 + T4); 76 | 77 | % Inertia Matrix and Christoffel Symbols 78 | M = compute_inertia_matrix(T, dq, 3); 79 | [C, c_sub] = compute_christoffel_matrix(M, q, dq, 3); 80 | 81 | % Gravity Vector 82 | % In order to retrieve the gravity vector, we need to compute the potential energy 83 | % For the potential energy, we first need to retrieve the heights of the CoMs 84 | h1 = dc1 * sin(q1); 85 | h2 = l1 * sin(q1); 86 | h3 = l1 * sin(q1) + (q3 - dc3) * sin(q1 + q2); 87 | h4 = l1 * sin(q1) + q3 * sin(q1 + q2) + dc4 * sin(q1 + q2 + q4); 88 | 89 | % Potential Energy 90 | U1 = m1 * g * h1; 91 | U2 = m2 * g * h2; 92 | U3 = m3 * g * h3; 93 | U4 = m4 * g * h4; 94 | U = simplify(U1 + U2 + U3 + U4); 95 | 96 | % Final Gravity Vector 97 | G = transpose(simplify(jacobian(U, q))); 98 | G = collect(G, g); 99 | 100 | % Complete Dynamic Model 101 | dynamic_model = simplify(M * ddq + C + G); 102 | 103 | % Linear parametrization of the dynamic model -> TBD 104 | 105 | % Equilibrium G = 0 106 | eqn = G == 0; 107 | solutions = solve(eqn, q); 108 | -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/Exams/Exam_21_07_12.m: -------------------------------------------------------------------------------- 1 | % % @ Exam 21/07/12 2 | % % -------- EXERCISE 1 -------- 3 | syms q1(t) q2(t) q3(t) 4 | syms L real 5 | 6 | % Define the joint vector and joint velocity vector as functions of time 7 | q = [q1(t); q2(t); q3(t)]; 8 | 9 | % Direct kinematics of 3R robot 10 | p = [L * cos(q1) + L * cos(q1 + q2) + L * cos(q1 + q2 + q3); 11 | L * sin(q1) + L * sin(q1 + q2) + L * sin(q1 + q2 + q3)]; 12 | 13 | % The general kinematic control law q_dot is: 14 | J = simplify(jacobian(p, q)); 15 | J_dot = diff(J, t); 16 | 17 | p_dot = diff(p, t); 18 | p_dot_dot = diff(p_dot, t); 19 | 20 | disp('p_dot = ') 21 | disp(p_dot) 22 | 23 | disp('p_dot_dot = ') 24 | disp(p_dot_dot) 25 | 26 | %% -------- EXERCISE 3 -------- 27 | syms q1 q2 q3 real 28 | syms dq1 dq2 dq3 real 29 | syms ddq1 ddq2 ddq3 real 30 | syms m1 m2 m3 real 31 | syms I1 I2 I3 real 32 | syms l1 real 33 | syms g 34 | syms dc1 dc2 dc3 real 35 | 36 | q = [q1; q2; q3]; 37 | dq = [dq1; dq2; dq3]; 38 | ddq = [ddq1; ddq2; ddq3]; 39 | 40 | num_joints = 3; 41 | 42 | % T computations 43 | 44 | % -------------------- T1 -------------------- 45 | % For computing px and py for q1, we need the DH Table (2R Planar) 46 | % dc_i = Distanza dall'inizio del joint i-1 e il centro di massa del link i 47 | % We use dc_i instead of L, because we are considering the distance from the 48 | % joint i-1 to the center of mass of the link i 49 | px1 = 0; 50 | py1 = 0; 51 | 52 | dpx1 = simplify(time_derivate(px1, q, dq)); 53 | dpy1 = simplify(time_derivate(py1, q, dq)); 54 | 55 | w1 = dq1; % Angular velocity of link 1 56 | v1 = simplify(dpx1^2 + dpy1^2); % Linear velocity of link 1 (already squared) 57 | 58 | T1 = simplify(0.5 * m1 * v1 + 0.5 * I1 * w1^2); 59 | 60 | % -------------------- T2 -------------------- 61 | px2 = simplify(q2 - dc2) * cos(q1); 62 | py2 = simplify(q2 - dc2) * sin(q1); 63 | 64 | dpx2 = simplify(time_derivate(px2, q, dq)); 65 | dpy2 = simplify(time_derivate(py2, q, dq)); 66 | 67 | w2 = dq1; % Angular velocity of link 2 68 | v2 = simplify(dpx2^2 + dpy2^2); % Linear velocity of link 2 (already squared) 69 | 70 | T2 = simplify(0.5 * m2 * v2); 71 | 72 | % -------------------- T3 -------------------- 73 | % This is a prismatic joint, so no angular velocity is present 74 | px3 = simplify(q2 * cos(q1) + dc3*cos(q1 + q3)); 75 | py3 = simplify(q2 * sin(q1) + dc3*sin(q1 + q3)); 76 | 77 | dpx3 = simplify(time_derivate(px3, q, dq)); 78 | dpy3 = simplify(time_derivate(py3, q, dq)); 79 | 80 | w3 = dq1 + dq3; % Angular velocity of link 3 81 | v3 = simplify(dpx3^2 + dpy3^2); % Linear velocity of link 3 (already squared) 82 | 83 | T3 = simplify(0.5 * m3 * v3 + 0.5 * I3 * w3^2); 84 | 85 | % ------------------------------------------- 86 | 87 | % Computation of T 88 | T = simplify(T1 + T2 + T3); 89 | 90 | % Inertia Matrix and Christoffel Symbols 91 | M = compute_inertia_matrix(T, dq, 3); 92 | [C, c_sub] = compute_christoffel_matrix(M, q, dq, 3); 93 | 94 | % --------------- MOVING FRAMES ALGORITHM --------------- 95 | % alpha, a_i, d_i, theta_i 96 | DH_table = [ -pi/2 0 l1 q1; 97 | 0 l2 0 q2 ; 98 | ]; 99 | 100 | % Vector rc containing the values ^ir_{ci} that is the distance 101 | % between the i-th CoM from the i-th reference frame 102 | syms rc1x rc1y rc1z rc2x real 103 | rc1=[rc1x; rc1y; rc1z]; 104 | rc2=[rc2x; 0; 0]; 105 | rc = [rc1, rc2]; 106 | 107 | % Vector m containing the masses of the links 108 | m = [m1; m2]; 109 | 110 | % Baricentric Inertia Matrices 111 | Ic1 = [Ic1xx Ic1xy Ic1xz; 112 | Ic1xy Ic1yy Ic1yz; 113 | Ic1xz Ic1yz Ic1zz]; 114 | Ic2 = diag([Ic2xx Ic2yy Ic2zz]); 115 | 116 | I = [Ic1,Ic2]; 117 | 118 | % Moving Frames Algorithm 119 | [w_mf, v_mf, vc_mf, T_mf] = moving_frames_algorithm(n_joints, DH_table, qdots, m, rc, [], I); 120 | % ------------------------------------------------------- 121 | 122 | % Gravity Vector 123 | G = 0; 124 | 125 | % Complete Dynamic Model 126 | dynamic_model = simplify(M * ddq + C + G); 127 | 128 | % Linear parametrization of the dynamic model 129 | syms a1 a2 a3 a4 a5 a6 a7 a8 a9 real 130 | av1 = dc3 * m3; 131 | av2 = m3 + m2; 132 | av3 = I3 + dc3^2 * m3; 133 | av4 = I1 + I2 + I3 + m3 * dc3; 134 | av5 = dc3; 135 | av6 = dc2; 136 | av7 = I3; 137 | av8 = m3; 138 | av9 = m2; 139 | 140 | av = [av1, av2, av3, av4, av5, av6, av7, av8, av9]; 141 | a = [a1, a2, a3, a4, a5, a6, a7, a8, a9]; 142 | 143 | Ma = subs(M, av, a); 144 | Ca = subs(C, av, a); 145 | 146 | Ya = simplify(Ma * transpose([ddq1, ddq2, ddq3]) + Ca); 147 | Ysol = extract_Y_v3(Ya, a); 148 | 149 | % To verify the correctness of the computation, 150 | % We need to verify that: Ysol * transpose(a) == Ya; -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/Exams/Exam_22_07_08.m: -------------------------------------------------------------------------------- 1 | % @ Exam 22/07/08 2 | % -------- EXERCISE 1 -------- 3 | % :( 4 | 5 | % -------- EXERCISE 2 -------- 6 | % :( 7 | 8 | % -------- EXERCISE 3 -------- 9 | syms q1 q2 10 | syms dq1 dq2 11 | syms qd t T pi 12 | syms a b c d 13 | syms m1 m2 g0 14 | syms ddx ddy 15 | syms ux uy 16 | 17 | % General parameters 18 | q = [q1; q2]; 19 | dq = [dq1; dq2]; 20 | tau = t/T; 21 | 22 | % Initial and final conditions 23 | q_in = [1; 0.3]; 24 | q_d = [0.6; 0.7]; 25 | q_delta = q_d - q_in; 26 | q_delta_x = q_delta(1); 27 | q_delta_y = q_delta(2); 28 | 29 | dq_in = [0; 0]; 30 | dq_d = [0; 0]; 31 | 32 | % Dynamic model of the robot 33 | % This must be divided by two equations since we want to retrieve a Dynamic Model 34 | % for each joint (2 prismatic joints) 35 | dynamic_joint_1 = m1 + m2 * ddx == ux; 36 | dynamic_joint_2 = m2 * ddy + m2 * g0 == uy; 37 | 38 | % Solve the system by isolating ddx and ddy (we do this manually, hence) 39 | ddx = ux/(m1 + m2); 40 | ddy = (uy/m2) - g0; 41 | ddy_pos = (uy/m2) + g0; 42 | 43 | syms ty_sw ty real 44 | q_delta_y = (1/2 * ddy * ty_sw^2) + (1/2 * ddy * ty_sw * (ty - ty_sw)); 45 | 46 | % We want to isolate ty_sw and ty from the previous equation 47 | ty = sqrt(q_delta_y/ddy) + sqrt(q_delta_y/ddy_pos); 48 | % ty_sw_p = isolate(q_delta_y == 0, ty_sw); 49 | ty_sw_p = ty/2 * ((m2 * g0)/uy); 50 | 51 | % Calculations for Tx 52 | tx = 2 * sqrt(abs(q_delta_x/ddx)); 53 | tx_sw = tx/2; 54 | 55 | % % Polynomial trajectory 56 | % q_pos = q_in + q_delta * (a*tau^3 + b*tau^2 + c*tau + d); 57 | % q_vel= diff(q_pos, t)/T; 58 | % q_acc = diff(q_vel, t)/T; 59 | 60 | % cond1 = subs(q_pos, t, 0) == q_in; 61 | % cond2 = subs(q_vel, t, 0) == dq_in; 62 | % cond3 = subs(q_pos, t, T) == q_d; 63 | % cond4 = subs(q_vel, t, T) == dq_d; 64 | 65 | % sol = solve([cond1; cond2; cond3; cond4], [a, b, c, d]); 66 | % coef_symbols = [ a b c d ]; 67 | % coef_values = [sol.a sol.b sol.c sol.d]; 68 | 69 | % q_pos = subs(q_pos, coef_symbols, coef_values ); 70 | % q_vel = subs(q_vel, coef_symbols, coef_values); 71 | % q_acc = subs(q_acc, coef_symbols, coef_values); 72 | % q_acc = collect( expand(q_acc), pi); 73 | 74 | % syms tau1 tau2 d2 tau_1m tau_2m 75 | 76 | % tau = [tau1 tau2]; 77 | % tau_m = [tau_1m tau_2m]; 78 | % M = [m1 + m2 0 ; 0 m2]; 79 | 80 | % U1 = 0; 81 | % U2 = m2g0(q2-d2); 82 | % U = U1+U2; 83 | % U = transpose( jacobian(U, q) ); 84 | 85 | % L = norm(q_delta); 86 | % ddq = inv(M)*( transpose(tau) - U ); 87 | % ddq = subs(ddq, [tau1 tau2], [tau_1m tau_2m]) 88 | % ddq_m = subs(ddq, [m1 m2 g0 tau_1m tau_2m], [5 3 9.8 40 40] ); 89 | % ddq_m = double(ddq_m) 90 | % T1 = sqrt(abs(4*(q_delta(1)/ddq_m(1)))) 91 | % T2 = sqrt(abs(4*q_delta(2)/ddq_m(2))) -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/Exams/Exam_22_09_09.m: -------------------------------------------------------------------------------- 1 | % % @ Exam 22/09/09 2 | % % -------- EXERCISE 1 -------- 3 | % syms l1 l2 l3 real 4 | % syms q1(t) q2(t) q3(t) 5 | % syms q1_dot q2_dot q3_dot real 6 | % num_joints = 3; 7 | 8 | % % Define the joint vector and joint velocity vector as functions of time 9 | % q = [q1(t); q2(t); q3(t)]; 10 | 11 | % % Task: End-effector position 12 | % r = [l1 * cos(q1(t)) + l2 * cos(q1(t) + q2(t)) + l3 * cos(q1(t) + q2(t) + q3(t)); 13 | % l1 * sin(q1(t)) + l2 * sin(q1(t) + q2(t)) + l3 * sin(q1(t) + q2(t) + q3(t));]; 14 | 15 | % % The general kinematic control law q_dot is: 16 | % % q_dot = J# * r_dot + (I - J# * J) * q_dot_0 17 | % % where q_dot_0 = - grad(H(q)) 18 | % J = simplify(jacobian(r, q)); 19 | % J_pinv = simplify(pinv(J)); 20 | 21 | % % r_dot 22 | % r_dot = diff(r, t); 23 | 24 | % % I 25 | % I = eye(3); 26 | 27 | % % H(q) 28 | % q_min = [-(3*pi)/4; -(3*pi)/4; -(3*pi)/4]; 29 | % q_max = [(3*pi)/4; (3*pi)/4; (3*pi)/4]; 30 | % q_bar = (q_min + q_max) / 2; 31 | % H = sum((q - q_bar).^2 / (q_max - q_min).^2, "all"); 32 | % H = 1/(2*num_joints) * H; 33 | 34 | % % grad(H) 35 | % gradH = gradient(H, q); 36 | 37 | % % q_dot: J# * r_dot + (I - J# * J) * q_dot_0 38 | % % Since we do not move the end-effector, r_dot = 0, hence: 39 | % % q_dot = (I - J# * J) * q_dot_0 40 | % q_dot = simplify((I - J_pinv * J) * -gradH); 41 | 42 | % disp('q_dot = (I - J^# * J) * q_dot_0') 43 | % disp(q_dot) 44 | 45 | % H_sol = double(subs(H, q, [0, 2*pi/3, 2*pi/3].')); 46 | % grad_H_sol = double(subs(gradH, q, [0, 2*pi/3, 2*pi/3].')); 47 | 48 | % disp('H(q) = ') 49 | % disp(H_sol) 50 | % disp('grad(H) = ') 51 | % disp(grad_H_sol) 52 | 53 | % -------- EXERCISE 2 -------- 54 | % :( 55 | 56 | % -------- EXERCISE 3 -------- 57 | syms B M g f real 58 | syms theta dtheta ddtheta theta_desired 59 | syms Kp 60 | % Derive the Dynamic model of the system of masses 61 | dynamic_fig_a = (B + M) * ddtheta + (M * g) == f; 62 | 63 | lyap_candidate = 1/2 * (B + M) * dtheta^2 + 1/2 * Kp * (theta - theta_desired)^2; 64 | lyap_candidate_dot = diff(lyap_candidate, theta) * dtheta + diff(lyap_candidate, dtheta) * ddtheta; -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/Exams/Exam_22_10_21.m: -------------------------------------------------------------------------------- 1 | % @ Exam 22/10/021 2 | % % -------- EXERCISE 1 -------- 3 | 4 | %% ========= Apply the Moving Frames Algorithm ========= 5 | syms Ic1xx Ic1xy Ic1xz Ic1yx Ic1yy Ic1yz Ic1zx Ic1zy Ic1zz Ic2xx Ic2yy Ic2zz real 6 | syms l1 l2 real 7 | syms m1 m2 real 8 | syms q1 q2 q1_dot q2_dot q1_ddot q2_ddot real 9 | syms alpha a d theta 10 | syms g real 11 | 12 | n_joints = 2; 13 | q = [q1; q2]; 14 | qdots = [q1_dot; q2_dot]; 15 | qddots = [q1_ddot; q2_ddot]; 16 | 17 | % alpha, a_i, d_i, theta_i 18 | DH_table = [ -pi/2, 0, q1, -pi/2; 19 | -pi/2, 0, 0, q2 ; 20 | ]; 21 | 22 | % Vector rc containing the values ^ir_{ci} that is the distance 23 | % between the i-th CoM from the i-th reference frame 24 | syms rc1x rc1y rc1z rc2x rc2y rc2z dc2 real 25 | rc1 = [0; 0; 0]; 26 | rc2 = [dc2; 0; 0]; 27 | rc = [rc1, rc2]; 28 | 29 | % Vector m containing the masses of the links 30 | m = [m1; m2]; 31 | 32 | % Baricentric Inertia Matrices 33 | Ic1 = diag([Ic1xx, Ic1yy, Ic1zz]); 34 | Ic2 = diag([Ic2xx, Ic2yy, Ic2zz]); 35 | 36 | I = [Ic1, Ic2]; 37 | 38 | % Moving Frames Algorithm 39 | [w, v, vc, T] = moving_frames_algorithm(n_joints, DH_table, qdots, m, rc, 1, I); 40 | 41 | % Extract the kinetic energy T 42 | T_total = simplify(T{1} + T{2}); 43 | 44 | % Computations for the Inertia Matrix 45 | M = compute_inertia_matrix(T_total, qdots, 3); 46 | 47 | % Christoffel Matrix 48 | [C, c_sub] = compute_christoffel_matrix(M, q, qdots, 3); 49 | disp("The Christoffel Matrix is:"); 50 | disp(simplify(C)); 51 | 52 | % Gravity Vector 53 | % In order to retrieve the gravity vector, we need to compute the potential energy 54 | % For the potential energy, we first need to retrieve the heights of the CoMs 55 | h1 = q1; 56 | h2 = q1 + dc2*cos(q2); 57 | 58 | % Gravity here is positive, hence g = g 59 | g = g; 60 | 61 | U1 = - m1 * g * h1; 62 | U2 = - m2 * g * h2; 63 | U = U1 + U2; 64 | 65 | G = transpose(simplify(jacobian(U, [q1, q2]))); 66 | G = collect(G, g); 67 | 68 | % Complete Dynamic Model | M(q) * ddq + C(q, dq) + G(q) = tau 69 | dynamic_model = simplify(M * transpose([q1_ddot, q2_ddot]) + C + G); 70 | 71 | % Linear parametrization of the dynamic model 72 | syms a1 a2 a3 real 73 | av1 = m1 + m2; 74 | av2 = dc2*m2; 75 | av3 = m2*dc2^2 + Ic2yy; 76 | av = [av1, av2, av3]; 77 | a = [a1, a2, a3]; 78 | 79 | %% Inertia Matrix from solutions 80 | % M = [m1 + m2, -m2*dc2*sin(q2); 81 | % -m2*dc2*sin(q2), m2*dc2^2+Ic2yy]; 82 | 83 | Ma = subs(M, av, a); 84 | Ca = subs(C, av, a); 85 | Ga = subs(G, av, a); 86 | 87 | Y_parametrized = simplify(Ma * transpose([q1_ddot, q2_ddot]) + Ca + Ga); 88 | Y_parametrized = collect(Y_parametrized, sin(q2)); 89 | Y = extract_Y_v3(Y_parametrized, a); 90 | 91 | disp('Dynamic Model parametrized with a:') 92 | disp(Y) 93 | 94 | %% ===================================================== 95 | % Now try to solve this exercise as the usual way, meaning computing: 96 | % - Kinetic Energy 97 | % - Potential Energy 98 | % - Compute T 99 | % - Compute M 100 | % - Compute C 101 | % - Compute G 102 | % - Compute U 103 | % - Compute the Dynamic Model 104 | -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/Exams/Exam_23_01_25.m: -------------------------------------------------------------------------------- 1 | % @ Exam 23/01/25 2 | % -------- EXERCISE 1.a -------- 3 | syms q1 q2 real 4 | syms dq1 dq2 real 5 | syms ddq1 ddq2 real 6 | syms m1 m2 I1 I2 real 7 | syms l1 g real 8 | syms dc1 real 9 | syms fv1 fv2 real 10 | 11 | q = [q1; q2]; 12 | dq = [dq1; dq2]; 13 | ddq = [ddq1; ddq2]; 14 | 15 | num_joints = 2; 16 | 17 | % T computations 18 | 19 | % -------------------- T1 -------------------- 20 | % For computing px and py for q1, we need the DH Table (2R Planar) 21 | % dc_i = Distanza dall'inizio del joint i-1 e il centro di massa del link i 22 | % We use dc_i instead of L, because we are considering the distance from the 23 | % joint i-1 to the center of mass of the link i 24 | px1 = dc1 * cos(q1); 25 | py1 = dc1 * sin(q1); 26 | 27 | dpx1 = time_derivate(px1, q, dq); 28 | dpy1 = time_derivate(py1, q, dq); 29 | 30 | w1 = dq1; % Angular velocity of link 1 31 | v1 = simplify(dpx1^2 + dpy1^2); % Linear velocity of link 1 32 | 33 | T1 = 0.5 * m1 * v1 + 0.5 * I1 * w1^2; 34 | 35 | % -------------------- T2 -------------------- 36 | % For computing px and py for q2, we need the DH Table (2R Planar) 37 | px2 = l1 * cos(q1); 38 | py2 = l1 * sin(q1); 39 | 40 | dpx2 = time_derivate(px2, q, dq); 41 | dpy2 = time_derivate(py2, q, dq); 42 | 43 | w2 = dq1 + dq2; % Angular velocity of link 2 44 | v2 = simplify(dpx2^2 + dpy2^2); % Linear velocity of link 2 45 | 46 | T2 = 0.5 * m2 * v2 + 0.5 * I2 * w2^2; 47 | 48 | % ------------------------------------------- 49 | 50 | % Computation of T 51 | T = simplify(T1 + T2); 52 | 53 | % Inertia Matrix and Christoffel Symbols 54 | M = compute_inertia_matrix(T, dq, 3); 55 | [C, c_sub] = compute_christoffel_matrix(M, q, dq, 3); 56 | 57 | % Gravity Vector 58 | % In order to retrieve the gravity vector, we need to compute the potential energy 59 | % For the potential energy, we first need to retrieve the heights of the CoMs 60 | h1 = dc1 * cos(q1); 61 | h2 = l1 * cos(q1); 62 | 63 | U1 = -m1 * g * h1; 64 | U2 = -m2 * g * h2; 65 | U = U1 + U2; 66 | 67 | G = transpose(simplify(jacobian(U, q))); 68 | G = collect(G, g); 69 | 70 | % Friction Vector: -F * q_dot (For a single joint is: -Fv_i * dq_i) 71 | Fv = [fv1; fv2]; 72 | F = Fv .* dq; 73 | 74 | % Complete Dynamic Model 75 | dynamic_model = simplify(M * ddq + C + G + F); 76 | disp('Dynamic Model:') 77 | disp(dynamic_model) 78 | 79 | % Linear parametrization of the dynamic model 80 | syms a1 a2 a3 a4 a5 real 81 | av1 = m1*dc1^2 + m2*l1^2 + I1 + I2; 82 | av2 = dc1*m1 + l1*m2; 83 | av3 = I2; 84 | av4 = fv1; 85 | av5 = fv2; 86 | av = [av1, av2, av3, av4, av5]; 87 | a = [a1, a2, a3, a4, a5]; 88 | 89 | Ma = subs(M, av, a); 90 | Ca = subs(C, av, a); 91 | Ga = subs(G, av, a); 92 | 93 | Ya = simplify(Ma * transpose([ddq1, ddq2]) + Ca + Ga); 94 | Ysol = extract_Y_v3(Ya, a); 95 | 96 | % -------- EXERCISE 1.b -------- 97 | % As for the steps, we need to find G, but we already computed it in the previous step 98 | 99 | % We need to find now the Jacobian of G 100 | G_q = simplify(jacobian(G, q)); 101 | 102 | % Find the maximum eigenvalue of G_q 103 | eig_G_q = simplify(eig(G_q)); 104 | max_eig = simplify(max(eig_G_q(2))); 105 | disp('Max eigenvalue of G_q:') 106 | disp(max_eig) 107 | 108 | alpha = g * (dc1*m1 + l1*m2); 109 | disp("Lower bound:") 110 | disp(alpha) 111 | 112 | % Kp must be >= alpha, but we do not choose it explicitly 113 | % Kd must be 0 instead, since we have viscous friction 114 | % In order to find a proper Kp: 115 | % ||G(q) - G(qd)|| <= Kp * ||q - qd|| 116 | % Then solve the inequality for Kp 117 | 118 | % -------- EXERCISE 1.c on paper -------- -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/Exams/Exam_24_04_24.m: -------------------------------------------------------------------------------- 1 | %% Apply the Moving Frames Algorithm 2 | syms Ic1xx Ic1xy Ic1xz Ic1yx Ic1yy Ic1yz Ic1zx Ic1zy Ic1zz Ic2xx Ic2yy Ic2zz real 3 | syms l1 l2 real 4 | syms m1 m2 real 5 | syms q1 q2 q1_dot q2_dot real 6 | syms alpha a d theta 7 | 8 | n_joints = 2; 9 | q = [q1; q2]; 10 | qdots = [q1_dot; q2_dot]; 11 | 12 | % alpha, a_i, d_i, theta_i 13 | DH_table = [ -pi/2, 0, l1, q1; 14 | 0, l2, 0, q2 ; 15 | ]; 16 | 17 | % Vector rc containing the values ^ir_{ci} that is the distance 18 | % between the i-th CoM from the i-th reference frame 19 | syms rc1x rc1y rc1z rc2x real 20 | rc1=[rc1x; rc1y; rc1z]; 21 | rc2=[rc2x; 0; 0]; 22 | rc = [rc1, rc2]; 23 | 24 | % Vector m containing the masses of the links 25 | m = [m1; m2]; 26 | 27 | % Baricentric Inertia Matrices 28 | Ic1 = [Ic1xx Ic1xy Ic1xz; 29 | Ic1xy Ic1yy Ic1yz; 30 | Ic1xz Ic1yz Ic1zz]; 31 | Ic2 = diag([Ic2xx, Ic2yy, Ic2zz]); 32 | 33 | I = [Ic1, Ic2]; 34 | 35 | % Moving Frames Algorithm 36 | [w, v, vc, T] = moving_frames_algorithm(n_joints, DH_table, qdots, m, rc, [], I); 37 | 38 | % Extract the kinetic energy T 39 | T = simplify(T{1} + T{2}); 40 | 41 | % Computations for the Inertia Matrix 42 | M = compute_inertia_matrix(T, qdots, 3); 43 | 44 | % EXTRA: Substitutions for simplifications 45 | M = subs(M, sin(q2)^2, (1-cos(q2)^2)); 46 | M = simplify(M); 47 | M = collect(M, cos(q2)); 48 | M = simplify(M); 49 | 50 | disp("The Inertia Matrix is:"); 51 | disp(M); 52 | 53 | % Christoffel Matrix 54 | [C, c_sub] = compute_christoffel_matrix(M, q, qdots, 3); 55 | disp("The Christoffel Matrix is:"); 56 | disp(simplify(C)); 57 | 58 | % Linear parametrization 59 | % In this case we already know the values from the solutions 60 | % but we should have parametrized the values manually 61 | syms a1 a2 a3 real 62 | M = [a1+a2*cos(q2)^2, 0; 63 | 0, a3]; 64 | [c,C] = compute_christoffel_matrix(M, q, qdots, 3); -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/Exams/Exam_24_06_12.m: -------------------------------------------------------------------------------- 1 | % @ Exam 21/07/12 2 | % -------- EXERCISE 1 -------- 3 | syms a2 a3 real positive 4 | syms rc1x rc1y rc1z rc2x real 5 | syms m1 m2 m3 real positive 6 | syms Ic1xx Ic1yy Ic1zz Ic2xx Ic2yy Ic2zz Ic3xx Ic3yy Ic3zz real positive 7 | syms dc1 dc2 dc3 real positive 8 | syms theta_1 theta_2 theta_3 real 9 | syms theta_1_dot theta_2_dot theta_3_dot real 10 | syms theta_1_ddot theta_2_ddot theta_3_ddot real 11 | 12 | % --------------- MOVING FRAMES ALGORITHM --------------- 13 | n_joints = 3; 14 | 15 | theta = [theta_1; theta_2; theta_3]; 16 | thetadots = [theta_1_dot; theta_2_dot; theta_3_dot]; 17 | thetaddots = [theta_1_ddot; theta_2_ddot; theta_3_ddot]; 18 | 19 | % alpha, a_i, d_i, theta_i 20 | DH_table = [ pi/2 0 0 theta_1; 21 | 0 a2 0 theta_2; 22 | 0 a3 0 theta_3; 23 | ]; 24 | 25 | % Vector m containing the masses of the links 26 | m = [m1; m2; m3]; 27 | 28 | % Vector rc containing the values ^ir_{ci} that is the distance 29 | % between the i-th CoM from the i-th reference frame 30 | rc1 = [dc1; 0; 0]; 31 | rc2 = [dc2 - a2; 0; 0]; 32 | rc3 = [dc3 - a3; 0; 0]; 33 | 34 | rc = [rc1, rc2, rc3]; 35 | 36 | % Vector I containing the inertia matrices of the links 37 | Ic1 = diag([Ic1xx Ic1yy Ic1zz]); 38 | Ic2 = diag([Ic2xx Ic2yy Ic2zz]); 39 | Ic3 = diag([Ic3xx Ic3yy Ic3zz]); 40 | 41 | I = [Ic1, Ic2, Ic3]; 42 | 43 | [w_mf, v_mf, vc_mf, T_mf] = moving_frames_algorithm(n_joints, DH_table, thetadots, m, rc, [], I); 44 | 45 | % Extract the T matrix 46 | T = simplify(T_mf{1} + T_mf{2} + T_mf{3}); 47 | 48 | % Inertia Matrix and Christoffel Symbols 49 | M = compute_inertia_matrix(T, thetadots, 3); 50 | [C, c_sub] = compute_christoffel_matrix(M, theta, thetadots, 3); 51 | 52 | % -------- EXERCISE 2 -------- 53 | -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/Exams/Exam_24_07_08.m: -------------------------------------------------------------------------------- 1 | % @ Exam 24/07/08 2 | % -------- EXERCISE 1 -------- 3 | syms q1 q2 q3 dq1 dq2 dq3 ddq1 ddq2 ddq3 real 4 | syms m1 m2 m3 real 5 | syms dc1 dc2 dc3 real 6 | syms g real 7 | syms Ic2 Ic3 real 8 | syms alpha a d theta 9 | syms l2 real % L2 needed for the DH Table 10 | 11 | q = [q1; q2; q3]; 12 | qdots = [dq1, dq2, dq3]; 13 | qddots = [ddq1, ddq2, ddq3]; 14 | 15 | % Computations for the Kinetic Energy 16 | % ------------------ T1 ------------------ 17 | T1 = 0.5 * m1 * dq1^2; 18 | 19 | % ------------------ T2 ------------------ 20 | % For computing px and py for q2, we need the DH Table (of a PRR Spatial) 21 | n_joints = 3; 22 | 23 | % alpha, a_i, d_i, theta_i 24 | DH_table = [ -pi/2 0 q1 0; 25 | pi/2 0 l2 q2 ; 26 | 0 dc3 0 q3 ; 27 | ]; 28 | 29 | DH = DHmatrix(alpha, a, d, theta); 30 | A = cell(1, n_joints); 31 | 32 | % Compute each transformation matrix 33 | for i = 1: n_joints 34 | A{i} = subs(DH, {alpha, a, d, theta}, DH_table(i, :)); 35 | end 36 | 37 | T = eye(4); 38 | 39 | %disp("-------------------------------------------------------------------") 40 | for i = 1 : n_joints 41 | % Display print statement 42 | %disp(['0^A_' num2str(i) '(q_' num2str(i) '):']); 43 | 44 | % Perform the simplify operation 45 | T = simplify(T * A{i}); 46 | 47 | % Display the simplified result 48 | %disp(T); 49 | end 50 | 51 | % output world-endEff T matrix 52 | %disp("-------------------------------------------------------------------") 53 | %disp('W^T_E:') 54 | %disp(T) 55 | 56 | % Remember. T is a matrix 4x4 57 | % Output WE position vector | From 1st to 3rd row of 4th column 58 | p = T(1:3, 4); 59 | %disp('Position vector p:') 60 | %disp(p) 61 | 62 | % Rotation Matrix -> From 1st to 3rd row of the first three columns 63 | R = T(1:3, 1:3); 64 | 65 | %disp("-------------------------------------------------------------------") 66 | 67 | % For computing the linear velocity of the second joint, we need to compute the position vector of the second joint. 68 | % We must, though, compute the position vector of the CoM of the second link 69 | px_joint2 = 0; 70 | py_joint2 = dc2; 71 | pz_joint2 = q1; 72 | 73 | % Derive the position vector of the CoM of the second link 74 | dpx_joint2 = diff(px_joint2, q1) * dq1 + diff(px_joint2, q2) * dq2; 75 | dpy_joint2 = diff(py_joint2, q1) * dq1 + diff(py_joint2, q2) * dq2; 76 | dpz_joint2 = diff(pz_joint2, q1) * dq1 + diff(pz_joint2, q2) * dq2; 77 | 78 | % ===> Linear velocity of the second joint 79 | velocity_joint2 = [dpx_joint2; dpy_joint2; dpz_joint2]; 80 | velocity_joint2 = simplify(velocity_joint2.' * velocity_joint2); 81 | 82 | % ===> Angular velocity of the second joint 83 | w2 = dq2; 84 | 85 | T2 = simplify(0.5 * m2 * velocity_joint2 + 0.5 * Ic2 * w2^2); 86 | 87 | % ------------------ T3 ------------------ 88 | % For computing the linear velocity of the third joint, we need to compute the position vector of the third joint. 89 | % We must, though, compute the position vector of the CoM of the third link 90 | px_joint3 = dc3*cos(q2)*cos(q3); 91 | py_joint3 = l2 + dc3*sin(q3); 92 | pz_joint3 = q1 - dc3*cos(q3)*sin(q2); 93 | 94 | % Derive the position vector of the CoM of the third link 95 | dpx_joint3 = diff(px_joint3, q1) * dq1 + diff(px_joint3, q2) * dq2 + diff(px_joint3, q3) * dq3; 96 | dpx_joint3 = collect(dpx_joint3, dc3); 97 | dpy_joint3 = diff(py_joint3, q1) * dq1 + diff(py_joint3, q2) * dq2 + diff(py_joint3, q3) * dq3; 98 | dpz_joint3 = diff(pz_joint3, q1) * dq1 + diff(pz_joint3, q2) * dq2 + diff(pz_joint3, q3) * dq3; 99 | dpz_joint3 = collect(dpz_joint3, dc3); 100 | 101 | % ===> Linear velocity of the second joint 102 | v_joint3 = simplify(transpose(R) * [dpx_joint3; dpy_joint3; dpz_joint3]); 103 | v_joint3 = simplify(norm(v_joint3.' * v_joint3)^2); 104 | 105 | % ===> Angular velocity of the second joint 106 | % To express the angular velocity of the joint 3 we can use the formula: S = R_dot' * R 107 | R_dot = diff(R, q1) * dq1 + diff(R, q2) * dq2 + diff(R, q3) * dq3; 108 | skew_matrix = simplify(R_dot * R'); 109 | 110 | % Now, for extracting the angular velocity, we need to extract the skew-symmetric matrix terms: 111 | % [-Skew23, skew13, -skew21] 112 | w3_0_3 = [-skew_matrix(2,3); skew_matrix(1,3); -skew_matrix(2,1)]; 113 | w3 = simplify(R' * w3_0_3); 114 | 115 | T3 = simplify(0.5 * m3 * v_joint3 + 0.5 * transpose(w3) * Ic3 * w3); 116 | 117 | % Computation of T 118 | T = simplify(T1 + T2 + T3); 119 | 120 | % Computations for the Inertia Matrix 121 | M = compute_inertia_matrix(T, [dq1, dq2, dq3], 3); 122 | 123 | % Christoffel Matrix 124 | [C, c_sub] = compute_christoffel_matrix(M, [q1, q2, q3], [dq1, dq2, dq3], 3); 125 | 126 | % ------------- MOVING FRAMES ALGORITHM ------------- 127 | n_joints = 3; 128 | 129 | % alpha, a_i, d_i, theta_i 130 | DH_table = [ -pi/2 0 q1 0; 131 | pi/2 0 l2 q2 ; 132 | 0 dc3 0 q3 ; 133 | ]; 134 | 135 | % Vector m containing the masses of the links 136 | m = [m1; m2; m3]; 137 | 138 | % Vector rc containing the values ^ir_{ci} that is the distance 139 | % between the i-th CoM from the i-th reference frame 140 | rc = [rc1; rc2; rc3]; 141 | 142 | % Vector I containing the baricentric inertia matrices of the links 143 | I = [Ic1, Ic2, Ic3]; 144 | 145 | [w_mf, v_mf, vc_mf, T_mf] = moving_frames_algorithm(n_joints, DH_table, qdots, m, rc, 1, I); 146 | % --------------------------------------------------- 147 | 148 | % Gravity Vector 149 | % In order to retrieve the gravity vector, we need to compute the potential energy 150 | % For the potential energy, we first need to retrieve the heights of the CoMs 151 | h1 = q1 - dc1; 152 | h2 = q1; 153 | h3 = q1 - dc3*cos(q3)*sin(q2); 154 | 155 | U1 = m1 * g * h1; 156 | U2 = m2 * g * h2; 157 | U3 = m3 * g * h3; 158 | U = U1 + U2 + U3; 159 | 160 | G = transpose(simplify(jacobian(U, [q1, q2, q3]))); 161 | G = collect(G, g); 162 | 163 | % Complete Dynamic Model | M(q) * ddq + C(q, dq) + G(q) = tau 164 | dynamic_model = simplify(M * transpose([ddq1, ddq2, ddq3]) + C + G); 165 | 166 | % Linear parametrization of the dynamic model 167 | syms a1 a2 a3 real 168 | av1 = m1 + m2 + m3; 169 | av2 = dc3*m3; 170 | av3 = m3 * dc3^2; 171 | av = [av1, av2, av3]; 172 | a = [a1, a2, a3]; 173 | 174 | Ma = subs(M, av, a); 175 | Ca = subs(C, av, a); 176 | Ga = subs(G, av, a); 177 | 178 | Y_parametrized = simplify(Ma * transpose([ddq1, ddq2, ddq3]) + Ca + Ga); 179 | Y = extract_Y_v3(Y_parametrized, a); 180 | 181 | disp('Dynamic Model parametrized with a:') 182 | disp(Y) 183 | 184 | % -------- EXERCISE 2 -------- 185 | % Done by formula 2.4.3 on Formulary 186 | 187 | % Define symbolic variables 188 | syms l1 l2 real 189 | syms q1(t) q2(t) 190 | syms q1_dot q2_dot real 191 | syms r_dot_dot real 192 | 193 | % Define the joint vector and joint velocity vector as functions of time 194 | q = [q1(t); q2(t)]; 195 | q_dot = [diff(q1(t), t); diff(q2(t), t)]; 196 | 197 | % Task: End-effector position in the x-direction 198 | r = l1*cos(q1(t)) + l2*cos(q1(t) + q2(t)); 199 | 200 | % Compute the task Jacobian matrix J_r(q) and J_r_dot(q, q_dot) 201 | J_r = jacobian(r, q); 202 | J_r_dot = diff(J_r, t); % time derivative of the Jacobian matrix J_r w.r.t. time 203 | 204 | % Compute h(q, q_dot) = J_r_dot * q_dot 205 | h_q_qdot = simplify(J_r_dot * q_dot); 206 | 207 | % Define the velocity gain matrix Kd 208 | Kd = diag([2, 2]); 209 | 210 | % Compute the pseudoinverse of the task Jacobian matrix 211 | J_r_pseudo = simplify(pinv(J_r)); 212 | 213 | % Compute the joint accelerations q_dot_dot 214 | q_dot_dot = J_r_pseudo * (r_dot_dot - h_q_qdot) + (eye(2) - J_r_pseudo * J_r) * (-Kd * q_dot); 215 | q_dot_dot = subs(q_dot_dot, [diff(q1(t), t), diff(q2(t), t)], [q1_dot, q2_dot]); 216 | 217 | % Substitute the given numerical values 218 | q_dot_dot_subs = subs(q_dot_dot, ... 219 | [l1, l2, q1(t), q2(t), q1_dot, q2_dot, r_dot_dot], ... 220 | [1, 1, pi/4, -pi/2, 1, -1, 1]); 221 | 222 | % Display the result 223 | disp('Joint Accelerations q_dot_dot:') 224 | disp(vpa(q_dot_dot_subs, 4)) 225 | -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/Exams/Exam_24_09_19.m: -------------------------------------------------------------------------------- 1 | % EXAM: 19/09/2024 2 | % Student: Scarano Gianmarco - 2047315 3 | 4 | % --------- EXERCISE 1 ---------- 5 | syms q1 q2 q1_dot q2_dot q1_dot_dot q2_dot_dot real 6 | syms m1 real 7 | syms m2 real 8 | syms dc1 real 9 | syms Ic1 real 10 | syms l1 l2 real 11 | syms g real 12 | 13 | n_joints = 2; 14 | q = [q1; q2]; 15 | qdots = [q1_dot; q2_dot]; 16 | qddots = [q1_dot_dot; q2_dot_dot]; 17 | 18 | % Computations for the Kinetic Energy 19 | % ------------------ T1 ------------------ 20 | % We must compute the position vector of the CoM of the first link 21 | px_joint1 = dc1 * cos(q1); 22 | py_joint1 = dc1 * sin(q1); 23 | 24 | % Derive the position vector of the CoM of the first link 25 | dpx_joint1 = diff(px_joint1, q1) * q1_dot; 26 | dpy_joint1 = diff(py_joint1, q1) * q1_dot; 27 | 28 | % ===> Linear velocity of the second joint 29 | velocity_joint1 = [dpx_joint1; dpy_joint1]; 30 | velocity_joint1 = simplify(velocity_joint1.' * velocity_joint1); 31 | 32 | % ===> Angular velocity of the second joint 33 | w1 = q1_dot; 34 | 35 | T1 = 0.5 * m1 * velocity_joint1 + 0.5 * w1.' * Ic1 * w1; 36 | 37 | % ------------------ T2 ------------------ 38 | % Since it is prismatic, we only need to compute the linear velocity of the second joint 39 | % We must compute the position vector of the CoM of the second link 40 | px_joint2 = l1 * cos(q1) + q2 * cos(q1); 41 | py_joint2 = l1 * sin(q1) + q2 * sin(q1); 42 | 43 | % Derive the position vector of the CoM of the second link 44 | dpx_joint2 = diff(px_joint2, q1) * q1_dot + diff(px_joint2, q2) * q2_dot; 45 | dpy_joint2 = diff(py_joint2, q1) * q1_dot + diff(py_joint2, q2) * q2_dot; 46 | 47 | % ===> Linear velocity of the second joint 48 | velocity_joint2 = [dpx_joint2; dpy_joint2]; 49 | velocity_joint2 = simplify(velocity_joint2.' * velocity_joint2); 50 | 51 | T2 = 0.5 * m2 * velocity_joint2; 52 | 53 | % Computation of T 54 | T = simplify(T1 + T2); 55 | 56 | % Computations for the Inertia Matrix 57 | M = compute_inertia_matrix(T, [q1_dot, q2_dot], 3); 58 | 59 | % Christoffel Matrix 60 | [C, c_sub] = compute_christoffel_matrix(M, [q1, q2], [q1_dot, q2_dot], 3); 61 | 62 | % Gravity Vector 63 | % In order to retrieve the gravity vector, we need to compute the potential energy 64 | % For the potential energy, we first need to retrieve the heights of the CoMs 65 | h1 = dc1 * sin(q1); 66 | h2 = l1 * sin(q1) + q2 * sin(q1); 67 | 68 | U1 = m1 * g * h1; 69 | U2 = m2 * g * h2; 70 | U = U1 + U2; 71 | 72 | G = transpose(simplify(jacobian(U, [q1, q2]))); 73 | G = collect(G, g); 74 | 75 | % Complete Dynamic Model | M(q) * ddq + C(q, dq) + G(q) = tau 76 | dynamic_model = simplify(M * transpose([q1_dot_dot, q2_dot_dot]) + C + G); 77 | 78 | % Try to expand G and C 79 | G = expand(G); 80 | C = expand(C); 81 | 82 | % Linear parametrization of the dynamic model 83 | syms a1 a2 a3 a4 a5 real 84 | av1 = m2; 85 | av2 = m1*dc1; 86 | av3 = l1*m2; 87 | av4 = 2 * l1^2; 88 | av5 = 4 * l1; 89 | av = [av1, av2, av3, av4, av5]; 90 | a = [a1, a2, a3, a4, a5]; 91 | 92 | Ma = subs(M, av, a); 93 | Ca = subs(C, av, a); 94 | Ga = subs(G, av, a); 95 | 96 | Y_parametrized = simplify(Ma * transpose([q1_dot_dot, q2_dot_dot]) + Ca + Ga); 97 | Y = extract_Y_v3(Y_parametrized, a); 98 | 99 | disp("Inertia Matrix:") 100 | disp(M) 101 | 102 | disp("Christoffel Matrix:") 103 | disp(C) 104 | 105 | disp("Gravity Vector:") 106 | disp(G) 107 | 108 | disp('Dynamic Model:') 109 | disp(dynamic_model) 110 | 111 | disp('Dynamic Model parametrized with a:') 112 | disp(Y) 113 | 114 | % --------- EXERCISE 2 ---------- 115 | syms a b k T real positive 116 | syms t real 117 | syms t_dot real 118 | syms m11 m12 m21 m22 real 119 | 120 | M_sym = [m11, m12; m21, m22]; 121 | q1_t = a + b * (1 - cos((pi*t)/T)); 122 | q2_t = k; 123 | 124 | q1_t_dot = time_derivate(q1_t, t, t_dot); 125 | q2_t_dot = time_derivate(q2_t, t, t_dot); 126 | 127 | m_multiplied = M_sym * [q1_t_dot; q2_t_dot]; 128 | m_multiplied = simplify(m_multiplied); 129 | m_multiplied = subs(m_multiplied, [m11, m21], [Ma(1, 1), Ma(2, 1)]); 130 | 131 | % --------- EXERCISE 4 ---------- 132 | syms Kd Kp X_eq X_e Y_eq Y_e Fd real positive 133 | 134 | eq = Kd*(X_eq - X_e) == Fd; 135 | eq1 = Kp*(Y_eq - Y_e) == 0; 136 | 137 | % Isolate now X_eq and Y_eq 138 | X_eq = solve(eq, X_eq); 139 | Y_eq = solve(eq1, Y_eq); 140 | disp('Equilibrium points:') 141 | disp(X_eq) 142 | disp(Y_eq) 143 | 144 | % --------- EXERCISE 5 ---------- 145 | % Define symbolic variables 146 | syms t a b k pi T real 147 | syms tau1max real 148 | 149 | % Define q1(t) 150 | q1_t = a + b * (1 - cos(pi * t / T)); 151 | q2_t = k; 152 | 153 | q1_dot_t = diff(q1_t, t); % First derivatives (qd_dot) 154 | q1_dot_dot_t = diff(q1_dot_t, t); % Second derivatives (qd_dot_dot) 155 | 156 | q2_dot_t = 0; % First derivatives (qd_dot) 157 | q2_dot_dot_t = 0; % Second derivatives (qd_dot_dot) 158 | 159 | % Substituting the trajectory into M, C, and G 160 | M_qd = subs(M, [q1, q2], [q1_t, q2_t]); 161 | C_qd = subs(C, [q1, q2, q1_dot, q2_dot], [q1_t, q2_t, q1_dot_t, q2_dot_t]); 162 | G_qd = subs(G, [q1, q2], [q1_t, q2_t]); 163 | 164 | % Substitute the second derivatives into the dynamic model 165 | tau_d = M_qd * [q1_dot_dot_t; q2_dot_dot_t] + C_qd * [q1_dot_t; q2_dot_t].' + G_qd; 166 | 167 | % Simplify the result 168 | tau_d = simplify(tau_d); 169 | 170 | % Extract tau_1 and tau_2 171 | tau_1_t = tau_d(1); 172 | tau_2_t = tau_d(2); 173 | disp('tau_1(t):') 174 | disp(tau_1_t) 175 | disp('tau_2(t):') 176 | disp(tau_2_t) -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/SNS.m: -------------------------------------------------------------------------------- 1 | function [new_values, old_values]= SNS(task, J_array, constraint_min, constraint_max, velocity_initial_condition) 2 | % This algorithm works for velocity and acceleration. 3 | % Change only equation to calculate desired task 4 | 5 | % PARAMETERS: 6 | % - Task: task to achieve 7 | % - J_array: array which contain the jacobian matrix. It is: 8 | % - {J} : For velocity 9 | % - {J, dJ} : For acceleration 10 | % - constrain_min/max: Array containing constraints for each joint 11 | % - velocity_initial_condition: Needed only for acceleration case, else you can put to 0 -> DEPRECATED 12 | 13 | % RETURNS: 14 | % - new_values: Array containing the new values for the joints 15 | % - old_values: Array containing the old values for the joints 16 | 17 | n_joint = length(constraint_min); 18 | task = reshape(task, length(task), 1); 19 | constraint_min = reshape(constraint_min, n_joint, 1); 20 | constraint_max = reshape(constraint_max, n_joint, 1); 21 | 22 | init_value = -999; 23 | new_values = linspace(init_value, init_value, n_joint); 24 | 25 | % Reshape the velocity_initial_condition to a column vector [num_joints x 1] 26 | if length(J_array) == 2 27 | velocity_initial_condition = reshape(velocity_initial_condition, n_joint, 1); 28 | end 29 | 30 | % Check if J_array is {J} or {J, dJ} 31 | % This means that we are working with velocity or acceleration respectively 32 | if isscalar(J_array) 33 | disp("---- SNS for velocity") 34 | J = J_array{1}; 35 | type_task = 'v'; 36 | elseif length(J_array) == 2 37 | disp("---- SNS for acceleration") 38 | J = J_array{1}; 39 | dJ = J_array{2}; 40 | type_task = 'a'; 41 | else 42 | disp("Error: J_array = {J, dJ} or J_array = {J}") 43 | return 44 | end 45 | 46 | % Check based on the type of task 47 | if type_task == 'v' 48 | old_values = pinv(J)*task; 49 | elseif type_task == 'a' 50 | old_values = pinv(J)*(task - dJ*velocity_initial_condition); 51 | end 52 | 53 | for i = 1:n_joint 54 | fprintf("==========> Loop number: %d", i) 55 | if type_task == 'v' 56 | result = pinv(J)*task; 57 | elseif type_task == 'a' 58 | result = pinv(J)*(task - dJ*velocity_initial_condition); 59 | end 60 | 61 | result = reshape(result, n_joint-i+1, 1); 62 | disp("Result: ") 63 | disp(result) 64 | 65 | % picking maximum violating constraint for max 66 | violating_max_logical = logical(result > constraint_max); 67 | violating_max_value = max(dot(result - constraint_max, violating_max_logical)); 68 | 69 | % picking maximum violating constraint for min 70 | violating_min_logical = logical(result < constraint_min); 71 | violating_min_value = min(dot(result - constraint_min, violating_min_logical)); 72 | 73 | % Print the violating constraints 74 | fprintf("Violating min: %d\n", violating_min_value) 75 | fprintf("Violating max: %d\n", violating_max_value) 76 | 77 | if all(~violating_min_logical) && all(~violating_max_logical) 78 | logic_subs = logical(new_values == init_value); 79 | 80 | counter_subs = 1; 81 | % Substitute the value in the new_values array 82 | for j = 1:length(new_values) 83 | if logic_subs(j) == 1 84 | new_values(j) = result(counter_subs); 85 | counter_subs = counter_subs + 1; 86 | end 87 | 88 | end 89 | 90 | % Reshape the new_values array to a column vector [num_joints x 1] 91 | new_values = reshape(new_values, n_joint, 1); 92 | break 93 | end 94 | 95 | % Check which constraint is violating the most 96 | if abs(violating_min_value) > abs(violating_max_value) 97 | violating_joint_index = find( (result-constraint_min) == violating_min_value ); 98 | violation_type = 'violating_min'; 99 | else 100 | violating_joint_index = find( (result-constraint_max) == violating_max_value ); 101 | violation_type = 'violating_max'; 102 | end 103 | 104 | fprintf("--- Joint %d exceeded bounds ==> %f\n", violating_joint_index, result(violating_joint_index)); 105 | 106 | if strcmp(violation_type, 'violating_min') 107 | new_values(violating_joint_index) = constraint_min(violating_joint_index); 108 | else 109 | new_values(violating_joint_index) = constraint_max(violating_joint_index); 110 | end 111 | fprintf("--- Joint %d now set at %f \n", violating_joint_index, new_values(violating_joint_index)) 112 | 113 | if type_task == 'v' 114 | selected_J = J(:, violating_joint_index); 115 | actual_velocity = new_values(violating_joint_index); 116 | task = task - selected_J*actual_velocity; 117 | 118 | elseif type_task == 'a' 119 | selected_J = J(:, violating_joint_index); 120 | 121 | actual_acceleration = new_values(violating_joint_index); 122 | 123 | task = task - selected_J*actual_acceleration; 124 | 125 | dJ(:,violating_joint_index) = []; 126 | velocity_initial_condition(violating_joint_index) = []; 127 | end 128 | 129 | % Empty all the values related to the violating joint 130 | J(:, violating_joint_index) = []; 131 | constraint_max(violating_joint_index) = []; 132 | constraint_min(violating_joint_index) = []; 133 | end 134 | end -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/christoffel_symbols.m: -------------------------------------------------------------------------------- 1 | function cell_c_k = christoffel_symbols(M, q) 2 | n = length(q); 3 | cell_c_k = cell(1, n); 4 | 5 | for i=1:n 6 | M_k = M(:, i); 7 | c_k = 0.5*(jacobian(M_k, q)+(jacobian(M_k, q)') - diff(M, q(i))); 8 | c_k = simplify(c_k); 9 | cell_c_k{i} = c_k; 10 | end 11 | end -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/compute_christoffel_matrix.m: -------------------------------------------------------------------------------- 1 | function [C, c_sub] = compute_christoffel_matrix(inertia_matrix, joint_pos, joint_vel, opt_steps) 2 | 3 | % inertia_matrix: expression of inertia matrix 4 | % joint_pos: variable of joint position q_i 5 | % ch_matrix_disp: can be true or false, it is used to display the 6 | % single christoffel matrix 7 | 8 | c_sub = {}; 9 | 10 | M = inertia_matrix; 11 | n_joint = length(joint_pos); 12 | C = sym(zeros(n_joint, 1)); 13 | joint_pos = reshape(joint_pos, n_joint, 1); 14 | joint_vel = reshape(joint_vel, n_joint, 1); 15 | 16 | for k = 1:n_joint 17 | joint = joint_pos(k); 18 | mk = M(:,k); % inertia matrix of joint k, its a columns 19 | 20 | %term1 = jacobian(mk,joint_pos); 21 | %term2 = jacobian(mk,joint_pos); 22 | %term3 = diff(M,joint); 23 | %ck = (1/2)*(term1 + transpose(term2) - term3 ); 24 | 25 | ck = (1/2)*(jacobian(mk,joint_pos)+transpose(jacobian(mk,joint_pos))-diff(M,joint)); 26 | ck = simplify(ck, Steps=opt_steps); 27 | c_sub{k} = ck; 28 | 29 | C(k) = simplify(transpose(joint_vel)*ck*joint_vel); 30 | end 31 | 32 | C = simplify(C, Steps=opt_steps); 33 | C = collect(C, joint_vel); 34 | end -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/compute_g.m: -------------------------------------------------------------------------------- 1 | function gq = compute_g(m,r0c,g,q,cartesian_dimension) 2 | % m -> column or row 3 | % g -> row 4 | % p -> each component r0_i as column 5 | n = length(m); 6 | U_vec = []; 7 | g_vec = []; 8 | 9 | for i=1:n 10 | U_i = simplify(-m(i)*g*r0c(1:cartesian_dimension, i)); 11 | fprintf("U_%d = ",i); 12 | disp(U_i); 13 | U_vec = [U_vec,U_i]; 14 | end 15 | 16 | U = collect(simplify(sum(U_vec)),[sin(q) cos(q)]); 17 | fprintf("U = sum(U_i) = "); 18 | disp(U) 19 | 20 | for i=1:n 21 | g_i = diff(U,q(i)); 22 | g_vec = [g_vec;g_i]; 23 | end 24 | gq = g_vec; 25 | end 26 | 27 | -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/compute_inertia_matrix.m: -------------------------------------------------------------------------------- 1 | function M = compute_inertia_matrix(kinetic_energy, joint_vel, opt_steps) 2 | 3 | % kinetic_energy: expression of kinetic exergy% kinetic_energy: expression of kinetic exergy 4 | % joint_vel: variable of joint velocity dq_i 5 | 6 | % element is computed as mij = dT/(dqi dqj), double derivative respect 7 | % to two joints 8 | 9 | T = kinetic_energy; 10 | n_joint = length(joint_vel); 11 | 12 | M = sym(zeros(n_joint, n_joint)); 13 | joint_vel = reshape(joint_vel, n_joint, 1); 14 | 15 | for i = 1:n_joint 16 | for j = 1:n_joint 17 | joint_i = joint_vel(i); 18 | joint_j = joint_vel(j); 19 | mij = diff(T, joint_i ); 20 | mij = diff(mij, joint_j); 21 | M(i,j) = simplify(mij); 22 | end 23 | end 24 | 25 | M = simplify(M, Steps=opt_steps); 26 | 27 | end -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/compute_potential_energy_matrix.m: -------------------------------------------------------------------------------- 1 | function [U, dU_T] = compute_potential_energy_matrix(masses, joint_pos, com_pos, gravity_term, opt_steps) 2 | 3 | % joint_pos: variable of joint position q_i 4 | % masses: variable of mass of links 5 | % com_pos: [x, y, z] ( also work with 2D ) 6 | % gravity term: [a, b, g] ( also work with 2D ) 7 | % com_pos and gravity term must have same shape 8 | 9 | n_joint = length(masses); 10 | U = sym(zeros(1, 1)); 11 | %gravity_term = reshape(gravity_term, length(gravity_term), 1); 12 | masses = reshape(masses, n_joint, 1); 13 | joint_pos = reshape(joint_pos, n_joint,1); 14 | 15 | 16 | for w = 1:n_joint 17 | 18 | joint_w_mass = masses(w); 19 | %com_pos_w = com_pos(:,w); 20 | com_pos_w = com_pos{w}; 21 | 22 | 23 | 24 | try 25 | %actual_u = joint_w_mass*transpose(gravity_term)*com_pos_w 26 | actual_u = joint_w_mass*transpose(gravity_term)*com_pos_w; 27 | fprintf("Potential energy of link %d: ", w) 28 | disp(actual_u) 29 | 30 | catch 31 | disp('Gravity term should be a row vector with shame same of com_i -> need to get a scalare g^T*com_i\n') 32 | end 33 | 34 | U = U + actual_u; 35 | 36 | end 37 | 38 | dU_T = transpose(jacobian(U, joint_pos)); 39 | dU_T = collect(dU_T, [joint_pos; gravity_term]); 40 | U = simplify(U, Steps=opt_steps); 41 | U = collect(U, [joint_pos; gravity_term]); 42 | 43 | size_dU_t = size(dU_T); 44 | 45 | if size_dU_t(1) > 1 & size_dU_t > 1 46 | disp("ERROR: vector of COM should contain be contain only z component!") 47 | end 48 | 49 | end -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/extract_Y_v3.m: -------------------------------------------------------------------------------- 1 | function Y = extract_Y_v3(Ya, dynamic_symbols) 2 | % M: inertia matrix 3 | % C: christoffel matrix 4 | % G: potential enrgy matrix 5 | % joint_acceleration: array with joint acceleration 6 | % this four items are use to build dynamic model 7 | 8 | % dynamic_model: in the form M*ddq + C + G 9 | % dynamic_coefficient: an array which contain symbol like [ m1+m3 I2 10 | % ...] 11 | % dynamic_symbols: how substitute coefficient, so [a1 a2 ... a_n] 12 | 13 | Y = sym([]); 14 | Y_columns = length(dynamic_symbols); 15 | dynamic_symbols = reshape(dynamic_symbols, 1, Y_columns); 16 | 17 | for i=1:Y_columns 18 | 19 | subs_array = zeros(1, Y_columns); 20 | subs_array(i) = 1; 21 | 22 | Yi = subs(Ya, dynamic_symbols, subs_array); 23 | 24 | Y = [Y, Yi]; 25 | end 26 | 27 | end 28 | 29 | 30 | -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/inertia_matrix_from_kinetic_energy.m: -------------------------------------------------------------------------------- 1 | function M = inertia_matrix_from_kinetic_energy(T, qdot_vector) 2 | % Takes as inputs: 3 | % - T = the kinetic energy of the robot 4 | % - qdot_vector = the vector of q_dot ex: [qd1,qd2,qd3] 5 | % 6 | % Output: 7 | % - M = the inertia matrix 8 | M = jacobian(jacobian(T, qdot_vector)', qdot_vector); 9 | end -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/inertia_matrix_to_coriolis.m: -------------------------------------------------------------------------------- 1 | function [c, C] = inertia_matrix_to_coriolis(M, q, dq) 2 | % Takes as inputs: 3 | % - M = the inertia matrix 4 | % - q = a vertical vector of q values 5 | % - dq = a vertical vector dot_q 6 | % Output: 7 | % - c = robot centrifugal and Coriolis term 8 | % - C = Christoffel matrices 9 | 10 | % Initialize variables 11 | n = length(q); % Number of joints 12 | C = cell(n, 1); % Initialize cell array to store Christoffel matrices 13 | 14 | % Loop over each joint 15 | for i = 1:n 16 | % Compute Christoffel matrix for the i-th joint 17 | disp(['Christoffel matrix for joint ', num2str(i)]) 18 | Mi = M(:, i); % Select the i-th column of the inertia matrix 19 | Ci = (1/2) * (jacobian(Mi, q) + jacobian(Mi, q)' - diff(M, q(i))); 20 | C{i} = Ci; % Store the Christoffel matrix 21 | 22 | % Display the Christoffel matrix 23 | disp(['C', num2str(i), ' = ']); 24 | disp(Ci); 25 | end 26 | 27 | % Compute robot centrifugal and Coriolis terms 28 | disp("Robot centrifugal and Coriolis terms") 29 | c = sym(zeros(n, 1)); 30 | for i = 1:n 31 | c(i) = dq' * C{i} * dq; 32 | disp(['c', num2str(i), ' = ']); 33 | disp(c(i)); 34 | end 35 | end 36 | -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/moving_frames_algorithm.m: -------------------------------------------------------------------------------- 1 | function [w, v, vc, T] = moving_frames_algorithm(num_joints, DH, qdots, m, rc, prismatic_indices, I) 2 | % This function performs the moving frame algorithm and returns the 3 | % vectors w, v, vc, and the values of T for each joint 4 | % 5 | % Inputs: 6 | % - num_joints = An integer representing the number of links of the robot 7 | % - DH = The DH matrix of the robot in the order [alpha, a, d, theta] 8 | % - qdots = A vector containing qdots ex: [qd1; qd2; qd3] 9 | % - m = a vector of masses [m1 ; ... ; mn] 10 | % - rc = a vector containing the values ^ir_{ci} that is the distance 11 | % between the i-th CoM from the i-th reference frame 12 | % - prismatic_indices = A list containing the list of prismatic indices, ex for a PPR = [1,2] 13 | % - I = a vector of 3 by 3 matrices [I1, .., In] which are the baricentric inertia matrices 14 | % 15 | % Outputs: 16 | % - w = The angular velocity vector for each joint 17 | % - v = The linear velocity vector for each joint 18 | % - vc = The velocity of the center of mass for each joint 19 | % - T = The kinetic energy for each joint 20 | 21 | % Check that the DH is consistent with the indicated number of joints 22 | if size(DH, 1) ~= num_joints 23 | error('Number of rows in DH matrix should be equal to the number of joints.'); 24 | end 25 | 26 | % Check that rc is consistent with the number of joints 27 | if size(rc,2) ~= num_joints 28 | error('Length of rc vector should be equal to the number of joints.'); 29 | end 30 | 31 | disp("---- Starting the Moving Frames Algorithm ----"); 32 | 33 | % Display the number of joints and prismatic indices 34 | disp(['Number of joints: ', num2str(num_joints)]); 35 | 36 | %START THE ALGORITHM: 37 | v00 = [0; 0; 0]; 38 | w00 = [0; 0; 0]; 39 | 40 | % Initialize outputs 41 | w = zeros(3, num_joints); 42 | v = zeros(3, num_joints); 43 | vc = zeros(3, num_joints); 44 | T = cell(num_joints); 45 | 46 | % Start the loop 47 | for i = 1:num_joints 48 | In=I(1:3, (i-1)*3+1:i*3); 49 | 50 | % Computing sigma for prismatic joints 51 | if ismember(i, prismatic_indices) 52 | sigma = 1; 53 | else 54 | sigma = 0; 55 | end 56 | 57 | % Computing A, R, and p 58 | A = DHMatrix_M(DH(i,:)); 59 | R = A(1:3,1:3); 60 | p = A(1:3,4); 61 | 62 | r_i = R' * p; 63 | 64 | % Computing omega 65 | if i == 1 66 | w_prev = w00; 67 | else 68 | w_prev = wi; 69 | end 70 | 71 | % Computing w_i 72 | wi = R' * (w_prev + (1 - sigma) * [0; 0; qdots(i)]); 73 | fprintf('The value of w_%d is:', i); 74 | fprintf('\n'); 75 | disp(wi); 76 | 77 | % Computing v 78 | if i == 1 79 | v_prev = v00; 80 | else 81 | v_prev = vi; 82 | end 83 | 84 | % Computing v_i 85 | vi = R' * (v_prev + sigma * [0; 0; qdots(i)]) + cross(wi, r_i, 1); 86 | fprintf('The value of v_%d is:', i); 87 | fprintf('\n'); 88 | disp(vi); 89 | 90 | % Computing vc_i 91 | vci = vi + cross(wi, rc(:, i)); 92 | fprintf('The value of vc_%d is:', i); 93 | fprintf('\n'); 94 | disp(vci); 95 | 96 | % Computing T_i 97 | Ti = simplify(0.5 * m(i) * (vci' * vci) + 0.5 * wi' * In * wi); 98 | fprintf('The value of T_%d is:', i); 99 | fprintf('\n'); 100 | disp(Ti) 101 | T{i} = Ti; 102 | end -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/newton_euler_algorithm.m: -------------------------------------------------------------------------------- 1 | function u_vector = newton_euler_algorithm(num_joints, DH, qd, qdd, m, I, r, rc, g0, prismatic_indices) 2 | % This function performs the moving frame algorithm and returns the 3 | % vectors w, v, vc, and the values of T for each joint 4 | % 5 | %inputs: 6 | % num_joints= an int reapresenting the number of links o fthe robot 7 | % 8 | % DH = the DH matrix of the robot in the order [alpha, a, d, theta ] 9 | % 10 | % qd = a vector containing qdots ex: [qd1; qd2; qd3] 11 | % 12 | % qdd = a vector containing qddots ex: [qdd1; qdd2; qdd3] 13 | % 14 | % m = a vector containing the masses of the links ex: [m1; m2; m3] 15 | % 16 | % I = a vector of kind [I1,I2..] 17 | % 18 | % r = a vector containing the values ^ir_{i-1,i} that is the distance 19 | % between frame i-1 and i computed with respect to frame i 20 | % 21 | % rc = a vector containing the values ^ir_{ci} that is the distance 22 | % between the i-th CoM from the i-th reference frame 23 | % 24 | % g0 = a vector representing g wrt fame 0 25 | % 26 | % prismatic indices= a list containing the list of prismatic indices, 27 | % ex for a PPR= [1,2] 28 | % 29 | %output: 30 | 31 | syms Ixx Iyy Izz real 32 | 33 | 34 | % Check that the DH is consistent with the indicated number of joints 35 | if size(DH, 1) ~= num_joints 36 | error('Number of rows in DH matrix should be equal to the number of joints.'); 37 | end 38 | 39 | % Check that the r is consistent with the number of joints 40 | if size(r,2) ~= num_joints 41 | size(r,2) 42 | error('Length of r vector should be equal to the number of joints.'); 43 | end 44 | 45 | % Check that rc is consistent with the number of joints 46 | if size(rc,2) ~= num_joints 47 | error('Length of rc vector should be equal to the number of joints.'); 48 | end 49 | 50 | % Display the number of joints and prismatic indices 51 | disp(['Number of joints: ', num2str(num_joints)]); 52 | 53 | rotations = sym(zeros(3,3, num_joints)); 54 | rotations_from_zero = sym(zeros(3,3, num_joints)); 55 | i_g=sym(zeros(3,num_joints)) 56 | 57 | for i=1:num_joints 58 | A=DHMatrix(DH(i,:)); 59 | R=A(1:3,1:3); 60 | rotations(:,:,i)=R; 61 | A=DHMatrix(DH(1:i,:)); 62 | R=A(1:3,1:3); 63 | rotations_from_zero(:,:,i)=R; 64 | i_g(:,i)=rotations_from_zero(:,:,i)'*g0; 65 | end 66 | 67 | %forward pass 68 | % -------- initialization ----------------- 69 | w_prev=[0;0;0]; 70 | w_dot_prev=[0;0;0]; 71 | a_prev=[0;0;0]; 72 | 73 | w_vector=zeros(3,num_joints); 74 | w_dot_vector=zeros(3,num_joints); 75 | a_vector=zeros(3,num_joints); 76 | a_c_vector=zeros(3,num_joints); 77 | 78 | % --------- starting forward pass ------------- 79 | for i=1:num_joints 80 | 81 | % This equation represents the angular velocity of the 𝑖-th link as a 82 | % function of the previous link's angular velocity and the current joint velocity 𝑞𝑑(𝑖). 83 | % In the context of the Jacobian, this is equivalent to computing the rotational part of 84 | % the Jacobian at each joint. The term [0;0;1] corresponds to the unit vector in the direction 85 | % of rotation for a revolute joint (if it's prismatic, it would be different). 86 | w=rotations(:,:,i)'*(w_prev+qd(i)*[0;0;1]); 87 | w=vpa(simplify(w)) 88 | fprintf("the w of link %d is:",i); 89 | disp(w) 90 | w_vector(:,i)=w; 91 | 92 | % This can also be seen as the derivative of the angular velocity "w" with respect to time. 93 | w_dot=rotations(:,:,i)'*(w_dot_prev+qdd(i)*[0;0;1]+cross(qd(i)*w_prev,[0;0;1])); 94 | fprintf("the w_dot of link %d is:",i); 95 | disp(w_dot) 96 | w_dot_vector(:,i)=w_dot; 97 | 98 | a=rotations(:,:,i)'*a_prev+cross(w_dot,r(:,i))+cross(w,r(:,i)) 99 | a=vpa(simplify(a)) 100 | fprintf("the a of link %d is:",i); 101 | disp(a); 102 | a_vector(:,i)=a; 103 | 104 | a_c=a+cross(w_dot,r(:,i))+cross(w,cross(w,r(:,i))) 105 | fprintf("the a_c of link %d is:",i); 106 | disp(a_c); 107 | a_c_vector(:,i)=a_c; 108 | 109 | %update the prev 110 | w_prev=w; 111 | W_dot_prev=w_dot; 112 | a_prev=a; 113 | end 114 | 115 | %starting backward pass 116 | %---------------- initialization -------------------- 117 | f_next=[0;0;0]; 118 | tau_next=[0;0;0]; 119 | 120 | f_vector=sym(zeros(3,num_joints)); 121 | tau_vector=sym(zeros(3,num_joints)); 122 | 123 | %--------------- starting backward pass -------------- 124 | for i=num_joints:-1:1 125 | if i == num_joints 126 | f=m(i)*(a_c_vector(:,i)-i_g(:,i)); 127 | else 128 | f=rotations(:,:,i+1)*f_next+m(i)*(a_c_vector(:,i)-i_g(:,i)); 129 | end 130 | f=vpa(simplify(f)); 131 | fprintf("the f of link %d is:",i); 132 | disp(f); 133 | f_vector(:,i)=f; 134 | 135 | if i ==num_joints 136 | tau=-cross(f,(r(:,i)+rc(:,i)))+I(i)*w_dot_vector(:,i)+cross(w_vector(:,i),(I(i)*w_vector(:,i))); 137 | else 138 | tau=rotations(:,:,i+1)*tau_next+ cross((rotations(:,:,i+1)*f_next),rc(:,i))-cross(f,(r(:,i)+rc(:,i)))+I(i)*w_dot_vector(:,i)+cross(w_vector(:,i),(I(i)*w_vector(:,i))); 139 | 140 | end 141 | tau=vpa(simplify(tau)); 142 | fprintf("the tau of link %d is:",i); 143 | disp(tau); 144 | tau_vector(:,i)=tau; 145 | 146 | f_next=f; 147 | tau_next=tau; 148 | end 149 | 150 | u_vector=sym(zeros(3,num_joints)) 151 | for i=1:num_joints 152 | if ismember(i, prismatic_indices) 153 | ui=f_vector(:,i)'*(rotations(:,:,i)'*[0;0;1]); 154 | else 155 | ui=tau_vector(:,i)'*(rotations(:,:,i)'*[0;0;1]); 156 | end 157 | ui=vpa(simplify(ui)); 158 | fprintf("u of link %d is:",i); 159 | disp(ui) 160 | u_vector(:,i)=ui; 161 | end 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/test.m: -------------------------------------------------------------------------------- 1 | syms q1 q2 q3 2 | syms a2 a3 3 | syms theta1 theta2 theta3 4 | syms dc1 5 | 6 | % alpha, a_i, d_i, theta_i 7 | syms alpha a d theta 8 | DH_table = [ pi/2 0 0 theta1; 9 | 0 a2 0 theta2; 10 | 0 a3 0 theta3; 11 | ]; 12 | 13 | DH = DHmatrix(alpha, a, d, theta); 14 | 15 | A = cell(1,n_joints); 16 | 17 | % Compute each transformation matrix 18 | % -- Remember: 19 | % A{1} = 0^A_1 20 | % A{2} = 1^A_2 21 | % A{3} = 2^A_3 22 | % A{4} = 3^A_4 23 | % ... 24 | % A{i} = i-1^A_i 25 | for i = 1: n_joints 26 | A{i} = subs(DH, {alpha, a, d, theta}, DH_table(i, :)); 27 | end 28 | 29 | T = eye(4); 30 | 31 | disp("-------------------------------------------------------------------") 32 | for i = 1 : n_joints 33 | % Display print statement 34 | disp(['0^A_' num2str(i) '(q_' num2str(i) '):']); 35 | 36 | % Perform the simplify operation 37 | T = simplify(T * A{i}); 38 | 39 | % Display the simplified result 40 | disp(T); 41 | end 42 | 43 | % v = alpha, a d, theta 44 | A01 = A{1}; 45 | A12 = A{2}; 46 | A23 = A{3}; 47 | 48 | R01 = A01(1:3, 1:3); 49 | R12 = A12(1:3, 1:3); 50 | R23 = A23(1:3, 1:3); 51 | 52 | t1 = A01(1:3, 4); 53 | t2 = A12(1:3, 4); 54 | t3 = A23(1:3, 4); 55 | 56 | t11 = simplify(inv(R01)*t1); 57 | t22 = simplify(inv(R12)*t2); 58 | t33 = simplify(inv(R23)*t3); -------------------------------------------------------------------------------- /Scripts/2024/Gianmarco/time_derivate.m: -------------------------------------------------------------------------------- 1 | function f_dt = time_derivate(f, var, d_var) 2 | f_dt = 0; 3 | 4 | for i = 1:length(var) 5 | current_var = var(i); 6 | d_current_var = d_var(i); 7 | 8 | f_dt = f_dt + diff(f, current_var)*d_current_var; 9 | end 10 | end -------------------------------------------------------------------------------- /Scripts/2024/Martina/DHMatrix.m: -------------------------------------------------------------------------------- 1 | function [T, A] = DHMatrix(DHTABLE) 2 | % Function that takes the DHTABLE and returns T 3 | % 4 | % parameters: 5 | % - DHTABLE: a n-vector of vectors in the order: [alpha a d theta] 6 | % and outputs: 7 | % - T: the product of all the matrices corresponding to each vector of 8 | % the DHTABLE 9 | % - A: a cell array containing all the matrices corresponding to each vector of the DHTABLE 10 | % 11 | % example 12 | % 13 | T = eye(4); 14 | nums = size(DHTABLE); 15 | 16 | A = cell(1,nums(1)); 17 | 18 | for i = 1:nums(1) 19 | line = DHTABLE(i, :); 20 | R = [cos(line(4)) -cos(line(1))*sin(line(4)) sin(line(1))*sin(line(4)) line(2)*cos(line(4)); 21 | sin(line(4)) cos(line(1))*cos(line(4)) -sin(line(1))*cos(line(4)) line(2)*sin(line(4)); 22 | 0 sin(line(1)) cos(line(1)) line(3); 23 | 0 0 0 1;]; 24 | A{i} = R; 25 | T = T * R; 26 | end 27 | 28 | if isa(T, 'sym') 29 | T = simplify(T); 30 | end 31 | end -------------------------------------------------------------------------------- /Scripts/2024/Martina/Exam_2016_3.m: -------------------------------------------------------------------------------- 1 | clear all 2 | clc 3 | syms Ixx Iyy Izz q1 q2 q3 q4 g1 g2 qd1 qd2 qd3 qd4 real 4 | syms l1 l2 l3 l4 dc1 dc2 dc3 dc4 m1 m2 m3 m4 vx vy real 5 | syms a1 a2 a3 a4 a5 a6 th thd m I rc1x rc2x rc1y rc2y I3 I4 real 6 | syms qdd1 qdd2 qdd3 qdd4 deltaq T l pdx pdy I1 I2 I3 d k tau1 tau2 real 7 | syms Iyy1 Ixx2 Iyy2 Izz2 rdot real 8 | digits(6) 9 | %% 10 | p=[cos(q1+q2+q3)+cos(q1+q2)+cos(q1); 11 | sin(q1+q2+q3)+sin(q1+q2)+sin(q1)] 12 | j=jacobian(p,[q1,q2,q3]) 13 | j=subs(j,[q1,q2,q3],[pi/6,pi/6,pi/6]) 14 | qd=[0;0;0] 15 | jdot=diff(j,q1)*qd1+diff(j,q2)*qd2+diff(j,q3)*qd3 16 | pddot=[4;2] 17 | qdot=simplify(pinv(j)*(pddot-jdot*qd)) 18 | q=vpa(subs(qdot,[q1,q2,q3],[pi/6,pi/6,pi/6])) 19 | pddotnew=pddot-j(:,3)*-4 20 | jnew=j(:,1:2) 21 | qddnew=vpa(subs(pinv(jnew)*(pddotnew-jdot(:,1:2)*qd(1:2)),[q1,q2,q3],[pi/6,pi/6,pi/6])) -------------------------------------------------------------------------------- /Scripts/2024/Martina/Exam_2016_3.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2024/Martina/Exam_2016_3.mlx -------------------------------------------------------------------------------- /Scripts/2024/Martina/Exam_2018_n_1.m: -------------------------------------------------------------------------------- 1 | clear all 2 | clc 3 | syms Ixx Iyy Izz q1 q2 q3 q4 g1 g2 qd1 qd2 qd3 qd4 l1 l2 l3 l4 dc1 dc2 dc3 dc4 m1 m2 m3 m4 real 4 | digits(6) 5 | %% 6 | r=[[0;q1;0],[0;q2;0],[l3;0;0],[l4;0;0]] 7 | rc=[[0;-dc1;0],[0;-dc2;0],[-l3+dc3;0;0],[-l4+dc4;0;0]] 8 | DH=[pi/2,0,q1,pi/2;pi/2,0,q2,pi/2; 9 | 0,l3,0,q3; 10 | 0,l4,0,q4] 11 | [w,v,vc,T]=moving_frames_algorithm(4,DH,[qd1;qd2;qd3;qd4],[m1;m2;m3;m4],r,rc,[1,2]) 12 | T_tot=simplify(T{1}+T{2}+T{3}+T{4}) 13 | T=(m4*((dc4*qd3 + dc4*qd4 + qd2*cos(q3 + q4) - qd1*sin(q3 + q4) + l3*qd3*cos(q4))^2 + (sin(q4)*(l3*qd3 + qd2*cos(q3) - qd1*sin(q3)) + cos(q4)*(qd1*cos(q3) + qd2*sin(q3)))^2))/2 + (Izz*qd3^2)/2 + (m3*((qd1*cos(q3) + qd2*sin(q3))^2 + (dc3*qd3 + qd2*cos(q3) - qd1*sin(q3))^2))/2 + (m1*qd1^2)/2 + (m2*(qd1^2 + qd2^2))/2 + Izz*(qd3/2 + qd4/2)*(qd3 + qd4) 14 | M=simplify(jacobian(jacobian(T,[qd1,qd2,qd3,qd4]),[qd1,qd2,qd3,qd4])) -------------------------------------------------------------------------------- /Scripts/2024/Martina/Exam_2018_n_1.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2024/Martina/Exam_2018_n_1.mlx -------------------------------------------------------------------------------- /Scripts/2024/Martina/Exam_Midterm_Exercise-2.m: -------------------------------------------------------------------------------- 1 | clear all 2 | clc 3 | syms Ixx Iyy Izz q1 q2 q3 q4 g1 g2 qd1 qd2 qd3 qd4 real 4 | syms l1 l2 l3 l4 dc1 dc2 dc3 dc4 m1 m2 m3 m4 vx vy real 5 | syms qdd1 qdd2 qdd3 qdd4 deltaq T l pdx pdy I1 I2 I3 d k tau1 tau2 real 6 | syms rc1x rc1y rc1z rc2x real 7 | syms a1 a2 a3 a4 a5 real; 8 | syms I1xx I1xy I1xz I1yx I1yy I1yz I1zx I1zy I1zz I2xx I2yy I2zz real 9 | digits(4) 10 | %% 11 | DH=[-pi/2,0,l1,q1; 12 | 0,l2,0,q2] 13 | r=[[0;-l1;0],[l2;0;0]] 14 | rc=[[rc1x;rc1y;rc1z],[rc2x;0;0]] 15 | m=[m1;m2] 16 | qd=[qd1;qd2] 17 | I1=[I1xx,I1xy,I1xz; 18 | I1yx,I1yy,I1yz; 19 | I1zx,I1zy,I1zz] 20 | I2=[I2xx,0,0; 21 | 0,I2yy,0; 22 | 0,0,I2zz] 23 | I=[I1,I2] 24 | [w,v,vc,T]=moving_frames_algorithm(2,DH,qd,m,r,rc,[],I) 25 | %% 26 | T=T{1}+T{2} 27 | M=simplify(inertia_matrix_from_kinetic_energy(T,qd)) 28 | %% 29 | 30 | M=[a1+a2*cos(q2)^2+a3*sin(q2)^2,0; 31 | 0,a4] 32 | [c,C]=inertia_matrix_to_coriolis(M,[q1,q2],[qd1;qd2]) 33 | U1=m1*[-9.81;0;0]'*[rc1x;rc1y;rc1z] 34 | U2=m2*([cos(q2),-sin(q2),0; sin(q2),cos(q2),0;0,0,1]'*[-9.81;0;0])'*[rc2x;0;0] 35 | U=U1+U2 36 | g=[diff(U,q1);diff(U,q2)] 37 | %% 38 | S1=qd'*C{1} 39 | S2=qd'*C{2} 40 | S=[S1;S2] 41 | Mdot=diff(M,q1)*qd1+diff(M,q2)*qd2 42 | skewsymm=Mdot-2*S 43 | g=[0;a5*sin(q2)] 44 | tau=M*[qdd1;qdd2]+c+g 45 | Y=regressor_matrix(tau,[a1;a2;a3;a4;a5]) 46 | syms qdd1 qdd2 t real 47 | tau=subs(tau,[q1,q2,qd1,qd2,qdd1,qdd2],[2*t,pi/4,2,0,0,0]) -------------------------------------------------------------------------------- /Scripts/2024/Martina/Exam_Midterm_Exercise-2.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2024/Martina/Exam_Midterm_Exercise-2.mlx -------------------------------------------------------------------------------- /Scripts/2024/Martina/Homework_2_NE_Proof.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | clc; 3 | 4 | syms q1 q2 q3 l1 l2 l3 m1 m2 m3 dc1 dc2 dc3 real 5 | digits(4); 6 | 7 | g0 = 9.81; 8 | g = [g0; 0; 0]; 9 | rc1 = [-l1+dc1;0;0]; 10 | rc2 = [-l2+dc2;0;0]; 11 | rc3 = [-l3+dc3;0;0]; 12 | 13 | DH = [0 l1 0 q1; 0 l2 0 q2; 0 l3 0 q3] 14 | 15 | zero_A1 = DHMatrix(DH(1,:)); 16 | zero_R1 = zero_A1(1:3,1:3); 17 | zero_A2 = DHMatrix(DH(1:2,:)); 18 | zero_R2 = zero_A2(1:3,1:3); 19 | zero_A3 = DHMatrix(DH); 20 | zero_R3 = zero_A3(1:3,1:3); 21 | 22 | one_A2 = DHMatrix(DH(2,:)); 23 | one_R2 = one_A2(1:3,1:3); 24 | two_A3 = DHMatrix(DH(3,:)); 25 | two_R3 = two_A3(1:3,1:3); 26 | 27 | % Forces f3, f2, f1 28 | f3 = vpa(simplify(-m3*zero_R3.'*g)) 29 | f2 = vpa(simplify(two_R3*f3 -m2*zero_R2.'*g)) 30 | f1 = vpa(simplify(one_R2*f2 -m1*zero_R1.'*g)) 31 | 32 | % Tau3 33 | three_r23 = [l3;0;0]; 34 | tau3 = vpa(simplify(cross(-f3,(three_r23+rc3)))) 35 | 36 | % Tau2 37 | two_r12 = [l2;0;0]; 38 | first_tau2 = vpa(simplify(two_R3*tau3)); 39 | second_tau2 = vpa(simplify(cross(two_R3*f3,rc2))); 40 | third_tau2 = vpa(simplify(cross(-f2,(two_r12+rc2)))); 41 | tau2 = vpa(simplify(first_tau2+second_tau2+third_tau2)) 42 | 43 | % Tau1 44 | one_r01 = [l1;0;0]; 45 | first_tau1 = vpa(simplify(one_R2*tau2)); 46 | second_tau1 = vpa(simplify(cross(one_R2*f2,rc1))); 47 | third_tau1 = vpa(simplify(-cross(f1,(one_r01+rc1)))); 48 | tau1 = vpa(simplify(first_tau1 + second_tau1 + third_tau1)) 49 | 50 | % u3 51 | three_z2 = vpa(simplify(two_R3.'*[0;0;1])); 52 | u3 = vpa(simplify(tau3.'*three_z2)) 53 | 54 | % u2 55 | two_z1 = vpa(simplify(one_R2.'*[0;0;1])); 56 | u2 = vpa(simplify(tau2.'*two_z1)) 57 | 58 | % u1 59 | one_z0 = vpa(simplify(zero_R1.'*[0;0;1])); 60 | u1 = vpa(simplify(tau1.'*one_z0)) -------------------------------------------------------------------------------- /Scripts/2024/Martina/Homework_2_NE_Proof.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2024/Martina/Homework_2_NE_Proof.mlx -------------------------------------------------------------------------------- /Scripts/2024/Martina/bound_of_g.m: -------------------------------------------------------------------------------- 1 | function alpha = bound_of_g(g,q) 2 | %Function that outputs the upper bound alpha 3 | % on g 4 | % 5 | %input: 6 | %- g = the gravity vector 7 | %- q = a vertical vector q [q1;q2;q3] 8 | % 9 | %output: the value of alpha 10 | 11 | dgdq=jacobian(g,q'); 12 | m=simplify(dgdq'*dgdq) 13 | eigenvalues=eig(m) 14 | display('maximum eigenvalue:') 15 | max_=simplify(max(eigenvalues)) 16 | alpha=simplify(sqrt(max_)) 17 | 18 | 19 | -------------------------------------------------------------------------------- /Scripts/2024/Martina/find_singularity.m: -------------------------------------------------------------------------------- 1 | function [solutions] = find_singularity(J, variables) 2 | %Function that computes the singularity of ajacobian matrix square or 3 | %not 4 | % 5 | %parameters: 6 | %J= jacobian 7 | %variables= vector of variables of the jacobians 8 | [m, n] = size(J); 9 | if m ~= n % If m not equal to n (joints) | If m == n, then it's square. 10 | if m < n 11 | for i = 1:n 12 | % Create a submatrix of the original non-square matrix by 13 | % removing one column at a time 14 | Jcopy=J; 15 | Jcopy(:,i)=[]; 16 | 17 | % Determinant of the i-th submatrix 18 | fprintf('Determinant of the %s -th submatrix: \n', mat2str(i)); 19 | disp(simplify(det(Jcopy))); 20 | 21 | % Compute the singularity (equal the determinant to zero) 22 | fprintf('singularity condition of the %s -th submatrix: \n', mat2str(i)); 23 | disp(simplify(det(Jcopy)==0)); 24 | end 25 | else 26 | %create all submatrix generated from removing any m-n rows 27 | 28 | %case of Jacobian 6*4 29 | if m==6 & n==4 30 | %create the sumatrices by creating all the 4by4 31 | %submatrices 32 | k=0; 33 | for i = 1:m 34 | % Create a submatrix of the original non-square matrix by 35 | % removing one column at a time 36 | Jcopy=J; 37 | Jcopy(i,:)=[]; 38 | for j=1:m-1 39 | k=k+1; 40 | Jcopy2=Jcopy; 41 | Jcopy2(j,:)=[]; 42 | % Determinant of the i-th submatrix 43 | fprintf('Determinant of the %s -th submatrix: \n', mat2str(k)); 44 | disp(simplify(det(Jcopy2))); 45 | end 46 | end 47 | else 48 | disp('The jacobian you have parsed has more rows than columns and its not a geometric jacobian') 49 | % Compute the determinant 50 | determinant_Jacobian = simplify(det(J.' * J)); 51 | 52 | disp('Determinant of the Jacobian:'); 53 | disp(simplify(determinant_Jacobian)); 54 | 55 | disp("You might want to check when the Jacobian loses full rank.") 56 | disp("For doing so, you have to solve the Determinant above for the values which make it == 0.") 57 | disp("Example: solve(simplify(det(J.' * J)) == 0, variables);") 58 | end 59 | end 60 | else 61 | % Compute the determinant 62 | determinant_Jacobian = simplify(det(J)); 63 | 64 | disp('Determinant of the Jacobian:'); 65 | disp(simplify(determinant_Jacobian)); 66 | 67 | n_var=length(variables) 68 | % Solve for all possible values of the symbols that make the determinant zero 69 | if n_var==2 70 | [q1_sol,q2_sol,~, ~] = solve(determinant_Jacobian == 0, variables, 'ReturnConditions', true); 71 | solutions=[q1_sol,q2_sol]; 72 | elseif n_var==3 73 | [q1_sol,q2_sol,q3_sol,~, ~] = solve(determinant_Jacobian == 0, variables, 'ReturnConditions', true); 74 | solutions=[q1_sol,q2_sol,q3_sol]; 75 | elseif n_var==4 76 | [q1_sol,q2_sol,q3_sol,q4_sol,~, ~] = solve(determinant_Jacobian == 0, variables, 'ReturnConditions', true); 77 | solutions=[q1_sol,q2_sol,q3_sol,q4_sol]; 78 | end 79 | disp('All possible solutions for the symbols that make the determinant zero:'); 80 | display(solutions); 81 | end 82 | end -------------------------------------------------------------------------------- /Scripts/2024/Martina/g_from_potential_energy.m: -------------------------------------------------------------------------------- 1 | function g = g_from_potential_energy(U,q) 2 | %Function that outputs the gravity vector from the 3 | %potential energy 4 | % 5 | %input: 6 | %- U = th potential energy (the sum of the potential energy of the single 7 | %links) 8 | %- q = a vertical vector q [q1;q2;q3] 9 | 10 | g=vpa(simplify(jacobian(U,q)))' -------------------------------------------------------------------------------- /Scripts/2024/Martina/inertia_matrix_from_kinetic_energy.m: -------------------------------------------------------------------------------- 1 | function M = inertia_matrix_from_kinetic_energy(T, qdot_vector) 2 | % Takes as inputs: 3 | % - T = the kinetic energy of the robot 4 | % - qdot_vector = the vector of q_dot ex: [qd1,qd2,qd3] 5 | % 6 | % Output: 7 | % - M = the inertia matrix 8 | M = jacobian(jacobian(T, qdot_vector)', qdot_vector); 9 | end -------------------------------------------------------------------------------- /Scripts/2024/Martina/inertia_matrix_to_coriolis.m: -------------------------------------------------------------------------------- 1 | function [c, C] = inertia_matrix_to_coriolis(M, q, dq) 2 | % Takes as inputs: 3 | % - M = the inertia matrix 4 | % - q = a vertical vector of q values 5 | % - dq = a vertical vector dot_q 6 | % Output: 7 | % - c = robot centrifugal and Coriolis term 8 | % - C = Christoffel matrices 9 | 10 | % Initialize variables 11 | n = length(q); % Number of joints 12 | C = cell(n, 1); % Initialize cell array to store Christoffel matrices 13 | 14 | % Loop over each joint 15 | for i = 1:n 16 | % Compute Christoffel matrix for the i-th joint 17 | disp(['Christoffel matrix for joint ', num2str(i)]) 18 | Mi = M(:, i); % Select the i-th column of the inertia matrix 19 | Ci = (1/2) * (jacobian(Mi, q) + jacobian(Mi, q)' - diff(M, q(i))); 20 | C{i} = Ci; % Store the Christoffel matrix 21 | 22 | % Display the Christoffel matrix 23 | disp(['C', num2str(i), ' = ']); 24 | disp(Ci); 25 | end 26 | 27 | % Compute robot centrifugal and Coriolis terms 28 | disp("Robot centrifugal and Coriolis terms") 29 | c = sym(zeros(n, 1)); 30 | for i = 1:n 31 | c(i) = dq' * C{i} * dq; 32 | disp(['c', num2str(i), ' = ']); 33 | disp(c(i)); 34 | end 35 | end 36 | -------------------------------------------------------------------------------- /Scripts/2024/Martina/is_inertia_matrix.m: -------------------------------------------------------------------------------- 1 | function is_an_inertia_matrix = is_inertia_matrix(matrix) 2 | 3 | % Check if the matrix is symmetric 4 | if ~isequal(matrix, matrix') 5 | error('Matrix is not symmetric'); 6 | end 7 | 8 | % Check if the matrix is positive definite 9 | % Check determinant 10 | if isa(matrix, 'sym') 11 | warning('The matrix is symbolic. Please check manually that the determinant is not negative.'); 12 | disp("determinant:") 13 | disp(det(matrix)) 14 | elseif det(matrix) <= 0 15 | is_an_inertia_matrix = false; 16 | disp("this matrix is not an inertia matrix since the determinant is negative") 17 | return; 18 | end 19 | 20 | % Compute eigenvalues 21 | eigenvalues = eig(matrix); 22 | 23 | % Check if all eigenvalues are positive 24 | if isa(matrix, 'sym') 25 | warning('The matrix is symbolic. Please check manually that the eigenvalues are all positive.'); 26 | disp("eigenvalues:") 27 | disp(eigenvalues) 28 | is_an_inertia_matrix = []; 29 | elseif all(eigenvalues > 0) 30 | is_an_inertia_matrix = true; 31 | else 32 | is_an_inertia_matrix = false; 33 | disp("this matrix is not an inertia matrix since some eigenvalues are negative") 34 | end 35 | 36 | if is_an_inertia_matrix == true 37 | disp("this is an inertia matrix, remember to check that the matrix is not dependent directly on q1!") 38 | end 39 | end -------------------------------------------------------------------------------- /Scripts/2024/Martina/moving_frames_algorithm.m: -------------------------------------------------------------------------------- 1 | function [w, v, vc, T] = moving_frames_algorithm(num_joints, DH, qdots, m, rc, prismatic_indices,I) 2 | % This function performs the moving frame algorithm and returns the 3 | % vectors w, v, vc, and the values of T for each joint 4 | % 5 | %inputs: 6 | % num_joints= an int reapresenting the number of links o fthe robot 7 | % 8 | % DH = the DH matrix of the robot in the order [alpha, a, d, theta ] 9 | % 10 | % qdts = a vector containing qdots ex: [qd1; qd2; qd3] 11 | % 12 | % m = a vector of masses [m1 ; ... ; mn] 13 | % 14 | % 15 | % rc = a vector containing the values ^ir_{ci} that is the distance 16 | % between the i-th CoM from the i-th reference frame 17 | % 18 | % prismatic indices= a list containing the list of prismatic indices, 19 | % ex for a PPR= [1,2] 20 | % 21 | % I = a vector of 3 by 3 matrices [I1,..,In] 22 | % 23 | %output: 24 | 25 | syms Ixx Iyy Izz real 26 | 27 | 28 | 29 | % Check that the DH is consistent with the indicated number of joints 30 | if size(DH, 1) ~= num_joints 31 | error('Number of rows in DH matrix should be equal to the number of joints.'); 32 | end 33 | 34 | 35 | % Check that rc is consistent with the number of joints 36 | if size(rc,2) ~= num_joints 37 | error('Length of rc vector should be equal to the number of joints.'); 38 | end 39 | 40 | % Display the number of joints and prismatic indices 41 | disp(['Number of joints: ', num2str(num_joints)]); 42 | 43 | 44 | %START THE ALGORITHM: 45 | v00=[0;0;0] 46 | w00=[0;0;0] 47 | 48 | % Initialize outputs 49 | w = zeros(3, num_joints); 50 | v = zeros(3, num_joints); 51 | vc = zeros(3, num_joints); 52 | T = cell(num_joints); 53 | 54 | 55 | %Start the loop 56 | for i = 1:num_joints 57 | In=I(1:3,(i-1)*3+1:i*3) 58 | 59 | 60 | if ismember(i, prismatic_indices) 61 | sigma = 1; 62 | else 63 | sigma =0; 64 | end 65 | A=DHMatrix(DH(i,:)); 66 | R=A(1:3,1:3) 67 | p=A(1:3,4) 68 | 69 | r_i=R'*p 70 | % computing omega 71 | if i ==1 72 | w_prev= w00; 73 | else 74 | w_prev= wi 75 | end 76 | 77 | wi=R'*(w_prev + (1- sigma)*[0;0;qdots(i)]); 78 | fprintf('the value of w_%d is:',i); 79 | disp(wi); 80 | 81 | % computing v 82 | if i ==1 83 | v_prev= v00; 84 | else 85 | v_prev= vi; 86 | end 87 | 88 | vi=R'*(v_prev + sigma*[0;0;qdots(i)])+ cross(wi,r_i,1); 89 | fprintf('The value of v_%d is:', i); 90 | disp(vi) 91 | 92 | %computing vc 93 | vci=vi+cross(wi,rc(:,i)); 94 | fprintf('The value of vc_%d is:',i); 95 | disp(vci) 96 | 97 | %computing T 98 | Ti=simplify(0.5*m(i)*(vci'*vci)+0.5*wi'*In*wi); 99 | fprintf('The value of T_%d is:', i); 100 | disp(Ti) 101 | T{i}=Ti; 102 | 103 | 104 | end -------------------------------------------------------------------------------- /Scripts/2024/Martina/newton_euler_algorithm.m: -------------------------------------------------------------------------------- 1 | function u_vector = newton_euler_algorithm(num_joints, DH, qd,qdd, m, I, r, rc, g0, prismatic_indices) 2 | % This function performs the moving frame algorithm and returns the 3 | % vectors w, v, vc, and the values of T for each joint 4 | % 5 | %inputs: 6 | % num_joints= an int reapresenting the number of links o fthe robot 7 | % 8 | % DH = the DH matrix of the robot in the order [alpha, a, d, theta ] 9 | % 10 | % qdts = a vector containing qdots ex: [qd1; qd2; qd3] 11 | % 12 | % m 13 | % 14 | % I = a vector of kind [I1,I2..] 15 | % 16 | % r = a vector containing the values ^ir_{i-1,i} that is the distance 17 | % between frame i-1 and i computed with respect to frame i 18 | % 19 | % rc = a vector containing the values ^ir_{ci} that is the distance 20 | % between the i-th CoM from the i-th reference frame 21 | % 22 | % g0= a vector representing g wrt fame 0 23 | % 24 | % prismatic indices= a list containing the list of prismatic indices, 25 | % ex for a PPR= [1,2] 26 | % 27 | % 28 | %output: 29 | 30 | syms Ixx Iyy Izz real 31 | 32 | 33 | % Check that the DH is consistent with the indicated number of joints 34 | if size(DH, 1) ~= num_joints 35 | error('Number of rows in DH matrix should be equal to the number of joints.'); 36 | end 37 | 38 | % Check that the r is consistent with the number of joints 39 | if size(r,2) ~= num_joints 40 | size(r,2) 41 | error('Length of r vector should be equal to the number of joints.'); 42 | end 43 | 44 | % Check that rc is consistent with the number of joints 45 | if size(rc,2) ~= num_joints 46 | error('Length of rc vector should be equal to the number of joints.'); 47 | end 48 | 49 | % Display the number of joints and prismatic indices 50 | disp(['Number of joints: ', num2str(num_joints)]); 51 | 52 | rotations = sym(zeros(3,3, num_joints)); 53 | rotations_from_zero = sym(zeros(3,3, num_joints)); 54 | i_g=sym(zeros(3,num_joints)) 55 | 56 | for i=1:num_joints 57 | A=DHMatrix(DH(i,:)); 58 | R=A(1:3,1:3); 59 | rotations(:,:,i)=R; 60 | A=DHMatrix(DH(1:i,:)); 61 | R=A(1:3,1:3); 62 | rotations_from_zero(:,:,i)=R; 63 | i_g(:,i)=rotations_from_zero(:,:,i)'*g0; 64 | end 65 | 66 | 67 | 68 | %forward pass 69 | % -------- initialization ----------------- 70 | w_prev=[0;0;0]; 71 | w_dot_prev=[0;0;0]; 72 | a_prev=[0;0;0]; 73 | 74 | w_vector=zeros(3,num_joints); 75 | w_dot_vector=zeros(3,num_joints); 76 | a_vector=zeros(3,num_joints); 77 | a_c_vector=zeros(3,num_joints); 78 | 79 | % --------- starting forward pass ------------- 80 | for i=1:num_joints 81 | w=rotations(:,:,i)'*(w_prev+qd(i)*[0;0;1]); 82 | w=vpa(simplify(w)) 83 | fprintf("the w of link %d is:",i); 84 | disp(w) 85 | w_vector(:,i)=w; 86 | 87 | w_dot=rotations(:,:,i)'*(w_dot_prev+qdd(i)*[0;0;1]+cross(qd(i)*w_prev,[0;0;1])); 88 | fprintf("the w_dot of link %d is:",i); 89 | disp(w_dot) 90 | w_dot_vector(:,i)=w_dot; 91 | 92 | a=rotations(:,:,i)'*a_prev+cross(w_dot,r(:,i))+cross(w,r(:,i)) 93 | a=vpa(simplify(a)) 94 | fprintf("the a of link %d is:",i); 95 | disp(a); 96 | a_vector(:,i)=a; 97 | 98 | a_c=a+cross(w_dot,r(:,i))+cross(w,cross(w,r(:,i))) 99 | fprintf("the a_c of link %d is:",i); 100 | disp(a_c); 101 | a_c_vector(:,i)=a_c; 102 | 103 | %update the prev 104 | w_prev=w; 105 | W_dot_prev=w_dot; 106 | a_prev=a; 107 | end 108 | 109 | %starting backward pass 110 | %---------------- initialization -------------------- 111 | f_next=[0;0;0]; 112 | tau_next=[0;0;0]; 113 | 114 | f_vector=sym(zeros(3,num_joints)); 115 | tau_vector=sym(zeros(3,num_joints)); 116 | 117 | %--------------- starting backward pass -------------- 118 | for i=num_joints:-1:1 119 | if i == num_joints 120 | f=m(i)*(a_c_vector(:,i)-i_g(:,i)); 121 | else 122 | f=rotations(:,:,i+1)*f_next+m(i)*(a_c_vector(:,i)-i_g(:,i)); 123 | end 124 | f=vpa(simplify(f)); 125 | fprintf("the f of link %d is:",i); 126 | disp(f); 127 | f_vector(:,i)=f; 128 | 129 | if i ==num_joints 130 | tau=-cross(f,(r(:,i)+rc(:,i)))+I(i)*w_dot_vector(:,i)+cross(w_vector(:,i),(I(i)*w_vector(:,i))); 131 | else 132 | tau=rotations(:,:,i+1)*tau_next+ cross((rotations(:,:,i+1)*f_next),rc(:,i))-cross(f,(r(:,i)+rc(:,i)))+I(i)*w_dot_vector(:,i)+cross(w_vector(:,i),(I(i)*w_vector(:,i))); 133 | 134 | end 135 | tau=vpa(simplify(tau)); 136 | fprintf("the tau of link %d is:",i); 137 | disp(tau); 138 | tau_vector(:,i)=tau; 139 | 140 | f_next=f; 141 | tau_next=tau; 142 | end 143 | 144 | u_vector=sym(zeros(3,num_joints)) 145 | for i=1:num_joints 146 | if ismember(i, prismatic_indices) 147 | ui=f_vector(:,i)'*(rotations(:,:,i)'*[0;0;1]); 148 | else 149 | ui=tau_vector(:,i)'*(rotations(:,:,i)'*[0;0;1]); 150 | end 151 | ui=vpa(simplify(ui)); 152 | fprintf("u of link %d is:",i); 153 | disp(ui) 154 | u_vector(:,i)=ui; 155 | end 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | -------------------------------------------------------------------------------- /Scripts/2024/Martina/reduced_gradient.m: -------------------------------------------------------------------------------- 1 | %% Reduced Gradient 2 | function qd_red = reduced_gradient(JA,JB,T,pd) 3 | % takes as inputs: 4 | % - JA = the jacobian matrix JA of decomposion of J 5 | % of size MxM, non singular 6 | % - JB = the jacobian matrix JB of decomposion of J 7 | % of size Mx(N-M) 8 | % - T = identity matrix, corresponding to the q 9 | % variables of the Jacobean JA and JB. 10 | % For example J has dimension 2x3 = [JA JB]. 11 | % JA =[J1 J3] and JB = [J2]. 12 | % T will be = [1 0 0; 0 0 1; 0 1 0], first column 13 | % [1;0;0;] because we have q1, second column is 14 | % [0;0;1;] because we have q3 and the last column is 15 | % [0;1;0;] because we have q2 16 | % - pd = the cartesian velocity 17 | % 18 | % output: 19 | % - qd_red = q dot obtain by reduced gradient 20 | 21 | syms q1 q2 q3 22 | %% JA and JB two matrices give by decomposion that compose J 23 | J = [JA JB]; 24 | 25 | %% control if JA is full row rank 26 | if rank(JA) == size(JA,1) 27 | invJA=inv(JA); 28 | else 29 | disp("my friend, you have a problem") 30 | end 31 | 32 | %% Compute gradient of the objective function given by the problem 33 | % for example: H(q) = sin^2(q2)+sin^2(q3) 34 | % in this case fist row is zero because we don't have variable q1 35 | gradq_H= [0; 2*sin(q2)*cos(q2); 2*sin(q3)*cos(q3)]; 36 | 37 | %% Compute reduced gradient 38 | first_part = invJA*JB; 39 | transpose_first = -transpose(first_part); 40 | redgrad_qb_H = [transpose_first 1]*(T*gradq_H); 41 | 42 | %% q dot reduced gradient 43 | qd_red = transpose(T)*[invJA*(pd - JB * redgrad_qb_H); redgrad_qb_H] 44 | -------------------------------------------------------------------------------- /Scripts/2024/Martina/regressor_matrix.m: -------------------------------------------------------------------------------- 1 | function Y = regressor_matrix(arg,a_vector) 2 | %input 3 | %a_vector= a vector of form [a1;a2;a3..] 4 | Y=simplify(jacobian(arg,a_vector)) -------------------------------------------------------------------------------- /Scripts/2024/Martina/test_moving_frame_midterm_2024.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2024/Martina/test_moving_frame_midterm_2024.mlx -------------------------------------------------------------------------------- /Scripts/2024/Martina/untitled2.m: -------------------------------------------------------------------------------- 1 | clear all 2 | clc 3 | syms q1 q2 q3 l y real 4 | digits(4); 5 | %% 6 | l=1 7 | 8 | p=[l*(cos(q1)+cos(q1+q2)+cos(q1+q2+q3)); 9 | l*(sin(q1)+sin(q1+q2)+sin(q1+q2+q3))] 10 | j=jacobian(p,[q1,q2,q3]) 11 | jp=pinv(j) 12 | %vpasolve((y-2)^2+(0.25^2-0.5^2),y) 13 | H=norm([cos(q1)+cos(q1+q2)-1;sin(q1)+sin(q1+q2)-1.567]) 14 | DeH=vpa(subs(simplify([diff(H,q1);diff(H,q2);diff(H,q3)]),[q1,q2,q3],[0,pi/2,-pi/2])) 15 | first_part=jp*[0;1] 16 | second_part=eye(3)-jp*j 17 | qdot=vpa(subs(first_part+second_part*-DeH,[q1,q2,q3],[0,pi/2,-pi/2])) 18 | vm=subs(j(:,1:2)*qdot(1:2),[q1,q2,q3],[0,pi/2,-pi/2]) 19 | qdot1=vpa(subs(jp*[0;1],[q1,q2,q3],[0,pi/2,-pi/2])) 20 | P1=vpa(simplify(subs(-jp*j,[q1,q2,q3],[0,pi/2,-pi/2]))) 21 | p2=[l*(cos(q1)+cos(q1+q2)); 22 | l*(sin(q1)+sin(q1+q2))] 23 | j2=subs(jacobian(p,[q1,q2,q3]),[q1,q2,q3],[0,pi/2,-pi/2]) 24 | qdot2=qdot1+pinv((j2*P1))*(vm-j2*qdot1) -------------------------------------------------------------------------------- /Scripts/2024/Martina/untitled2.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/universitymarr/Robotics2/2971b443af6c8eb978672a215c1463630baff446/Scripts/2024/Martina/untitled2.mlx --------------------------------------------------------------------------------