├── .gitignore ├── Exercises ├── CollectionOfProblems_v11.pdf ├── Exercise_I │ ├── Examples │ │ ├── Example_1_SimpleOscillator.slx │ │ └── Example_2_ExampleLinkage.slx │ ├── Solutions │ │ ├── Solution_2.pdf │ │ ├── Solution_2a_new_model.slx │ │ ├── Solution_2a_simple_link.slx │ │ ├── Solution_2a_simple_pendulum.slx │ │ ├── Solution_2a_simple_pendulum_analysis.slx │ │ ├── Solution_2b_HDPE.slx │ │ ├── Solution_2c_Moon.slx │ │ ├── Solution_3.pdf │ │ ├── Solution_3a_DoublePendulum.png │ │ ├── Solution_3a_DoublePendulum.slx │ │ ├── Solution_3b_DoublePendulum_ManualParameters.slx │ │ ├── Solution_3c_DoublePendulum_input.slx │ │ ├── Solution_4.pdf │ │ ├── Solution_4a_4BarLinkage.slx │ │ ├── Solution_4b_4BarLinkage_balanced.bmp │ │ ├── Solution_4b_4BarLinkage_balanced.slx │ │ ├── Solution_4c_4BarLinkage_OutOfRange.slx │ │ ├── Solution_5.pdf │ │ ├── Solution_5_ErrorSystem.slx │ │ ├── Solution_5_ErrorSystem_fixed.slx │ │ └── Thumbs.db │ └── simscape_tutorials │ │ ├── init_model.mat │ │ ├── main.m │ │ ├── simple_pendulum_analysis.slx │ │ ├── simple_pendulum_analysis.slx.original │ │ ├── simple_pendulum_analysis.slxc │ │ └── slprj │ │ └── sim │ │ └── varcache │ │ └── simple_pendulum_analysis │ │ ├── checksumOfCache.mat │ │ ├── tmwinternal │ │ └── simulink_cache.xml │ │ └── varInfo.mat ├── Exercise_II │ ├── ClassFiles │ │ ├── CoSysCLASS.m │ │ ├── EnvironmentCLASS.m │ │ └── VectorCLASS.m │ ├── Examples │ │ ├── Example_3_SingleTransformations.m │ │ ├── Example_4_DoubleTransformations.m │ │ └── Example_5_Derivatives.m │ ├── Solutions │ │ ├── AB_inv_fct.m │ │ ├── A_IC_fct.m │ │ ├── BetterCoSysCLASS.m │ │ ├── C3_omega_IC3_fct.m │ │ ├── C3_omega_IC3_vec_fct.m │ │ ├── Solution_10.pdf │ │ ├── Solution_19_InertiaMatrix.m │ │ ├── Solution_6_EulerAngles.m │ │ ├── Solution_7_EulerAngles_Derivatives.m │ │ ├── Solution_8.pdf │ │ ├── Solution_8a_IntegratingAngularVelocities.m │ │ ├── Solution_8b_IntegratingAngularVelocities.m │ │ └── Solution_8c_IntegratingAngularVelocities.m │ ├── Templates │ │ ├── Template_7_EulerAngles_Derivatives.m │ │ ├── Template_8a_IntegratingAngularVelocities.m │ │ ├── Template_8b_IntegratingAngularVelocities.m │ │ └── Template_9_BetterCoSysCLASS.m │ └── TestFiles │ │ └── TestFile_9_BetterCoSysCLASS.m ├── Exercise_III │ ├── Examples │ │ └── Example_6_DemonstrateRotationStability.m │ ├── Problem_12_Output.jpg │ ├── Problem_12b_Output.jpg │ ├── Problem_14_Output.jpg │ ├── Problem_15_Output.jpg │ ├── Solutions │ │ ├── BoundCoSysCLASS.m │ │ ├── BoundVectorCLASS.m │ │ ├── RigidBodyDynamicsCLASS.m │ │ ├── RigidBodyKinematicsCLASS.m │ │ ├── ShowInertia.m │ │ ├── Solution_12.pdf │ │ ├── Solution_13.pdf │ │ ├── Solution_15.pdf │ │ ├── Solution_16.pdf │ │ ├── Solution_18.pdf │ │ ├── Solution_19.pdf │ │ ├── Solution_20.pdf │ │ └── Thumbs.db │ ├── Templates │ │ ├── Template_11_BoundCoSysCLASS.m │ │ ├── Template_11_BoundVectorCLASS.m │ │ ├── Template_12_RigidBodyKinematicsCLASS.m │ │ ├── Template_15_RigidBodyDynamicsCLASS.m │ │ └── Template_17_ShowInertia.m │ ├── TestFiles │ │ ├── Problem_12_Output.jpg │ │ ├── Problem_15_Output.jpg │ │ ├── TestFile_11_BoundVectorAndCoSys.m │ │ ├── TestFile_12_RigidBodyKinematics.m │ │ ├── TestFile_14_VisualizingTwists.m │ │ ├── TestFile_15_RigidBodyDynamics.m │ │ ├── TestFile_17_ShowInertia.m │ │ └── Thumbs.db │ └── Thumbs.db ├── Exercise_IV │ ├── ClassFiles │ │ ├── LinkedBodyObjectCLASS.m │ │ └── LinkedJointObjectCLASS.m │ ├── Examples │ │ ├── Example_7_LinkedList.m │ │ └── Example_8_SymbolicTriplePendulum.m │ ├── Solutions │ │ ├── LinkedRigidBodyDynamicsCLASS.m │ │ ├── LinkedRollingContactJointCLASS.m │ │ ├── LinkedRotationalJointCLASS.m │ │ ├── Problem_25_Output.jpg │ │ ├── Solution_21.pdf │ │ ├── Solution_22_RecursiveKinematics.m │ │ ├── Solution_22_RecursiveKinematics_RecursiveFunctionCall.m │ │ ├── Solution_25_TestRollingContactJoint.m │ │ ├── Solution_26.pdf │ │ ├── Solution_27.pdf │ │ ├── Solution_28.pdf │ │ └── Thumbs.db │ ├── Templates │ │ ├── Template_22_RecursiveKinematics.m │ │ ├── Template_24_LinkedRigidBodyDynamicsCLASS.m │ │ ├── Template_24_LinkedRotationalJointCLASS.m │ │ ├── Template_25_LinkedRollingContactJointCLASS.m │ │ └── Template_25_TestRollingContactJoint.m │ └── TestFiles │ │ ├── TestData_24_TriplePendulumSim.mat │ │ ├── TestFile_24_RotationalJoint.m │ │ └── Thumbs.db ├── Exercise_V │ ├── ClassFiles │ │ ├── LinkedBodyObjectCLASS_v2.m │ │ ├── LinkedGenericJointCLASS_v2.m │ │ ├── LinkedJointObjectCLASS_v2.m │ │ ├── LinkedRigidBodyDynamicsCLASS_v2.m │ │ ├── LinkedRotationalJointCLASS_v2.m │ │ ├── RigidBodyDynamicsCLASS_v2.m │ │ └── RigidBodyKinematicsCLASS_v2.m │ ├── Examples │ │ └── Example_9_2DOFManipulator.m │ ├── Solutions │ │ ├── LinkedGenericJointVeAcJaCLASS.m │ │ ├── LinkedRigidBodyDynamicsVeAcJaCLASS.m │ │ ├── LinkedRollingContactJointVeAcJaCLASS.m │ │ ├── LinkedRotationalJointVeAcJaCLASS.m │ │ ├── LinkedTranslationalJointVeAcJaCLASS.m │ │ ├── LinkedVirtual3DOFJointVeAcJaCLASS.m │ │ ├── Solution_30.pdf │ │ ├── Solution_32_GravityCompensation.m │ │ ├── Solution_35.pdf │ │ ├── Solution_36.pdf │ │ ├── Solution_36_DoublePendulum.m │ │ └── Thumbs.db │ ├── Templates │ │ ├── Template_31_LinkedGenericJointVeAcJaCLASS.m │ │ ├── Template_31_LinkedRigidBodyDynamicsVeAcJaCLASS.m │ │ ├── Template_31_LinkedRotationalJointVeAcJaCLASS.m │ │ ├── Template_32_GravityCompensation.m │ │ ├── Template_33_LinkedRollingContactJointVeAcJaCLASS.m │ │ ├── Template_34_LinkedTranslationalJointVeAcJaCLASS.m │ │ └── Template_34_LinkedVirtual3DOFJointVeAcJaCLASS.m │ ├── TestFiles │ │ ├── Problem_33_Output.jpg │ │ ├── TestData_30_31_TriplePendulumSim.mat │ │ ├── TestData_32_RobotTrajectory.mat │ │ ├── TestData_34_RunningData.mat │ │ ├── TestData_34_WalkingData.mat │ │ ├── TestFile_30_RotationalJoint_v2.m │ │ ├── TestFile_31_RotationalJointVeAcJa.m │ │ ├── TestFile_33_RollingContactJoint.m │ │ ├── TestFile_34_Biped.m │ │ └── Thumbs.db │ └── Thumbs.db ├── Exercise_VI │ ├── Solutions │ │ ├── LinkedRigidBodyDynamicsMfgCLASS.m │ │ ├── Solution_37_ControllerEvaluation.m │ │ ├── Solution_37_ControllerEvaluation_dr.m │ │ ├── Solution_38_PassiveDynamicWalker.m │ │ ├── Solution_39.pdf │ │ ├── Solution_40.pdf │ │ ├── Solution_40_DoublePendulumEquations.m │ │ ├── Solution_41.pdf │ │ └── Thumbs.db │ ├── Templates │ │ ├── Template_37_ControllerEvaluation.m │ │ ├── Template_37_LinkedRigidBodyDynamicsMfgCLASS.m │ │ └── Template_38_PassiveDynamicWalker.m │ └── TestFiles │ │ ├── BetterGraphics │ │ ├── CADInnerLegs.txt │ │ ├── CADMainAxis.txt │ │ ├── CADOuterLegs.txt │ │ ├── CADWeight.txt │ │ ├── CoGPositions.m │ │ ├── ContStateDefinition.m │ │ ├── FootPtPositions.m │ │ ├── GraphicalKinematicsWrapper.m │ │ ├── LinkPositions.m │ │ ├── OutputCLASS.m │ │ ├── PassiveWalker3DCLASS.m │ │ ├── Struct2Vec.m │ │ ├── SystParamDefinition.m │ │ ├── TransformVertices.m │ │ ├── Vec2Struct.m │ │ └── rndread.m │ │ └── TestData_37_ControllerEvaluation.mat ├── Exercise_VII │ ├── Examples │ │ ├── Example_10_ForwardDynamicsWithLoops.m │ │ ├── Example_11_CreateVelLambdaFigure.m │ │ ├── Example_11_PDW_Collisions.m │ │ ├── Example_12_VirtualModelControl.m │ │ ├── Example_14_PerformanceComparisonNumerical.asv │ │ ├── Example_14_PerformanceComparisonNumerical.m │ │ ├── Example_15_PerformanceComparisonAnalytical.m │ │ └── M_mat.m │ ├── Solutions │ │ ├── Solution_43.pdf │ │ ├── Solution_43_ImplicitConstraints.m │ │ ├── Solution_44_EventDetection.m │ │ ├── Solution_45.m │ │ ├── Solution_45.pdf │ │ ├── Solution_46.pdf │ │ ├── Solution_46_RoboticArmCollision.m │ │ ├── Solution_47.pdf │ │ ├── Solution_TBD_PDW_Continuous.m │ │ ├── Thumbs.db │ │ └── skew.m │ ├── Templates │ │ ├── Template_43_ImplicitConstraints.m │ │ ├── Template_44_EventDetection.m │ │ ├── Template_46_RoboticArmCollision.m │ │ └── skew.m │ └── TestFiles │ │ └── TestData_46_RoboticArmCollision.mat └── Schedule.pdf ├── LectureNotes ├── 0_Introduction.pdf ├── 1_Fundamentals.pdf ├── 2_RigidBodyDynamics.pdf ├── 3_Constraints_Joints.pdf ├── 4_RecursiveKinematics.pdf ├── 5_MultiBodyDynamics.pdf ├── 6_LoopsAndContact.pdf ├── Exercise_I.pdf ├── Exercise_II.pdf ├── Exercise_III.pdf ├── Exercise_IV.pdf └── Exercise_V.pdf ├── Multibodydynamics_engine ├── matlab │ ├── UML.pdf │ ├── classes │ │ ├── BetterCoSysCLASS.m │ │ ├── BoundCoSysCLASS.m │ │ ├── BoundVectorCLASS.m │ │ ├── CoSysCLASS.m │ │ ├── EnvironmentCLASS.m │ │ ├── LinkedBodyObjectCLASS_v2.m │ │ ├── LinkedGenericJointCLASS_v2.m │ │ ├── LinkedGenericJointVeAcJaCLASS.m │ │ ├── LinkedJointObjectCLASS_v2.m │ │ ├── LinkedRigidBodyDynamicsCLASS_v2.m │ │ ├── LinkedRigidBodyDynamicsMfgCLASS.m │ │ ├── LinkedRigidBodyDynamicsVeAcJaCLASS.m │ │ ├── LinkedRollingContactJointVeAcJaCLASS.m │ │ ├── LinkedRotationalJointCLASS_v2.m │ │ ├── LinkedRotationalJointVeAcJaCLASS.m │ │ ├── LinkedTranslationalJointVeAcJaCLASS.m │ │ ├── LinkedVirtual3DOFJointVeAcJaCLASS.m │ │ ├── RigidBodyDynamicsCLASS_v2.m │ │ ├── RigidBodyKinematicsCLASS_v2.m │ │ ├── VectorCLASS.m │ │ └── skew.m │ └── testfiles │ │ ├── Solution_43.pdf │ │ ├── Solution_43_ImplicitConstraints.m │ │ ├── Solution_44_EventDetection.m │ │ ├── Solution_45.m │ │ ├── Solution_45.pdf │ │ ├── Solution_46.pdf │ │ ├── Solution_46_RoboticArmCollision.m │ │ ├── TestData_30_31_TriplePendulumSim.mat │ │ ├── TestData_32_RobotTrajectory.mat │ │ ├── TestData_34_RunningData.mat │ │ ├── TestData_34_WalkingData.mat │ │ ├── TestData_37_ControllerEvaluation.mat │ │ ├── TestFile_30_RotationalJoint_v2.m │ │ ├── TestFile_31_RotationalJointVeAcJa.m │ │ ├── TestFile_32_GravityCompensation.m │ │ ├── TestFile_33_RollingContactJoint.m │ │ ├── TestFile_34_Biped.m │ │ ├── TestFile_37_ControllerEvaluation.m │ │ ├── TestFile_37_ControllerEvaluation_dr.m │ │ ├── TestFile_38_PassiveDynamicWalker.m │ │ └── TestFile_90_DoublePendulum.m └── python │ ├── .ipynb_checkpoints │ ├── Untitled-checkpoint.ipynb │ └── test-checkpoint.py │ ├── .vscode │ ├── .ropeproject │ │ ├── config.py │ │ └── objectdb │ ├── launch.json │ └── settings.json │ ├── classes │ ├── GenericJoint.py │ ├── MultiRigidBody.py │ ├── PositionBilateralConstraint.py │ ├── RigidBody.py │ ├── RigidJoint.py │ ├── RollingContactJoint.py │ ├── RotationalJoint.py │ ├── SpringDamper.py │ ├── TranslationalJoint.py │ ├── __init__.py │ ├── robotics_helpfuns.py │ └── vpython_ext.py │ └── testfiles │ ├── TestFile_91_TriplePendulum.py │ ├── TestFile_92_BilateralConstraints.py │ └── TestFile_93_Suspension.py └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | *.~m 2 | *.png 3 | *.jpg 4 | *.pyc 5 | -------------------------------------------------------------------------------- /Exercises/CollectionOfProblems_v11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/CollectionOfProblems_v11.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_I/Examples/Example_1_SimpleOscillator.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Examples/Example_1_SimpleOscillator.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Examples/Example_2_ExampleLinkage.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Examples/Example_2_ExampleLinkage.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_2.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_2a_new_model.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_2a_new_model.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_2a_simple_link.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_2a_simple_link.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_2a_simple_pendulum.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_2a_simple_pendulum.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_2a_simple_pendulum_analysis.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_2a_simple_pendulum_analysis.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_2b_HDPE.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_2b_HDPE.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_2c_Moon.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_2c_Moon.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_3.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_3a_DoublePendulum.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_3a_DoublePendulum.png -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_3a_DoublePendulum.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_3a_DoublePendulum.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_3b_DoublePendulum_ManualParameters.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_3b_DoublePendulum_ManualParameters.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_3c_DoublePendulum_input.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_3c_DoublePendulum_input.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_4.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_4.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_4a_4BarLinkage.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_4a_4BarLinkage.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_4b_4BarLinkage_balanced.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_4b_4BarLinkage_balanced.bmp -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_4b_4BarLinkage_balanced.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_4b_4BarLinkage_balanced.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_4c_4BarLinkage_OutOfRange.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_4c_4BarLinkage_OutOfRange.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_5.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_5.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_5_ErrorSystem.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_5_ErrorSystem.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Solution_5_ErrorSystem_fixed.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Solution_5_ErrorSystem_fixed.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/Solutions/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/Solutions/Thumbs.db -------------------------------------------------------------------------------- /Exercises/Exercise_I/simscape_tutorials/init_model.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/simscape_tutorials/init_model.mat -------------------------------------------------------------------------------- /Exercises/Exercise_I/simscape_tutorials/main.m: -------------------------------------------------------------------------------- 1 | L = 20; 2 | H = 1; 3 | W = 1; 4 | rgb = [1 0 0]; 5 | rho = 2700; 6 | 7 | figure; % Open a new figure 8 | hold on; 9 | plot(out.q); % Plot the pendulum angle 10 | plot(out.w); % Plot the pendulum angular velocity 11 | 12 | figure; 13 | plot(out.q.data, out.w.data); 14 | 15 | 16 | A = [1 1; 1 -1]/2^0.5 17 | det(A) 18 | A^-1 19 | 20 | 21 | syms H P c Rk M Pk 22 | M = Pk^-1; 23 | P = Rk^-1 24 | UV = [H.'*P*H+M 0; 0 P-P*H*(H.'*P*H+M)^-1*H.'*P] 25 | 26 | inv(UV) -------------------------------------------------------------------------------- /Exercises/Exercise_I/simscape_tutorials/simple_pendulum_analysis.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/simscape_tutorials/simple_pendulum_analysis.slx -------------------------------------------------------------------------------- /Exercises/Exercise_I/simscape_tutorials/simple_pendulum_analysis.slx.original: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/simscape_tutorials/simple_pendulum_analysis.slx.original -------------------------------------------------------------------------------- /Exercises/Exercise_I/simscape_tutorials/simple_pendulum_analysis.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/simscape_tutorials/simple_pendulum_analysis.slxc -------------------------------------------------------------------------------- /Exercises/Exercise_I/simscape_tutorials/slprj/sim/varcache/simple_pendulum_analysis/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/simscape_tutorials/slprj/sim/varcache/simple_pendulum_analysis/checksumOfCache.mat -------------------------------------------------------------------------------- /Exercises/Exercise_I/simscape_tutorials/slprj/sim/varcache/simple_pendulum_analysis/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 14+AaOIf6fJgcuguOP7H/g== 5 | 6 | -------------------------------------------------------------------------------- /Exercises/Exercise_I/simscape_tutorials/slprj/sim/varcache/simple_pendulum_analysis/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_I/simscape_tutorials/slprj/sim/varcache/simple_pendulum_analysis/varInfo.mat -------------------------------------------------------------------------------- /Exercises/Exercise_II/ClassFiles/EnvironmentCLASS.m: -------------------------------------------------------------------------------- 1 | % EnvironmentCLASS < handle 2 | % 3 | % Defines a 'physical' environment in which all further components are 4 | % embedded. Since this is a purely numerical tool, all 'physical' 5 | % definitions refer to vectors and coordinates given in the (inertial) 6 | % coordinates of this graphics window. 7 | % 8 | % Methods: 9 | % Env = EnvironmentCLASS() % Creates a new physical (graphical) 10 | % environment 11 | % Env.delete() % Closes the environment and removes it from 12 | % the memory 13 | % Env.toggleAxis() % Toggles between showing and hiding the axis 14 | % of the environment 15 | % Env.resetOutput() % Fits the axis to the size of the objects in 16 | % it and resets all view parameter 17 | % 18 | % Properties: 19 | % fig % Handle to the figure that stores the 20 | % graphical output 21 | % 22 | % C. David Remy remy@inm.uni-stuttgart.de 23 | % Matlab R2018 24 | % 10/12/2018 25 | % v21 26 | % 27 | classdef EnvironmentCLASS < handle 28 | % Public Properties 29 | properties 30 | fig % handle to the figure window where the graphical output happens 31 | end 32 | % Private Properties 33 | properties (SetAccess = private, GetAccess = private) 34 | % Handles to the graphical output 35 | ax 36 | light_handle 37 | % Indicates whether the graphical (inertial) axis are visible or 38 | % not. 39 | axVisible = 1; 40 | end 41 | 42 | % Public Methods 43 | methods 44 | % Constructor creates a graphics environment 45 | function obj = EnvironmentCLASS() 46 | obj.fig = figure; 47 | set(obj.fig, 'Color', [0.95 0.95 0.95]); 48 | obj.ax = axes; 49 | obj.light_handle = camlight('right'); 50 | resetOutput(obj); 51 | end 52 | % Destructor closes the figure upon deletion 53 | function delete(obj) 54 | close(obj.fig); 55 | end 56 | % Update output 57 | function resetOutput(obj) 58 | figure(obj.fig); 59 | if obj.axVisible 60 | axis(obj.ax, 'on'); 61 | else 62 | axis(obj.ax, 'off'); 63 | end 64 | axis(obj.ax, 'equal'); 65 | view(obj.ax, [-37.5,30]) 66 | axis(obj.ax, 'tight'); 67 | a = axis(obj.ax); 68 | axis(obj.ax, a+[-1,1,-1,1,-1,1]); 69 | box(obj.ax, 'on') 70 | grid(obj.ax, 'on') 71 | camproj(obj.ax, 'perspective'); 72 | camlight(obj.light_handle, 'right') 73 | view(obj.ax, [125, 25]) 74 | end 75 | % Toggle the visibility of the graphical axis: 76 | function toggleAxis(obj) 77 | obj.axVisible = 1-obj.axVisible; 78 | resetOutput(obj); 79 | end 80 | end 81 | % Private Methods 82 | % - none 83 | end -------------------------------------------------------------------------------- /Exercises/Exercise_II/Examples/Example_3_SingleTransformations.m: -------------------------------------------------------------------------------- 1 | % Script to demonstrate the use of the following components: 2 | % EnvironmentCLASS, VectorCLASS, and CoSysCLASS 3 | % 4 | % C. David Remy remy@inm.uni-stuttgart.de 5 | % Matlab R2018 6 | % 10/12/2018 7 | % v21 8 | % 9 | clear all 10 | 11 | % SET UP THE PATH 12 | % This needs to be done only once: 13 | % Click "Set Path" in the menu of the Matlab main window. 14 | % Click "Add Folder..." and add the folder in which you want to store the 15 | % class files (e.g., "...\CDR\Matlab\ClassFiles") 16 | % Click "Save" (so you don't have to repeat this in the future) 17 | % Click "Close" 18 | % Done... 19 | % Now copy every file that ends in ...CLASS into this folder. 20 | 21 | % Define some RGB values: 22 | red = [1;0;0]; 23 | green = [0;1;0]; 24 | blue = [0;0;1]; 25 | black = [0;0;0]; 26 | grey = [0.3;0.3;0.3]; 27 | mag = [1;0;1]; 28 | yel = [1;1;0]; 29 | 30 | %% Create a 'physical' (graphical) environment: 31 | Env = EnvironmentCLASS(); 32 | 33 | %% Coordinate system C with the same orientation as the graphics co-sys: 34 | A_IC = eye(3); 35 | C = CoSysCLASS(Env,A_IC); 36 | C.color = green; 37 | C.name = 'C'; 38 | 39 | 40 | %% Create a coordinate system that is rotated by 30 deg about the 3-axis: 41 | gamma = 30*pi/180; 42 | A_IB =[cos(gamma), -sin(gamma), 0; 43 | sin(gamma), cos(gamma), 0; 44 | 0, 0, 1]; 45 | B = CoSysCLASS(Env,A_IB); 46 | B.color = red; 47 | B.name = 'B'; 48 | 49 | %% Define a vector in this coordinate system which has the following 50 | % components: 51 | B_v = [0.5;0.5;0]; 52 | v1 = VectorCLASS(Env, B, B_v); 53 | v1.color = black; 54 | v1.name = 'v1'; 55 | 56 | %% A good visual check is to see if the columns of A_CB are indeed the unit 57 | % vectors of B expressed in C: 58 | disp(A_IB) 59 | %% similarly, the columns of the inverse transformation matrix A_BC = A_CB' 60 | % should be the unit vectors of C expressed in B. We rotate the 'world', 61 | % so we can check this more easily. Also, it shows that these are only 62 | % relative descriptions. No coordinate system is more important than the 63 | % other 64 | disp(A_IB') 65 | Env.toggleAxis; 66 | camup([0,-0.5,0.87]) 67 | %% And rotate back: 68 | camup([0, 0, 1]) 69 | Env.toggleAxis; 70 | 71 | %% A Vector v2, that has the same components as v1, but is defined in 72 | % coordinate system C, will point in a different direction: 73 | C_v = B_v; 74 | v2 = VectorCLASS(Env, C, C_v); 75 | v2.color = grey; 76 | v2.name = 'v2'; 77 | 78 | %% But if we convert the B-components into C-components, the two vectors will be identical: 79 | C_v = A_IC'*A_IB*B_v; 80 | v2.setCoords(C, C_v); 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | -------------------------------------------------------------------------------- /Exercises/Exercise_II/Examples/Example_4_DoubleTransformations.m: -------------------------------------------------------------------------------- 1 | % Script to further demonstrate the use of the following components: 2 | % EnvironmentCLASS, VectorCLASS, and CoSysCLASS 3 | % 4 | % C. David Remy cdremy@umich.edu 5 | % Matlab R2012b 6 | % 9/12/2013 7 | % v11 8 | % 9 | close all; 10 | clearvars; 11 | 12 | % Some RGB values: 13 | red = [1;0;0]; 14 | green = [0;1;0]; 15 | blue = [0;0;1]; 16 | black = [0;0;0]; 17 | grey = [0.3;0.3;0.3]; 18 | mag = [1;0;1]; 19 | yel = [1;1;0]; 20 | 21 | %% Create a 'physical' (graphical) environment: 22 | Env = EnvironmentCLASS(); 23 | 24 | %% Coordinate system I at the same location as the graphics co-sys: 25 | I = CoSysCLASS(Env, eye(3)); 26 | I.color = green; 27 | I.name = 'I'; 28 | 29 | %% Create a coordinate system that is rotated by 20 deg about the 3-axis: 30 | gamma = 20*pi/180; 31 | A_IC1 =[cos(gamma), -sin(gamma), 0; 32 | sin(gamma), cos(gamma), 0; 33 | 0, 0, 1]; 34 | C1 = CoSysCLASS(Env, A_IC1); 35 | C1.color = red; 36 | C1.name = 'C1'; 37 | 38 | %% Create a coordinate system that is rotated by 15 deg about the 2-axis: 39 | beta = 15*pi/180; 40 | A_C1C2 =[+cos(beta), 0, +sin(beta); 41 | 0 , 1, 0; 42 | -sin(beta), 0, +cos(beta)]; 43 | 44 | C2 = CoSysCLASS(Env,A_C1C2); 45 | C2.color = blue; 46 | C2.name = 'C2'; 47 | 48 | %% Ups, this didn't do what we wanted, since CoSysCLASS needs an A_IC2 49 | %% transformation. Let's fix that 50 | A_IC2 = A_IC1 * A_C1C2; 51 | C2.A_IC = A_IC2; 52 | 53 | %% Create a vector and show it in different coordinate systems 54 | C2_v = [1;1;0]; 55 | v = VectorCLASS(Env, C2, C2_v); 56 | v.color = black; 57 | v.name = 'v'; 58 | 59 | I_v = A_IC1 * A_C1C2 * C2_v; 60 | disp('I_v:'); 61 | disp(I_v); 62 | 63 | v2 = VectorCLASS(Env, I, I_v); 64 | v2.color = grey; 65 | v2.name = 'v2'; 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /Exercises/Exercise_II/Solutions/AB_inv_fct.m: -------------------------------------------------------------------------------- 1 | function AB_inv = AB_inv_fct(alpha,beta,gamma) 2 | %AB_INV_FCT 3 | % AB_INV = AB_INV_FCT(ALPHA,BETA,GAMMA) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.3. 6 | % 12-Nov-2019 11:06:31 7 | 8 | t2 = cos(beta); 9 | t3 = cos(gamma); 10 | t4 = sin(beta); 11 | t5 = sin(gamma); 12 | t6 = 1.0./t2; 13 | AB_inv = reshape([t3.*t6,-t5,t3.*t4.*t6,t5.*t6,t3,t4.*t5.*t6,0.0,0.0,1.0],[3,3]); 14 | -------------------------------------------------------------------------------- /Exercises/Exercise_II/Solutions/A_IC_fct.m: -------------------------------------------------------------------------------- 1 | function A_IC = A_IC_fct(alpha,beta,gamma) 2 | %A_IC_FCT 3 | % A_IC = A_IC_FCT(ALPHA,BETA,GAMMA) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.3. 6 | % 12-Nov-2019 11:06:32 7 | 8 | t2 = cos(alpha); 9 | t3 = cos(beta); 10 | t4 = cos(gamma); 11 | t5 = sin(alpha); 12 | t6 = sin(beta); 13 | t7 = sin(gamma); 14 | A_IC = reshape([t3.*t4,t3.*t7,-t6,-t2.*t7+t4.*t5.*t6,t2.*t4+t5.*t6.*t7,t3.*t5,t5.*t7+t2.*t4.*t6,-t4.*t5+t2.*t6.*t7,t2.*t3],[3,3]); 15 | -------------------------------------------------------------------------------- /Exercises/Exercise_II/Solutions/C3_omega_IC3_fct.m: -------------------------------------------------------------------------------- 1 | function C3_omega_IC3 = C3_omega_IC3_fct(alpha,beta,gamma,alpha_dot,beta_dot,gamma_dot) 2 | %C3_OMEGA_IC3_FCT 3 | % C3_OMEGA_IC3 = C3_OMEGA_IC3_FCT(ALPHA,BETA,GAMMA,ALPHA_DOT,BETA_DOT,GAMMA_DOT) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 02-Jan-2019 11:17:36 7 | 8 | t2 = cos(alpha); 9 | t3 = cos(beta); 10 | t4 = sin(alpha); 11 | t5 = beta_dot.*t4; 12 | t6 = beta_dot.*t2; 13 | t7 = gamma_dot.*t3.*t4; 14 | t8 = sin(beta); 15 | t9 = gamma_dot.*t8; 16 | C3_omega_IC3 = reshape([0.0,-t5+gamma_dot.*t2.*t3,-t6-t7,t5-gamma_dot.*t2.*t3,0.0,alpha_dot-t9,t6+t7,-alpha_dot+t9,0.0],[3,3]); 17 | -------------------------------------------------------------------------------- /Exercises/Exercise_II/Solutions/C3_omega_IC3_vec_fct.m: -------------------------------------------------------------------------------- 1 | function C3_omega_IC3_vec = C3_omega_IC3_vec_fct(alpha,beta,gamma,alpha_dot,beta_dot,gamma_dot) 2 | %C3_OMEGA_IC3_VEC_FCT 3 | % C3_OMEGA_IC3_VEC = C3_OMEGA_IC3_VEC_FCT(ALPHA,BETA,GAMMA,ALPHA_DOT,BETA_DOT,GAMMA_DOT) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 02-Jan-2019 11:17:36 7 | 8 | t2 = sin(alpha); 9 | t3 = cos(alpha); 10 | t4 = cos(beta); 11 | C3_omega_IC3_vec = [alpha_dot-gamma_dot.*sin(beta);beta_dot.*t3+gamma_dot.*t2.*t4;-beta_dot.*t2+gamma_dot.*t3.*t4]; 12 | -------------------------------------------------------------------------------- /Exercises/Exercise_II/Solutions/Solution_10.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_II/Solutions/Solution_10.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_II/Solutions/Solution_19_InertiaMatrix.m: -------------------------------------------------------------------------------- 1 | clear all; close all; clc; 2 | 3 | % Inertia matrix for cuboid at the COG 4 | Icub_G = @(dim,m) [dim(2)^2+dim(3)^2 0 0; 0 dim(1)^2+dim(3)^2 0; 0 0 dim(1)^2+dim(2)^2]*m/12; 5 | 6 | % skew symmetric matrix from vector - test: skew(sym('a',[3,1],'real')) 7 | % skew = @(r) [0 -r(3) r(2); r(3) 0 -r(1); -r(2) r(1) 0]; 8 | % parallel axes theorem 9 | % translate_I = @(I_G,r_BG,m) I_G + m * skew(r_BG)'*skew(r_BG); 10 | % rotate_I = @(B_I_G,A_BC) A_BC' * B_I_G * A_BC; 11 | 12 | 13 | syms m real 14 | dim1 = [4,2,4]'; 15 | dim2 = [1,1,4]'; 16 | r_BG = [-1.5 1.5 0]'; 17 | 18 | 19 | I = Icub_G(dim1,m) + 2 * translate_I(Icub_G(dim2,m),r_BG,m) 20 | 21 | 22 | 23 | function Sr = skew (r) 24 | % skew symmetric matrix from vector - test: skew(sym('a',[3,1],'real')) 25 | Sr = [ 0 -r(3) r(2); 26 | r(3) 0 -r(1); 27 | -r(2) r(1) 0 ]; 28 | end 29 | 30 | function C_I_G = rotate_I (B_I_G, A_BC) 31 | % change of reference frame of the Inertia matrix 32 | C_I_G = A_BC' * B_I_G * A_BC; 33 | end 34 | 35 | function C_I_B = translate_I (C_I_G, C_r_BG, m) 36 | % parallel axes theorem 37 | C_I_B = C_I_G + m * skew(C_r_BG)'*skew(C_r_BG); 38 | end -------------------------------------------------------------------------------- /Exercises/Exercise_II/Solutions/Solution_6_EulerAngles.m: -------------------------------------------------------------------------------- 1 | %% Init everything 2 | % Clear memory, command window, and figures 3 | clear all 4 | close all 5 | clc 6 | % Some RGB values: 7 | red = [1;0;0]; 8 | green = [0;1;0]; 9 | blue = [0;0;1]; 10 | black = [0;0;0]; 11 | grey = [0.3;0.3;0.3]; 12 | mag = [1;0;1]; 13 | yel = [1;1;0]; 14 | 15 | %% Composing rotations: 16 | Env = EnvironmentCLASS(); 17 | 18 | % Coordinate system I at the same location as the graphics co-sys: 19 | I = CoSysCLASS(Env, eye(3)); 20 | I.color = green; 21 | I.name = 'I'; 22 | 23 | % Create a coordinate system that is rotated by 30 deg about the 3-axis: 24 | gamma = 30*pi/180; 25 | A_IC1 =[cos(gamma), -sin(gamma), 0; 26 | sin(gamma), cos(gamma), 0; 27 | 0, 0, 1]; 28 | C1 = CoSysCLASS(Env, A_IC1); 29 | C1.color = red; 30 | C1.name = 'C1'; 31 | 32 | %% Create a coordinate system that is rotated by 45 deg about the 2-axis: 33 | beta = 45*pi/180; 34 | A_C1C2 =[+cos(beta), 0, +sin(beta); 35 | 0 , 1, 0; 36 | -sin(beta), 0, +cos(beta)]; 37 | 38 | A_IC2 = A_IC1 * A_C1C2; 39 | C2 = CoSysCLASS(Env,A_IC2); 40 | C2.color = blue; 41 | C2.name = 'C2'; 42 | 43 | 44 | %% Create a coordinate system that is rotated by 60 deg about the 1-axis: 45 | alpha = 60*pi/180; 46 | A_C2C3 =[1, 0 , 0; 47 | 0, +cos(alpha), -sin(alpha); 48 | 0, +sin(alpha), +cos(alpha)]; 49 | 50 | A_IC3 = A_IC1 * A_C1C2 * A_C2C3; 51 | C3 = CoSysCLASS(Env,A_IC3); 52 | C3.color = mag; 53 | C3.name = 'C3'; 54 | 55 | %% Create a vector v 56 | C3_v = [1;1;1]; 57 | v = VectorCLASS(Env, C3, C3_v); 58 | v.color = black; 59 | v.name = 'v'; 60 | 61 | %% Compute numerical results (via transformations and built in functions): 62 | % in I: 63 | disp(A_IC1 * A_C1C2 * A_C2C3 * C3_v) 64 | disp(v.getCoords(I)) 65 | % in C1: 66 | disp(A_C1C2 * A_C2C3 * C3_v) 67 | disp(v.getCoords(C1)) 68 | % in C2: 69 | disp(A_C2C3 * C3_v) 70 | disp(v.getCoords(C2)) 71 | % in C3: 72 | disp(C3_v) 73 | disp(v.getCoords(C3)) 74 | 75 | print(gcf,'-r600','-djpeg','Problem_6_Output.jpg'); 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /Exercises/Exercise_II/Solutions/Solution_8.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_II/Solutions/Solution_8.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_II/Solutions/Solution_8a_IntegratingAngularVelocities.m: -------------------------------------------------------------------------------- 1 | %% Init everything 2 | % Clear memory, command window, and figures 3 | clear all 4 | close all 5 | clc 6 | % Some RGB values: 7 | red = [1;0;0]; 8 | green = [0;1;0]; 9 | blue = [0;0;1]; 10 | black = [0;0;0]; 11 | grey = [0.3;0.3;0.3]; 12 | mag = [1;0;1]; 13 | yel = [1;1;0]; 14 | 15 | %% Create an animation in which the angular velocity is constant and 16 | %% represents a rotation about the direction of [1;1;1]) with unit velocity 17 | %% i.e.: I_omega_IC_vec = sqrt(1/3)*[1;1;1]. 18 | % 19 | % Possibility one (integrating the direction cosine): 20 | % 21 | % Compute A_IC_dot from the relationships: 22 | % C_omega_IC = A_CI*I_omega_IC*A_IC 23 | % A_IC_dot = A_IC * C_omega_IC 24 | % 25 | % A_IC_dot = I_omega_IC * A_IC 26 | % 27 | n = 100; 28 | delta_t = 2*pi/n; 29 | % Initialize the rotation matrix 30 | gamma = pi/2; 31 | A_IC = [+cos(gamma), -sin(gamma), 0; 32 | +sin(gamma), +cos(gamma), 0; 33 | 0 , 0 , 1]; 34 | % Create a 'physical' (graphical) environment: 35 | Env = EnvironmentCLASS(); 36 | % Inertial frame: 37 | I = CoSysCLASS(Env,eye(3)); 38 | I.color = green; 39 | I.name = 'I'; 40 | % Rotated frame: 41 | C = CoSysCLASS(Env,A_IC); 42 | C.color = red; 43 | C.name = 'C'; 44 | % Define rotation velocity (as vector) 45 | I_omega_IC_vec = sqrt(1/3)*[1;1;1]; 46 | I_omega_IC = [0,-I_omega_IC_vec(3),+I_omega_IC_vec(2); 47 | +I_omega_IC_vec(3),0,-I_omega_IC_vec(1); 48 | -I_omega_IC_vec(2),+I_omega_IC_vec(1),0]; 49 | % Show angular velocity vector: 50 | v_omega = VectorCLASS(Env, I, I_omega_IC_vec); 51 | v_omega.color = yel; 52 | % Animate: 53 | for i = 1:n 54 | % Compute derivative: 55 | A_IC_dot = I_omega_IC * A_IC; 56 | % Integration: 57 | A_IC = A_IC + A_IC_dot*delta_t; 58 | C.A_IC = A_IC; 59 | drawnow(); 60 | end 61 | print(gcf,'-r600','-djpeg','Problem_8a_Output.jpg'); 62 | -------------------------------------------------------------------------------- /Exercises/Exercise_II/Solutions/Solution_8c_IntegratingAngularVelocities.m: -------------------------------------------------------------------------------- 1 | %% Init everything 2 | % Clear memory, command window, and figures 3 | clear all 4 | close all 5 | clc 6 | % Some RGB values: 7 | red = [1;0;0]; 8 | green = [0;1;0]; 9 | blue = [0;0;1]; 10 | black = [0;0;0]; 11 | grey = [0.3;0.3;0.3]; 12 | mag = [1;0;1]; 13 | yel = [1;1;0]; 14 | 15 | %% Create an animation in which the angular velocity is constant and 16 | %% represents a rotation about the direction of [1;1;1]) with unit velocity 17 | %% i.e.: I_omega_IC_vec = sqrt(1/3)*[1;1;1]. 18 | % 19 | % Possibility one (integrating the direction cosine): 20 | % 21 | % Compute A_IC_dot from the relationships: 22 | % C_omega_IC = A_CI*I_omega_IC*A_IC 23 | % A_IC_dot = A_IC * C_omega_IC 24 | % 25 | % A_IC_dot = I_omega_IC * A_IC 26 | % 27 | n = 100; 28 | delta_t = 2*pi/n; 29 | % Initialize the rotation matrix 30 | gamma = pi/2; 31 | A_IC = [+cos(gamma), -sin(gamma), 0; 32 | +sin(gamma), +cos(gamma), 0; 33 | 0 , 0 , 1]; 34 | % Create a 'physical' (graphical) environment: 35 | Env = EnvironmentCLASS(); 36 | % Inertial frame: 37 | I = CoSysCLASS(Env,eye(3)); 38 | I.color = green; 39 | I.name = 'I'; 40 | % Rotated frame: 41 | C = CoSysCLASS(Env,A_IC); 42 | C.color = red; 43 | C.name = 'C'; 44 | % Define rotation velocity (as vector) 45 | I_omega_IC_vec = sqrt(1/3)*[1;1;1]; 46 | I_omega_IC = [0,-I_omega_IC_vec(3),+I_omega_IC_vec(2); 47 | +I_omega_IC_vec(3),0,-I_omega_IC_vec(1); 48 | -I_omega_IC_vec(2),+I_omega_IC_vec(1),0]; 49 | % Show angular velocity vector: 50 | v_omega = VectorCLASS(Env, I, I_omega_IC_vec); 51 | v_omega.color = yel; 52 | % Animate: 53 | for i = 1:n 54 | % Compute derivative (unnecessary): 55 | A_IC_dot = I_omega_IC * A_IC; 56 | % Integration (exact): 57 | A_IC = expm(delta_t* I_omega_IC)*A_IC; 58 | C.A_IC = A_IC; 59 | drawnow(); 60 | end 61 | print(gcf,'-r600','-djpeg','Problem_8c_Output.jpg'); 62 | -------------------------------------------------------------------------------- /Exercises/Exercise_II/Templates/Template_8a_IntegratingAngularVelocities.m: -------------------------------------------------------------------------------- 1 | %% Init everything 2 | % Clear memory, command window, and figures 3 | clear all 4 | close all 5 | clc 6 | % Some RGB values: 7 | red = [1;0;0]; 8 | green = [0;1;0]; 9 | blue = [0;0;1]; 10 | black = [0;0;0]; 11 | grey = [0.3;0.3;0.3]; 12 | mag = [1;0;1]; 13 | yel = [1;1;0]; 14 | 15 | %% Create an animation in which the angular velocity is constant and 16 | %% represents a rotation about the direction of [1;1;1]) with unit velocity 17 | %% i.e.: I_omega_IC_vec = sqrt(1/3)*[1;1;1]. 18 | % 19 | % Possibility one (integrating the direction cosine): 20 | % 21 | % Compute A_IC_dot from the relationships: 22 | % C_omega_IC = A_CI*I_omega_IC*A_IC 23 | % A_IC_dot = A_IC * C_omega_IC 24 | % 25 | % A_IC_dot = I_omega_IC * A_IC 26 | % 27 | n = 100; 28 | delta_t = 2*pi/n; 29 | % Initialize the rotation matrix 30 | gamma = pi/2; 31 | A_IC = [+cos(gamma), -sin(gamma), 0; 32 | +sin(gamma), +cos(gamma), 0; 33 | 0 , 0 , 1]; 34 | % Create a 'physical' (graphical) environment: 35 | Env = EnvironmentCLASS(); 36 | % Inertial frame: 37 | I = CoSysCLASS(Env,eye(3)); 38 | I.color = green; 39 | I.name = 'I'; 40 | % Rotated frame: 41 | C = CoSysCLASS(Env,A_IC); 42 | C.color = red; 43 | C.name = 'C'; 44 | % Define rotation velocity (as vector) 45 | I_omega_IC_vec = sqrt(1/3)*[1;1;1]; 46 | % ************************************************************************* 47 | % ToDo: 48 | % Complete the statement to compute the matrix omega-tilde to learn how to 49 | % implement a skew symmetric matrix: 50 | % ************************************************************************* 51 | I_omega_IC = ; 52 | 53 | % Show angular velocity vector: 54 | v_omega = VectorCLASS(Env, I, I_omega_IC_vec); 55 | v_omega.color = yel; 56 | % Animate: 57 | for i = 1:n 58 | % Compute derivative: 59 | % ************************************************************************* 60 | % ToDo: 61 | % Complete the statement to compute the time-derivative of A_IC, to learn 62 | % how to implement derivatives of transformations: 63 | % ************************************************************************* 64 | A_IC_dot = ; 65 | % Integration: 66 | A_IC = A_IC + A_IC_dot*delta_t; 67 | C.A_IC = A_IC; 68 | drawnow(); 69 | end 70 | print(gcf,'-r600','-djpeg','Problem_8a_Output.jpg'); 71 | -------------------------------------------------------------------------------- /Exercises/Exercise_II/TestFiles/TestFile_9_BetterCoSysCLASS.m: -------------------------------------------------------------------------------- 1 | %% Init everything 2 | % Clear memory, command window, and figures 3 | clear all 4 | close all 5 | clc 6 | % Some RGB values: 7 | red = [1;0;0]; 8 | green = [0;1;0]; 9 | blue = [0;0;1]; 10 | black = [0;0;0]; 11 | grey = [0.3;0.3;0.3]; 12 | mag = [1;0;1]; 13 | yel = [1;1;0]; 14 | 15 | %% Composing rotations: 16 | Env = EnvironmentCLASS(); 17 | 18 | % Coordinate system I at the same location as the graphics co-sys: 19 | I = BetterCoSysCLASS(Env, eye(3)); 20 | I.color = green; 21 | I.name = 'I'; 22 | 23 | % Create a coordinate system that is rotated by 30 deg about the 3-axis: 24 | gamma = 30*pi/180; 25 | A_IC1 =[cos(gamma), -sin(gamma), 0; 26 | sin(gamma), cos(gamma), 0; 27 | 0, 0, 1]; 28 | C1 = BetterCoSysCLASS(Env, A_IC1, I); 29 | C1.color = red; 30 | C1.name = 'C1'; 31 | 32 | % Create a coordinate system that is rotated by 45 deg about the 2-axis: 33 | beta = 45*pi/180; 34 | A_C1C2 =[+cos(beta), 0, +sin(beta); 35 | 0 , 1, 0; 36 | -sin(beta), 0, +cos(beta)]; 37 | 38 | C2 = BetterCoSysCLASS(Env, A_C1C2, C1); 39 | C2.color = blue; 40 | C2.name = 'C2'; 41 | 42 | % Create a coordinate system that is rotated by 60 deg about the 1-axis: 43 | alpha = 60*pi/180; 44 | A_C2C3 =[1, 0 , 0; 45 | 0, +cos(alpha), -sin(alpha); 46 | 0, +sin(alpha), +cos(alpha)]; 47 | 48 | C3 = BetterCoSysCLASS(Env, A_C2C3, C2); 49 | C3.color = mag; 50 | C3.name = 'C3'; 51 | 52 | % Create a vector v 53 | C3_v = [1;1;1]; 54 | v = VectorCLASS(Env, C3, C3_v); 55 | v.color = black; 56 | v.name = 'v'; 57 | 58 | % Compute numerical results (via transformations and built in functions): 59 | % in I: 60 | disp(A_IC1 * A_C1C2 * A_C2C3 * C3_v) 61 | disp(v.getCoords(I)) 62 | % in C1: 63 | disp(A_C1C2 * A_C2C3 * C3_v) 64 | disp(v.getCoords(C1)) 65 | % in C2: 66 | disp(A_C2C3 * C3_v) 67 | disp(v.getCoords(C2)) 68 | % in C3: 69 | disp(C3_v) 70 | disp(v.getCoords(C3)) 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /Exercises/Exercise_III/Problem_12_Output.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Problem_12_Output.jpg -------------------------------------------------------------------------------- /Exercises/Exercise_III/Problem_12b_Output.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Problem_12b_Output.jpg -------------------------------------------------------------------------------- /Exercises/Exercise_III/Problem_14_Output.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Problem_14_Output.jpg -------------------------------------------------------------------------------- /Exercises/Exercise_III/Problem_15_Output.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Problem_15_Output.jpg -------------------------------------------------------------------------------- /Exercises/Exercise_III/Solutions/Solution_12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Solutions/Solution_12.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_III/Solutions/Solution_13.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Solutions/Solution_13.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_III/Solutions/Solution_15.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Solutions/Solution_15.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_III/Solutions/Solution_16.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Solutions/Solution_16.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_III/Solutions/Solution_18.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Solutions/Solution_18.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_III/Solutions/Solution_19.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Solutions/Solution_19.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_III/Solutions/Solution_20.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Solutions/Solution_20.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_III/Solutions/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Solutions/Thumbs.db -------------------------------------------------------------------------------- /Exercises/Exercise_III/TestFiles/Problem_12_Output.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/TestFiles/Problem_12_Output.jpg -------------------------------------------------------------------------------- /Exercises/Exercise_III/TestFiles/Problem_15_Output.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/TestFiles/Problem_15_Output.jpg -------------------------------------------------------------------------------- /Exercises/Exercise_III/TestFiles/TestFile_11_BoundVectorAndCoSys.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | red = [1;0;0]; 5 | green = [0;1;0]; 6 | blue = [0;0;1]; 7 | 8 | % Create a 'physical' (graphical) environment: 9 | env = EnvironmentCLASS(); 10 | 11 | % Create an inertial reference frame: 12 | I = CoSysCLASS(env, eye(3)); 13 | I.color = green; 14 | I.name = 'I'; 15 | 16 | % Create a bound coordinate system with some initial values: 17 | C = BoundCoSysCLASS(env, eye(3), zeros(3,1)); 18 | C.color = red; 19 | C.name = 'C'; 20 | % Define transformation as a simple rotation about the 3-axis, and update 21 | % the bound CoSys accordingly: 22 | gamma = 30*pi/180; 23 | A_IC = [+cos(gamma), -sin(gamma), 0; 24 | +sin(gamma), +cos(gamma), 0; 25 | 0 , 0 , 1]; 26 | C.A_IC = A_IC; 27 | % Do the same for the offset: 28 | I_r_IC = [1;2;0]; 29 | C.I_r_IC = I_r_IC; 30 | 31 | % Create a bound vector system with some initial values: 32 | v = BoundVectorCLASS(env, C, zeros(3,1), zeros(3,1)); 33 | v.color = blue; 34 | v.name = 'vector'; 35 | % Redefine the vector and the offset: 36 | C_v = [0; -2; 0]; 37 | C_r_CO = [1;0;0]; 38 | v.setCoords(C, C_v, C_r_CO); 39 | 40 | % Calling resetOutput, adjusts the scaling of the graphical output window, 41 | % such that both coordinate systems are visible. 42 | env.resetOutput(); 43 | 44 | % Save screen shot: 45 | print(gcf,'-r600','-djpeg','Problem_11_Output.jpg'); 46 | -------------------------------------------------------------------------------- /Exercises/Exercise_III/TestFiles/TestFile_14_VisualizingTwists.m: -------------------------------------------------------------------------------- 1 | %% INITIALIZE: 2 | clear all 3 | close all 4 | clc 5 | green = [0;1;0]; 6 | 7 | % Create a 'physical' (graphical) environment: 8 | env = EnvironmentCLASS(); 9 | 10 | % Create an inertial reference frame: 11 | I = CoSysCLASS(env, eye(3)); 12 | I.color = green; 13 | I.name = 'I'; 14 | 15 | % Create a body, and fill in some arbitrary values to visualize the 16 | % different kinematic values: 17 | B = RigidBodyKinematicsCLASS(env); 18 | B.bodyName = 'B'; 19 | B.A_IB = eye(3); 20 | B.B_omega_B = [ 1; 1; 0]; 21 | B.B_omegaDot_B = [ 0; 1; 1]; 22 | B.B_r_IB = [ 1; 1; 1]; 23 | B.B_v_B = [-1; 0; 0]; 24 | B.B_a_B = [ 0;-1; 0]; 25 | % Resize the graphics window to show all components 26 | env.resetOutput(); 27 | 28 | %% Problem 14: 29 | % Define initial conditions: 30 | B.A_IB = eye(3); 31 | B.B_omega_B = [ 0; 1.5; 1.5]; 32 | B.B_omegaDot_B = [ 0; 0; 0]; 33 | B.B_r_IB = [ 0; 0; 0]; 34 | B.B_v_B = [ 0; 0; 0]; 35 | B.B_a_B = [ 1.5; 1.5; 0]; 36 | 37 | % Set up integration 38 | delta_t = 0.01; % Stepsize of integrator 39 | delta_t_show = 0.01; % Stepsize of output 40 | t_start = 0; 41 | t_end = 1; 42 | 43 | % Perform integration: 44 | t = t_start; 45 | next_t_output = t_start; 46 | while t next_t_output 57 | B.updateGraphics(); 58 | env.resetOutput(); 59 | drawnow() 60 | next_t_output = next_t_output + delta_t_show; 61 | end 62 | end 63 | 64 | % Save final screen shot: 65 | B.updateGraphics(); 66 | env.resetOutput(); 67 | print(gcf,'-r600','-djpeg','Problem_14_Output.jpg'); -------------------------------------------------------------------------------- /Exercises/Exercise_III/TestFiles/TestFile_15_RigidBodyDynamics.m: -------------------------------------------------------------------------------- 1 | %% INITIALIZE: 2 | clear all 3 | close all 4 | clc 5 | green = [0;1;0]; 6 | 7 | % Create a 'physical' (graphical) environment: 8 | env = EnvironmentCLASS(); 9 | 10 | % Create an inertial reference frame: 11 | I = CoSysCLASS(env, eye(3)); 12 | I.color = green; 13 | I.name = 'I'; 14 | 15 | % Create a body, and fill in some arbitrary values to visualize the 16 | % different kinematic and dynamic values: 17 | B = RigidBodyDynamicsCLASS(env); 18 | B.bodyName = 'B'; 19 | B.A_IB = eye(3); 20 | B.B_omega_B = [ .01; 0.01; 1]; 21 | B.B_omegaDot_B = [ 0; 1; 1]; 22 | B.B_r_IB = [ 1; 1; 1]; 23 | B.B_v_B = [0; 0; 0]; 24 | B.B_a_B = [ 0;-1; 0]; 25 | % Define mass and inertia for this rigid body: 26 | B.m_B = 1; % [Kg] 27 | B.B_I_B = diag([1.5,2,2.5]); % [Kg*m^2] 28 | % Resize the graphics window to show all components 29 | env.resetOutput(); 30 | 31 | %% QUESTION 15a: 32 | % Define initial conditions for position/orientation and velocities 33 | B.A_IB = eye(3); 34 | B.B_omega_B = [ 1; 1; 1]; 35 | B.B_r_IB = [ 0; 0; 0]; 36 | B.B_v_B = [ 0; 0; 0]; 37 | 38 | % Set the auto updating of graphics to false, so the simulation runs a bit 39 | % faster: 40 | B.autoUpdate = false; 41 | 42 | % Set up integration 43 | delta_t = 0.001; % Stepsize of integrator 44 | delta_t_show = 0.1; % Stepsize of output 45 | t_start = 0; 46 | t_end = 30; 47 | 48 | % Perform integration: 49 | t = t_start; 50 | next_t_output = t_start; 51 | while t next_t_output 64 | B.updateGraphics(); 65 | env.resetOutput(); 66 | drawnow() 67 | next_t_output = next_t_output + delta_t_show; 68 | end 69 | end 70 | 71 | % Display the orientation and position at the end of the run for comparison 72 | disp(B.A_IB); 73 | disp(B.B_r_IB); 74 | 75 | % Save final screen shot: 76 | B.updateGraphics(); 77 | env.resetOutput(); 78 | print(gcf,'-r600','-djpeg','Problem_15_Output.jpg'); -------------------------------------------------------------------------------- /Exercises/Exercise_III/TestFiles/TestFile_17_ShowInertia.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | Env = EnvironmentCLASS(); 5 | CoSysCLASS(Env,eye(3)); 6 | m = 10; 7 | % I = [ 1.0, 0.0, 0.0; 8 | % 0.0, 1.0, 0.0; 9 | % 0.0, 0.0, 1.0]; 10 | % I = [ 1.5, 0.0, 0.0; 11 | % 0.0, 2.0, 0.0; 12 | % 0.0, 0.0, 2.5]; 13 | I = [ 1.56,-0.16, 0.23; 14 | -0.16, 1.95,-0.50; 15 | 0.23,-0.50, 2.50]; 16 | % I = [ 1.0, 0.0, 0.0; 17 | % 0.0, 1.0, 0.0; 18 | % 0.0, 0.0, 3.0]; 19 | % 20 | 21 | ShowInertia(I,m,1); % Six points 22 | ShowInertia(I,m,2); % Four points 23 | ShowInertia(I,m,3); % Ellipsoid 24 | ShowInertia(I,m,4); % Cuboid 25 | 26 | print(gcf,'-r600','-djpeg','Problem_17_Output.jpg','-opengl'); -------------------------------------------------------------------------------- /Exercises/Exercise_III/TestFiles/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/TestFiles/Thumbs.db -------------------------------------------------------------------------------- /Exercises/Exercise_III/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_III/Thumbs.db -------------------------------------------------------------------------------- /Exercises/Exercise_IV/ClassFiles/LinkedJointObjectCLASS.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedJointObjectCLASS < handle 2 | % 3 | % Defines an abstract joint object that is part of a linked list of bodies 4 | % and joints. 5 | % 6 | % Public Methods: 7 | % joint = LinkedJointObjectCLASS(predBody, sucBody) 8 | % Creates an abstract joint object that links the body 9 | % specified in 'predBody' with the body specified in 'sucBody'. 10 | % Both are objects of the type 'LinkedBodyObjectCLASS', 11 | % and function as predecessor and successor in a kinematic tree. 12 | % joint.delete() 13 | % Removes the joint from the graphics output and memory 14 | % joint.recursiveOutput(spaceString) 15 | % This function recursively displays the object properties in 16 | % the matlab command window. Then calls the 'recursiveOutput' 17 | % routine of the successor body. Display starts with an 18 | % identation of 'spaceString'. 19 | % sucBody = joint.getSuccessorBody() 20 | % Returns the successor of this joint. Can be used to write 21 | % recursive algorithms that simply jump over joints. 22 | % 23 | % Public Properties: 24 | % description % A string describing the joint 25 | % 26 | % C. David Remy remy@inm.uni-stuttgart.de 27 | % Matlab R2018 28 | % 12/21/2018 29 | % v23 30 | % 31 | classdef LinkedJointObjectCLASS < handle 32 | % Public Properties 33 | properties 34 | description = ''; % A string describing the joint 35 | end 36 | % Private Properties 37 | properties (SetAccess = private, GetAccess = private) 38 | predBody; % This is the predecessor of the joint in the kinematic 39 | % tree 40 | sucBody; % This is the successor of the joint in the kinematic 41 | % tree 42 | end 43 | % Public Methods 44 | methods 45 | % Constructor 46 | function obj = LinkedJointObjectCLASS(predBody, sucBody) 47 | % Store the information about the predecessor and successor of 48 | % this joint: 49 | obj.predBody = predBody; 50 | obj.sucBody = sucBody; 51 | % Now link this body outward and inward, by registering it 52 | % with the parent and child bodies: 53 | sucBody.setParentJoint(obj); 54 | predBody.addChildJoint(obj); 55 | end 56 | % Destructor (empty) 57 | function delete(obj) 58 | end 59 | function recursiveOutput(obj, spaceString) 60 | disp([spaceString,'###### BEGIN JOINT #####']); 61 | % Display some information about this joint: 62 | disp([spaceString, '" ',obj.description,' "']); 63 | % recursively call the successor body and add three spaces so 64 | % we get a nice indentation: 65 | obj.sucBody.recursiveOutput([spaceString,' ']); 66 | disp([spaceString,'###### END JOINT #######']); 67 | end 68 | % This function allows accessing the private property sucBody: 69 | function sucBody = getSuccessorBody(obj) 70 | sucBody = obj.sucBody; 71 | end 72 | end 73 | % Private Methods 74 | methods (Access = private) 75 | % - none 76 | end 77 | end 78 | 79 | -------------------------------------------------------------------------------- /Exercises/Exercise_IV/Examples/Example_7_LinkedList.m: -------------------------------------------------------------------------------- 1 | %% INITIALIZE: 2 | clear all 3 | close all 4 | clc 5 | 6 | % Define four body-objects: 7 | ground = LinkedBodyObjectCLASS(); 8 | bodyA = LinkedBodyObjectCLASS(); 9 | bodyB = LinkedBodyObjectCLASS(); 10 | bodyC = LinkedBodyObjectCLASS(); 11 | 12 | % Define three joint-object that connect these bodies: 13 | joint1 = LinkedJointObjectCLASS(ground,bodyA); 14 | joint2 = LinkedJointObjectCLASS(bodyA,bodyB); 15 | joint3 = LinkedJointObjectCLASS(ground,bodyC); 16 | 17 | % Set the date stored in each: 18 | ground.description = 'This is ground.'; 19 | bodyA.description = 'This is Body A.'; 20 | bodyB.description = 'This is Body B.'; 21 | bodyC.description = 'This is Body C.'; 22 | joint1.description = 'This is Joint 1.'; 23 | joint2.description = 'This is Joint 2.'; 24 | joint3.description = 'This is Joint 3.'; 25 | 26 | % Recursively call the output routine of each body: 27 | disp('Information about all the objects in the tree:') 28 | disp(' '); 29 | ground.recursiveOutput(''); 30 | -------------------------------------------------------------------------------- /Exercises/Exercise_IV/Examples/Example_8_SymbolicTriplePendulum.m: -------------------------------------------------------------------------------- 1 | %% INITIALIZE: 2 | clear all 3 | close all 4 | clc 5 | 6 | % Create a 'physical' (graphical) environment: 7 | env = EnvironmentCLASS(); 8 | 9 | % Define symbolic variables for geometrical parameters and joint angles: 10 | syms l real 11 | 12 | 13 | % Define a triple pendulum 14 | % Define the bodies: 15 | ground = LinkedRigidBodyDynamicsCLASS(env); 16 | ground.bodyName = 'Ground'; 17 | ground.autoUpdate = false; 18 | ground.m_B = 1; % Since this is ground, m and I do not matter 19 | ground.B_I_B = eye(3)*0.0001; 20 | ground.scale = 0.5; % Defines how large the CoSys will be drawn 21 | link1 = LinkedRigidBodyDynamicsCLASS(env); 22 | link1.bodyName = 'Link 1'; 23 | link1.autoUpdate = false; 24 | link1.m_B = 10; % m and I just given for visualization 25 | link1.B_I_B = diag([0.0042,0.8354,0.8354]); 26 | link1.scale = l/2; % Defines how large the CoSys will be drawn 27 | link2 = LinkedRigidBodyDynamicsCLASS(env); 28 | link2.bodyName = 'Link 2'; 29 | link2.autoUpdate = false; 30 | link2.m_B = 10; % m and I just given for visualization 31 | link2.B_I_B = diag([0.0042,0.8354,0.8354]); 32 | link2.scale = l/2; % Defines how large the CoSys will be drawn 33 | link3 = LinkedRigidBodyDynamicsCLASS(env); 34 | link3.bodyName = 'Link 3'; 35 | link3.autoUpdate = false; 36 | link3.m_B = 10; % m and I just given for visualization 37 | link3.B_I_B = diag([0.0042,0.8354,0.8354]); 38 | link3.scale = l/2; % Defines how large the CoSys will be drawn 39 | 40 | % Define the joints and the interconnection of the bodies: 41 | joint1 = LinkedRotationalJointCLASS(env, ground, link1); 42 | joint1.P_r_PDp = [0;0;0]; 43 | joint1.A_PDp = [0,1,0;-1,0,0;0,0,1]; 44 | joint1.S_r_SDs = [-l/2;0;0]; 45 | joint1.A_SDs = eye(3); 46 | joint1.jointName = 'Joint 1'; 47 | joint1.autoUpdate = false; 48 | joint1.scale = 0.2; 49 | 50 | joint2 = LinkedRotationalJointCLASS(env, link1, link2); 51 | joint2.P_r_PDp = [+l/2;0;0]; 52 | joint2.A_PDp = eye(3); 53 | joint2.S_r_SDs = [-l/2;0;0]; 54 | joint2.A_SDs = eye(3); 55 | joint2.jointName = 'Joint 2'; 56 | joint2.autoUpdate = false; 57 | joint2.scale = 0.2; 58 | 59 | joint3 = LinkedRotationalJointCLASS(env, link2, link3); 60 | joint3.P_r_PDp = [+l/2;0;0]; 61 | joint3.A_PDp = eye(3); 62 | joint3.S_r_SDs = [-l/2;0;0]; 63 | joint3.A_SDs = eye(3); 64 | joint3.jointName = 'Joint 3'; 65 | joint3.autoUpdate = false; 66 | joint3.scale = 0.2; 67 | 68 | % Define the joint angles as symbolic values: 69 | syms q1 q2 q3 real 70 | joint1.q = q1; 71 | joint2.q = q2; 72 | joint3.q = q3; 73 | 74 | % Compute the forward kinematics starting with a root at position and 75 | % orientation zero: 76 | ground.recursiveForwardKinematics(); 77 | I_r_IB3 = link3.A_IB*link3.B_r_IB; 78 | I_r_IB3 = simplify(I_r_IB3); 79 | disp(I_r_IB3) 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /Exercises/Exercise_IV/Solutions/Problem_25_Output.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_IV/Solutions/Problem_25_Output.jpg -------------------------------------------------------------------------------- /Exercises/Exercise_IV/Solutions/Solution_21.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_IV/Solutions/Solution_21.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_IV/Solutions/Solution_22_RecursiveKinematics.m: -------------------------------------------------------------------------------- 1 | 2 | q = [0.1, -pi/4, 0.2, pi/4]; % joint coordinates 3 | q = [0.1, -pi/4, 0.2, -pi/4, 0.2, -pi/4, 0.2, pi/4, 0.2, pi/4, 0.2, pi/4]; % joint coordinates 4 | d = 0.15; % length of the final link 5 | 6 | % number of rotational joints (points P_k) 7 | n = length(q)/2; 8 | 9 | % Initialize matrix P containing position of and orientation at each of the 10 | % points P_k and the origin. The first row P(1,:)=[0,0,0] is the x 11 | % position, y position, and orientation of the origin. The (k+1)-th row 12 | % P(k+1,:) contains the x position, y position, and orientation at the k-th 13 | % point P_k 14 | P = zeros(n+1,3); 15 | 16 | % iteratively find position and orientation at each point P_k based on 17 | % the position and orientation at point P_k-1, the kth joint angle q(2*k), and the kth translation q(2*k-1) 18 | for k = 1:n 19 | P(k+1,:) = [P(k,1)-q(2*k-1)*sin(P(k,3)), ... % x-position of P_k 20 | P(k,2)+q(2*k-1)*cos(P(k,3)), ... % y-position of P_k 21 | P(k,3)+q(2*k)]; % orientation at P_k 22 | end 23 | 24 | % position and orientation at the end-effector 25 | Pe = [P(end,1)-d*sin(P(end,3)), ... 26 | P(end,2)+d*cos(P(end,3)), ... 27 | P(end,3)]; 28 | disp(Pe); 29 | 30 | 31 | % plot the resulting robotic arm 32 | figure; axis equal; 33 | line([P(:,1);Pe(1)], [P(:,2);Pe(2)]); 34 | 35 | -------------------------------------------------------------------------------- /Exercises/Exercise_IV/Solutions/Solution_22_RecursiveKinematics_RecursiveFunctionCall.m: -------------------------------------------------------------------------------- 1 | function Solution_22_RecursiveKinematics_RecursiveFunctionCall 2 | 3 | q = [0.1, -pi/4, 0.2, pi/4]; % joint coordinates 4 | q = [0.1, -pi/4, 0.2, -pi/4, 0.2, -pi/4, 0.2, pi/4, 0.2, pi/4, 0.2, pi/4]; % joint coordinates 5 | d = 0.15; % length of the final link 6 | 7 | % number of rotational joints (points P_k) 8 | n = length(q)/2; 9 | 10 | % extend the coordinates vector q with the length (d) and relative 11 | % orientation (0) of the end-effector 12 | q = [q, d, 0]; 13 | 14 | % position and orientation at the end-effector 15 | Pe = RecursiveJoint(q, n+1); 16 | disp(Pe); 17 | end 18 | 19 | 20 | function P = RecursiveJoint(q, k) 21 | if k == 0 22 | P = [0, 0, 0]; % origin 23 | else 24 | P_prev = RecursiveJoint(q, k-1); % position and orientation at the previous joint 25 | P = [P_prev(1)-q(2*k-1)*sin(P_prev(3)), ... % x-position of P 26 | P_prev(2)+q(2*k-1)*cos(P_prev(3)), ... % y-position of P 27 | P_prev(3)+q(2*k)]; % orientation at P 28 | end 29 | end 30 | -------------------------------------------------------------------------------- /Exercises/Exercise_IV/Solutions/Solution_26.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_IV/Solutions/Solution_26.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_IV/Solutions/Solution_27.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_IV/Solutions/Solution_27.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_IV/Solutions/Solution_28.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_IV/Solutions/Solution_28.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_IV/Solutions/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_IV/Solutions/Thumbs.db -------------------------------------------------------------------------------- /Exercises/Exercise_IV/Templates/Template_22_RecursiveKinematics.m: -------------------------------------------------------------------------------- 1 | 2 | q = [0.1, -pi/4, 0.2, pi/4]; % joint coordinates 3 | q = [0.1, -pi/4, 0.2, -pi/4, 0.2, -pi/4, 0.2, pi/4, 0.2, pi/4, 0.2, pi/4]; % joint coordinates 4 | d = 0.15; % length of the final link 5 | 6 | % number of rotational joints (points P_k) 7 | n = length(q)/2; 8 | 9 | % Initialize matrix P containing position of and orientation at each of the 10 | % points P_k and the origin. The first row P(1,:)=[0,0,0] is the x 11 | % position, y position, and orientation of the origin. The (k+1)-th row 12 | % P(k+1,:) contains the x position, y position, and orientation at the k-th 13 | % point P_k 14 | P = zeros(n+1,3); 15 | 16 | % iteratively find position and orientation at each point P_k based on 17 | % the position and orientation at point P_k-1, the kth joint angle q(2*k), and the kth translation q(2*k-1) 18 | for k = 1:n 19 | % ************************************************************************* 20 | % ToDo: 21 | % Complete the code to iteratively compute the position/orientation of 22 | % element k+1 to write a first recurive kinematics function: 23 | % ************************************************************************* 24 | P(k+1,:) = ; 25 | end 26 | 27 | % position and orientation at the end-effector 28 | % ************************************************************************* 29 | % ToDo: 30 | % Complete the code to update the position and orientation of the 31 | % end-effector to better understand that there might be different types of 32 | % joints/links in a kinematic tree: 33 | % ************************************************************************* 34 | Pe = ; 35 | disp(Pe); 36 | 37 | % plot the resulting robotic arm 38 | figure; axis equal; 39 | line([P(:,1);Pe(1)], [P(:,2);Pe(2)]); 40 | 41 | -------------------------------------------------------------------------------- /Exercises/Exercise_IV/TestFiles/TestData_24_TriplePendulumSim.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_IV/TestFiles/TestData_24_TriplePendulumSim.mat -------------------------------------------------------------------------------- /Exercises/Exercise_IV/TestFiles/TestFile_24_RotationalJoint.m: -------------------------------------------------------------------------------- 1 | %% INITIALIZE: 2 | clear all 3 | close all 4 | clc 5 | 6 | % Create a 'physical' (graphical) environment: 7 | env = EnvironmentCLASS(); 8 | 9 | 10 | % Define a triple pendulum 11 | % Define the bodies: 12 | ground = LinkedRigidBodyDynamicsCLASS(env); 13 | ground.bodyName = 'Ground'; 14 | ground.autoUpdate = false; 15 | ground.m_B = 1; % Since this is ground, m and I do not matter 16 | ground.B_I_B = eye(3)*0.0001; 17 | ground.scale = 0.5; % Defines how large the CoSys will be drawn 18 | link1 = LinkedRigidBodyDynamicsCLASS(env); 19 | link1.bodyName = 'Link 1'; 20 | link1.autoUpdate = false; 21 | link1.m_B = 10; % m and I just given for visualization 22 | link1.B_I_B = diag([0.0042,0.8354,0.8354]); 23 | link1.scale = 0.5; % Defines how large the CoSys will be drawn 24 | link2 = LinkedRigidBodyDynamicsCLASS(env); 25 | link2.bodyName = 'Link 2'; 26 | link2.autoUpdate = false; 27 | link2.m_B = 10; % m and I just given for visualization 28 | link2.B_I_B = diag([0.0042,0.8354,0.8354]); 29 | link2.scale = 0.5; % Defines how large the CoSys will be drawn 30 | link3 = LinkedRigidBodyDynamicsCLASS(env); 31 | link3.bodyName = 'Link 3'; 32 | link3.autoUpdate = false; 33 | link3.m_B = 10; % m and I just given for visualization 34 | link3.B_I_B = diag([0.0042,0.8354,0.8354]); 35 | link3.scale = 0.5; % Defines how large the CoSys will be drawn 36 | 37 | % Define the joints and the interconnection of the bodies: 38 | joint1 = LinkedRotationalJointCLASS(env, ground, link1); 39 | joint1.P_r_PDp = [0;0;0]; 40 | joint1.A_PDp = [0,1,0;-1,0,0;0,0,1]; 41 | joint1.S_r_SDs = [-0.5;0;0]; 42 | joint1.A_SDs = eye(3); 43 | joint1.jointName = 'Joint 1'; 44 | joint1.autoUpdate = false; 45 | joint1.scale = 0.2; 46 | 47 | joint2 = LinkedRotationalJointCLASS(env, link1, link2); 48 | joint2.P_r_PDp = [+0.5;0;0]; 49 | joint2.A_PDp = eye(3); 50 | joint2.S_r_SDs = [-0.5;0;0]; 51 | joint2.A_SDs = eye(3); 52 | joint2.jointName = 'Joint 2'; 53 | joint2.autoUpdate = false; 54 | joint2.scale = 0.2; 55 | 56 | joint3 = LinkedRotationalJointCLASS(env, link2, link3); 57 | joint3.P_r_PDp = [+0.5;0;0]; 58 | joint3.A_PDp = eye(3); 59 | joint3.S_r_SDs = [-0.5;0;0]; 60 | joint3.A_SDs = eye(3); 61 | joint3.jointName = 'Joint 3'; 62 | joint3.autoUpdate = false; 63 | joint3.scale = 0.2; 64 | 65 | % Show the topology: 66 | ground.recursiveOutput(''); 67 | 68 | % Define the joint angles: 69 | joint1.q = +pi/6; 70 | joint2.q = -pi/4; 71 | joint3.q = +pi/3; 72 | 73 | % Compute the forward kinematics starting with a root at position and 74 | % orientation zero: 75 | ground.recursiveForwardKinematics(); 76 | % Update the graphics 77 | ground.recursiveGraphicsUpdate(); 78 | 79 | % Save final screen shot: 80 | axis([-1,1,-3,3,-3.5,0.5]); 81 | print(gcf,'-r600','-djpeg','Problem_24_Output.jpg'); 82 | 83 | 84 | 85 | %% Animation: 86 | pause(3); 87 | data = load('TestData_24_TriplePendulumSim.mat'); 88 | 89 | delta_t_show = 0.05; 90 | next_t_output = data.t(1); 91 | 92 | while next_t_output < data.t(end) 93 | ind = find( data.t>=next_t_output, 1); 94 | 95 | % Update the joint angles: 96 | joint1.q = pi/2 + data.q1(ind); 97 | joint2.q = data.q2(ind); 98 | joint3.q = data.q3(ind); 99 | 100 | % Compute the forward kinematics starting with a root at position and 101 | % orientation zero: 102 | ground.recursiveForwardKinematics(); 103 | % Update the graphics 104 | ground.recursiveGraphicsUpdate(); 105 | drawnow; 106 | 107 | next_t_output = next_t_output + delta_t_show; 108 | end 109 | 110 | -------------------------------------------------------------------------------- /Exercises/Exercise_IV/TestFiles/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_IV/TestFiles/Thumbs.db -------------------------------------------------------------------------------- /Exercises/Exercise_V/ClassFiles/LinkedJointObjectCLASS_v2.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedJointObjectCLASS_v2 < handle 2 | % 3 | % Defines an abstract joint object that is part of a linked list of bodies 4 | % and joints. 5 | % 6 | % Public Methods: 7 | % joint = LinkedJointObjectCLASS_v2(predBody, sucBody) 8 | % Creates an abstract joint object that links the body 9 | % specified in 'predBody' with the body specified in 'sucBody'. 10 | % Both are objects of the type 'LinkedBodyObjectCLASS', 11 | % and function as predecessor and successor in a kinematic tree. 12 | % joint.delete() 13 | % Removes the joint from the graphics output and memory 14 | % joint.recursiveOutput(spaceString) 15 | % This function recursively displays the object properties in 16 | % the matlab command window. Then calls the 'recursiveOutput' 17 | % routine of the successor body. Display starts with an 18 | % identation of 'spaceString'. 19 | % sucBody = joint.getSuccessorBody() 20 | % Returns the successor of this joint. Can be used to write 21 | % recursive algorithms that simply jump over joints. 22 | % 23 | % Public Properties: 24 | % description % A string describing the joint 25 | % 26 | % C. David Remy remy@inm.uni-stuttgart.de 27 | % Matlab R2018 28 | % 12/21/2018 29 | % v23 30 | % 31 | classdef LinkedJointObjectCLASS_v2 < handle 32 | % Public Properties 33 | properties 34 | description = ''; % A string describing the joint 35 | end 36 | % Protected Properties 37 | properties (SetAccess = protected, GetAccess = protected) 38 | predBody; % This is the predecessor of the joint in the kinematic 39 | % tree 40 | sucBody; % This is the successor of the joint in the kinematic 41 | % tree 42 | end 43 | % Public Methods 44 | methods 45 | % Constructor 46 | function obj = LinkedJointObjectCLASS_v2(predBody, sucBody) 47 | % Store the information about the predecessor and successor of 48 | % this joint: 49 | obj.predBody = predBody; 50 | obj.sucBody = sucBody; 51 | % Now link this body outward and inward, by registering it 52 | % with the parent and child bodies: 53 | sucBody.setParentJoint(obj); 54 | predBody.addChildJoint(obj); 55 | end 56 | % Destructor (empty) 57 | function delete(obj) 58 | end 59 | function recursiveOutput(obj, spaceString) 60 | disp([spaceString,'###### BEGIN JOINT #####']); 61 | % Display some information about this joint: 62 | disp([spaceString, '" ',obj.description,' "']); 63 | % recursively call the successor body and add three spaces so 64 | % we get a nice indentation: 65 | obj.sucBody.recursiveOutput([spaceString,' ']); 66 | disp([spaceString,'###### END JOINT #######']); 67 | end 68 | % This function allows accessing the private property sucBody: 69 | function sucBody = getSuccessorBody(obj) 70 | sucBody = obj.sucBody; 71 | end 72 | end 73 | % Protected Methods 74 | methods (Access = protected) 75 | % - none 76 | end 77 | end 78 | 79 | -------------------------------------------------------------------------------- /Exercises/Exercise_V/ClassFiles/LinkedRigidBodyDynamicsCLASS_v2.m: -------------------------------------------------------------------------------- 1 | % LinkedRigidBodyDynamicsCLASS_v2 < RigidBodyDynamicsCLASS_v2 & LinkedBodyObjectCLASS_v2 2 | % 3 | % Defines a rigid body that is represented by all its 4 | % kinematic and dynamic properties and that is part of a linked list. 5 | % 6 | % Public Methods: 7 | % B = LinkedRigidBodyDynamicsCLASS_v2(env) 8 | % Creates a body in the graphical environment 'env'. The body's 9 | % initial position, velocity, and acceleration are set to 10 | % standard values. It is not connected to any joints. 11 | % B.delete() 12 | % Removes the body from the graphics output and memory 13 | % B.recursiveForwardKinematics(obj, B_r_IB, A_IB) 14 | % This function recieves the bodies position and orientation 15 | % from the parent joint, saves it, and passes it on to the child 16 | % joints. 17 | % B.recursiveGraphicsUpdate(obj) 18 | % Calls the internal 'update' function to force an update of all 19 | % graphics objects. Then recursively calls the child joints. 20 | % + All methods inherited from RigidBodyDynamicsCLASS_v2 & 21 | % LinkedBodyObjectCLASS_v2 22 | % 23 | % Public Properties: 24 | % All inherited from RigidBodyDynamicsCLASS_v2 & LinkedBodyObjectCLASS_v2 25 | % 26 | % C. David Remy remy@inm.uni-stuttgart.de 27 | % Matlab R2018 28 | % 12/21/2018 29 | % v22 30 | % 31 | classdef LinkedRigidBodyDynamicsCLASS_v2 < RigidBodyDynamicsCLASS_v2 & LinkedBodyObjectCLASS_v2 32 | % Public Properties 33 | properties 34 | % All inherited 35 | end 36 | % Protected Properties 37 | properties (SetAccess = protected, GetAccess = protected) 38 | % All inherited 39 | end 40 | % Public Methods 41 | methods 42 | % Constructor (calls superclass constructors) 43 | function obj = LinkedRigidBodyDynamicsCLASS_v2(env) 44 | % Superclass constructors must be called explicitly, as we need 45 | % to decide which arguments we pass to each constructor. 46 | % 47 | % Call superclass constructors to create a rigid dynamic body 48 | % and a body object that is part of a linked list: 49 | obj@RigidBodyDynamicsCLASS_v2(env); 50 | obj@LinkedBodyObjectCLASS_v2(); 51 | end 52 | % Destructor (empty) 53 | function delete(obj) 54 | % Superclass desctructor is called automatically 55 | end 56 | function recursiveForwardKinematics(obj, B_r_IB, A_IB) 57 | % Position and orientation, as well as velocities and 58 | % accelerations are given by the parent joint and passed in its 59 | % call of 'recursiveForwardKinematics' 60 | if obj.isRoot == false 61 | obj.A_IB = A_IB; 62 | obj.B_r_IB = B_r_IB; 63 | else 64 | obj.A_IB = eye(3); 65 | obj.B_r_IB = zeros(3,1); 66 | end 67 | 68 | if obj.isLeaf == false 69 | % If this is no leaf, recursively call the child joints: 70 | for i = 1:obj.nChildren 71 | obj.childJoints{i}.recursiveForwardKinematics(obj.B_r_IB, obj.A_IB); 72 | end 73 | end 74 | end 75 | function recursiveGraphicsUpdate(obj) 76 | % Calls the 'updateGraphics()' function to force an update of 77 | % all graphics objects. Then recursively calls the child 78 | % joints. 79 | obj.updateGraphics(); 80 | if obj.isLeaf == false 81 | % If this is no leaf, recursively call the child joints: 82 | for i = 1:obj.nChildren 83 | obj.childJoints{i}.recursiveGraphicsUpdate(); 84 | end 85 | end 86 | end 87 | end 88 | % Protected Methods 89 | methods (Access = protected) 90 | % -none 91 | end 92 | end 93 | 94 | -------------------------------------------------------------------------------- /Exercises/Exercise_V/ClassFiles/LinkedRotationalJointCLASS_v2.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedRotationalJointCLASS_v2 < LinkedGenericJointCLASS_v2 2 | % 3 | % Defines a rotational joint in a kinematic tree that is implemented as a 4 | % set of linked objects. 5 | % 6 | % Public Methods: 7 | % joint = LinkedRotationalJointCLASS_v2(env, predBody, sucBody) 8 | % Creates a rotational joint object that links the body 9 | % specified in 'predBody' with the body specified in 'sucBody'. 10 | % Both are objects of the type 'LinkedRigidBodyDynamicsCLASS_v2', 11 | % and function as predecessor and successor in a kinematic tree. 12 | % The joint is shown in the graphical environment 'env'. 13 | % joint.delete() 14 | % Removes the joint from the graphics output and memory 15 | % + All methods inherited from LinkedGenericJointCLASS_v2 16 | % 17 | % Public Properties: 18 | % All inherited from LinkedGenericJointCLASS_v2 19 | % 20 | % C. David Remy remy@inm.uni-stuttgart.de 21 | % Matlab R2018 22 | % 12/21/2018 23 | % v22 24 | % 25 | classdef LinkedRotationalJointCLASS_v2 < LinkedGenericJointCLASS_v2 26 | % Public Properties 27 | properties (SetAccess = public, GetAccess = public) 28 | % All inherited 29 | end 30 | % Protected Properties 31 | properties (SetAccess = protected, GetAccess = protected) 32 | % All inherited 33 | end 34 | % Public Methods 35 | methods 36 | % Constructor 37 | function obj = LinkedRotationalJointCLASS_v2(env, predBody, sucBody) 38 | % Superclass constructors must be called explicitly, as we need 39 | % to decide which arguments we pass to each constructor. 40 | % 41 | % Invoke superclass constructor to create generic joint: 42 | obj = obj@LinkedGenericJointCLASS_v2(env, predBody, sucBody); 43 | end 44 | % Destructor (empty) 45 | function delete(obj) 46 | % Superclass desctructor is called automatically 47 | end 48 | end 49 | % Protected Methods 50 | methods (Access = protected) 51 | function [Dp_r_DpDs, A_DpDs] = JointFunction(obj, q) 52 | % Overwrite generic JointFunction: 53 | gamma = q; 54 | Dp_r_DpDs = [0;0;0]; 55 | A_DpDs = [+cos(gamma), -sin(gamma), 0; 56 | +sin(gamma), +cos(gamma), 0; 57 | +0 , +0 , 1]; 58 | end 59 | end 60 | end 61 | 62 | -------------------------------------------------------------------------------- /Exercises/Exercise_V/Examples/Example_9_2DOFManipulator.m: -------------------------------------------------------------------------------- 1 | %% Define symbolic variables and the vector of generalized coordinates: 2 | clear all 3 | clc 4 | 5 | syms q1 q2 real 6 | q = [q1; q2]; 7 | syms q1_dot q2_dot real 8 | q_dot = [q1_dot; q2_dot]; 9 | 10 | syms l m_1 m_2 g real 11 | 12 | Fg_1 = [0; -m_1*g]; 13 | Fg_2 = [0; -m_2*g]; 14 | 15 | %% Kinematics: 16 | % We could also do this with our recursive toolbox, but for such a simple 17 | % system it's easier to do it by hand: 18 | 19 | x_1 = [+sin(q1)*l 20 | -cos(q1)*l]; 21 | x_2 = [+sin(q1)*(l+q2) 22 | -cos(q1)*(l+q2)]; 23 | 24 | % Get jacobians via partial derivatives: 25 | Jf_1 = jacobian(x_1, q); 26 | disp('J_1:') 27 | disp(Jf_1); 28 | Jf_2 = jacobian(x_2, q); 29 | disp('J_2:') 30 | disp(Jf_2); 31 | 32 | % Get bias acceleration via taking the partial derivative of "J_f*q_dot" 33 | % and multiplying it with q_dot: 34 | sigma_1 = simplify(expand(jacobian(Jf_1*q_dot, q)*q_dot)); 35 | disp('sigma_1:') 36 | disp(sigma_1); 37 | sigma_2 = simplify(expand(jacobian(Jf_2*q_dot, q)*q_dot)); 38 | disp('sigma_2:') 39 | disp(sigma_2); 40 | 41 | %% Put together the components of the EOM: 42 | 43 | M = simplify(expand( Jf_1'*m_1*Jf_1 + Jf_2'*m_2*Jf_2 )); 44 | disp('M(q):') 45 | disp(M) 46 | 47 | f = simplify(expand( -Jf_1'*m_1*sigma_1 + -Jf_2'*m_2*sigma_2 )); 48 | disp('f(q, q_dot):') 49 | disp(f) 50 | 51 | g = simplify(expand( Jf_1'*Fg_1 + Jf_2'*Fg_2 )); 52 | disp('g(q):') 53 | disp(g) 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /Exercises/Exercise_V/Solutions/LinkedRollingContactJointVeAcJaCLASS.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedRollingContactJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 2 | % 3 | % Defines a rolling contact joint in a kinematic tree 4 | % that is implemented as a set of linked objects. 5 | % This class allows the computation of Positions/Orientations, Velocities, 6 | % Accelerations, and Jacobians. 7 | % 8 | % Public Methods: 9 | % joint = LinkedRollingContactJointVeAcJaCLASS(env, predBody, sucBody) 10 | % Creates a rotational joint object that links the body 11 | % specified in 'predBody' with the body specified in 'sucBody'. 12 | % Both are objects of the type 'LinkedRigidBodyDynamicsVeAcJaCLASS', 13 | % and function as predecessor and successor in a kinematic tree. 14 | % The joint is shown in the graphical environment 'env'. 15 | % joint.delete() 16 | % Removes the joint from the graphics output and memory 17 | % + All methods inherited from LinkedGenericJointVeAcJaCLASS 18 | % 19 | % Public Properties: 20 | % All inherited from LinkedGenericJointVeAcJaCLASS 21 | % 22 | % C. David Remy remy@inm.uni-stuttgart.de 23 | % Matlab R2018 24 | % 12/21/2018 25 | % v22 26 | % 27 | classdef LinkedRollingContactJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 28 | % Public Properties 29 | properties (SetAccess = public, GetAccess = public) 30 | r = 0; % Rolling contact radius 31 | end 32 | % Protected Properties 33 | properties (SetAccess = protected, GetAccess = protected) 34 | % All inherited 35 | end 36 | % Public Methods 37 | methods 38 | % Constructor 39 | function obj = LinkedRollingContactJointVeAcJaCLASS(env, predBody, sucBody) 40 | % Superclass constructors must be called explicitly, as we need 41 | % to decide which arguments we pass to each constructor. 42 | % 43 | % Invoke superclass constructor to create generic joint: 44 | obj = obj@LinkedGenericJointVeAcJaCLASS(env, predBody, sucBody); 45 | end 46 | % Destructor (empty) 47 | function delete(obj) 48 | % Superclass desctructor is called automatically 49 | end 50 | end 51 | % Protected Methods 52 | methods (Access = protected) 53 | function [Dp_r_DpDs, A_DpDs] = JointFunction(obj, q) 54 | % Overwrite generic JointFunction: 55 | gamma = q; 56 | Dp_r_DpDs = [-obj.r*gamma;0;0]; 57 | A_DpDs = [+cos(gamma), -sin(gamma), 0; 58 | +sin(gamma), +cos(gamma), 0; 59 | +0 , +0 , 1]; 60 | end 61 | function [Dp_rDot_DpDs, Dp_omega_DpDs] = JointVelocity(obj, q, qDot) 62 | % Overwrite generic JointVelocity: 63 | Dp_rDot_DpDs = [-obj.r*qDot; 0; 0]; 64 | Dp_omega_DpDs = [0; 0; qDot]; 65 | end 66 | function [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] = JointAcceleration(obj, q, qDot, qDDot) 67 | % Overwrite generic JointAcceleration: 68 | Dp_rDDot_DpDs = [-obj.r*qDDot; 0; 0]; 69 | Dp_omegaDot_DpDs = [0; 0; qDDot]; 70 | end 71 | function [S, R] = JointJacobian(obj, q, qIndex, nq) 72 | % Overwrite generic JointJacobian: 73 | S = zeros(3,nq); 74 | R = zeros(3,nq); 75 | S(:,qIndex) = [-obj.r;0;0]; 76 | R(:,qIndex) = [0;0;1]; 77 | end 78 | end 79 | end 80 | 81 | -------------------------------------------------------------------------------- /Exercises/Exercise_V/Solutions/LinkedRotationalJointVeAcJaCLASS.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedRotationalJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 2 | % 3 | % Defines a rotational joint in a kinematic tree 4 | % that is implemented as a set of linked objects. 5 | % This class allows the computation of Positions/Orientations, Velocities, 6 | % Accelerations, and Jacobians. 7 | % 8 | % Public Methods: 9 | % joint = LinkedRotationalJointVeAcJaCLASS(env, predBody, sucBody) 10 | % Creates a rotational joint object that links the body 11 | % specified in 'predBody' with the body specified in 'sucBody'. 12 | % Both are objects of the type 'LinkedRigidBodyDynamicsVeAcJaCLASS', 13 | % and function as predecessor and successor in a kinematic tree. 14 | % The joint is shown in the graphical environment 'env'. 15 | % joint.delete() 16 | % Removes the joint from the graphics output and memory 17 | % + All methods inherited from LinkedGenericJointVeAcJaCLASS 18 | % 19 | % Public Properties: 20 | % All inherited from LinkedGenericJointVeAcJaCLASS 21 | % 22 | % C. David Remy remy@inm.uni-stuttgart.de 23 | % Matlab R2018 24 | % 12/21/2018 25 | % v22 26 | % 27 | classdef LinkedRotationalJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 28 | % Public Properties 29 | properties (SetAccess = public, GetAccess = public) 30 | % All inherited 31 | end 32 | % Protected Properties 33 | properties (SetAccess = protected, GetAccess = protected) 34 | % All inherited 35 | end 36 | % Public Methods 37 | methods 38 | % Constructor 39 | function obj = LinkedRotationalJointVeAcJaCLASS(env, predBody, sucBody) 40 | % Superclass constructors must be called explicitly, as we need 41 | % to decide which arguments we pass to each constructor. 42 | % 43 | % Invoke superclass constructor to create generic joint: 44 | obj = obj@LinkedGenericJointVeAcJaCLASS(env, predBody, sucBody); 45 | end 46 | % Destructor (empty) 47 | function delete(obj) 48 | % Superclass desctructor is called automatically 49 | end 50 | end 51 | % Protected Methods 52 | methods (Access = protected) 53 | function [Dp_r_DpDs, A_DpDs] = JointFunction(obj, q) 54 | % Overwrite generic JointFunction: 55 | gamma = q; 56 | Dp_r_DpDs = [0;0;0]; 57 | A_DpDs = [+cos(gamma), -sin(gamma), 0; 58 | +sin(gamma), +cos(gamma), 0; 59 | +0 , +0 , 1]; 60 | end 61 | function [Dp_rDot_DpDs, Dp_omega_DpDs] = JointVelocity(obj, q, qDot) 62 | % Overwrite generic JointVelocity: 63 | Dp_rDot_DpDs = zeros(3,1); 64 | Dp_omega_DpDs = [0; 0; qDot]; 65 | end 66 | function [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] = JointAcceleration(obj, q, qDot, qDDot) 67 | % Overwrite generic JointAcceleration: 68 | Dp_rDDot_DpDs = zeros(3,1); 69 | Dp_omegaDot_DpDs = [0; 0; qDDot]; 70 | end 71 | function [S, R] = JointJacobian(obj, q, qIndex, nq) 72 | % Overwrite generic JointJacobian: 73 | S = zeros(3,nq); 74 | R = zeros(3,nq); 75 | S(:,qIndex) = [0;0;0]; 76 | R(:,qIndex) = [0;0;1]; 77 | end 78 | end 79 | end 80 | 81 | -------------------------------------------------------------------------------- /Exercises/Exercise_V/Solutions/LinkedTranslationalJointVeAcJaCLASS.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedTranslationalJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 2 | % 3 | % Defines a translational joint in a kinematic tree 4 | % that is implemented as a set of linked objects. 5 | % This class allows the computation of Positions/Orientations, Velocities, 6 | % Accelerations, and Jacobians. 7 | % 8 | % Public Methods: 9 | % joint = LinkedTranslationalJointVeAcJaCLASS(env, predBody, sucBody) 10 | % Creates a rotational joint object that links the body 11 | % specified in 'predBody' with the body specified in 'sucBody'. 12 | % Both are objects of the type 'LinkedRigidBodyDynamicsVeAcJaCLASS', 13 | % and function as predecessor and successor in a kinematic tree. 14 | % The joint is shown in the graphical environment 'env'. 15 | % joint.delete() 16 | % Removes the joint from the graphics output and memory 17 | % + All methods inherited from LinkedGenericJointVeAcJaCLASS 18 | % 19 | % Public Properties: 20 | % All inherited from LinkedGenericJointVeAcJaCLASS 21 | % 22 | % C. David Remy remy@inm.uni-stuttgart.de 23 | % Matlab R2018 24 | % 12/21/2018 25 | % v22 26 | % 27 | classdef LinkedTranslationalJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 28 | % Public Properties 29 | properties (SetAccess = public, GetAccess = public) 30 | % All inherited 31 | end 32 | % Protected Properties 33 | properties (SetAccess = protected, GetAccess = protected) 34 | % All inherited 35 | end 36 | % Public Methods 37 | methods 38 | % Constructor 39 | function obj = LinkedTranslationalJointVeAcJaCLASS(env, predBody, sucBody) 40 | % Superclass constructors must be called explicitly, as we need 41 | % to decide which arguments we pass to each constructor. 42 | % 43 | % Invoke superclass constructor to create generic joint: 44 | obj = obj@LinkedGenericJointVeAcJaCLASS(env, predBody, sucBody); 45 | end 46 | % Destructor (empty) 47 | function delete(obj) 48 | % Superclass desctructor is called automatically 49 | end 50 | end 51 | % Protected Methods 52 | methods (Access = protected) 53 | function [Dp_r_DpDs, A_DpDs] = JointFunction(obj, q) 54 | % Overwrite generic JointFunction: 55 | deltaX = q; 56 | Dp_r_DpDs = [deltaX;0;0]; 57 | A_DpDs = eye(3); 58 | end 59 | function [Dp_rDot_DpDs, Dp_omega_DpDs] = JointVelocity(obj, q, qDot) 60 | % Overwrite generic JointVelocity: 61 | Dp_rDot_DpDs = [qDot; 0; 0]; 62 | Dp_omega_DpDs = zeros(3,1); 63 | end 64 | function [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] = JointAcceleration(obj, q, qDot, qDDot) 65 | % Overwrite generic JointAcceleration: 66 | Dp_rDDot_DpDs = [qDDot; 0; 0]; 67 | Dp_omegaDot_DpDs = zeros(3,1); 68 | end 69 | function [S, R] = JointJacobian(obj, q, qIndex, nq) 70 | % Overwrite generic JointJacobian: 71 | S = zeros(3,nq); 72 | R = zeros(3,nq); 73 | S(:,qIndex) = [1;0;0]; 74 | R(:,qIndex) = [0;0;0]; 75 | end 76 | end 77 | end 78 | 79 | -------------------------------------------------------------------------------- /Exercises/Exercise_V/Solutions/LinkedVirtual3DOFJointVeAcJaCLASS.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedVirtual3DOFJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 2 | % 3 | % Defines a virtual 3 DOF joint in a kinematic tree 4 | % that is implemented as a set of linked objects. 5 | % This class allows the computation of Positions/Orientations, Velocities, 6 | % Accelerations, and Jacobians. 7 | % 8 | % Public Methods: 9 | % joint = LinkedVirtual3DOFJointVeAcJaCLASS(env, predBody, sucBody) 10 | % Creates a rotational joint object that links the body 11 | % specified in 'predBody' with the body specified in 'sucBody'. 12 | % Both are objects of the type 'LinkedRigidBodyDynamicsVeAcJaCLASS', 13 | % and function as predecessor and successor in a kinematic tree. 14 | % The joint is shown in the graphical environment 'env'. 15 | % joint.delete() 16 | % Removes the joint from the graphics output and memory 17 | % + All methods inherited from LinkedGenericJointVeAcJaCLASS 18 | % 19 | % Public Properties: 20 | % All inherited from LinkedGenericJointVeAcJaCLASS 21 | % 22 | % C. David Remy remy@inm.uni-stuttgart.de 23 | % Matlab R2018 24 | % 12/21/2018 25 | % v22 26 | % 27 | classdef LinkedVirtual3DOFJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 28 | % Public Properties 29 | properties (SetAccess = public, GetAccess = public) 30 | % All inherited 31 | end 32 | % Protected Properties 33 | properties (SetAccess = protected, GetAccess = protected) 34 | % All inherited 35 | end 36 | % Public Methods 37 | methods 38 | % Constructor 39 | function obj = LinkedVirtual3DOFJointVeAcJaCLASS(env, predBody, sucBody) 40 | % Superclass constructors must be called explicitly, as we need 41 | % to decide which arguments we pass to each constructor. 42 | % 43 | % Invoke superclass constructor to create generic joint: 44 | obj = obj@LinkedGenericJointVeAcJaCLASS(env, predBody, sucBody); 45 | end 46 | % Destructor (empty) 47 | function delete(obj) 48 | % Superclass desctructor is called automatically 49 | end 50 | end 51 | % Protected Methods 52 | methods (Access = protected) 53 | function [Dp_r_DpDs, A_DpDs] = JointFunction(obj, q) 54 | % Overwrite generic JointFunction: 55 | deltaX = q(1); 56 | deltaY = q(2); 57 | gamma = q(3); 58 | Dp_r_DpDs = [deltaX; deltaY;0]; 59 | A_DpDs = [+cos(gamma), -sin(gamma), 0; 60 | +sin(gamma), +cos(gamma), 0; 61 | +0 , +0 , 1]; 62 | end 63 | function [Dp_rDot_DpDs, Dp_omega_DpDs] = JointVelocity(obj, q, qDot) 64 | % Overwrite generic JointVelocity: 65 | Dp_rDot_DpDs = [qDot(1); qDot(2); 0]; 66 | Dp_omega_DpDs = [0; 0; qDot(3)]; 67 | end 68 | function [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] = JointAcceleration(obj, q, qDot, qDDot) 69 | % Overwrite generic JointAcceleration: 70 | Dp_rDDot_DpDs = [qDDot(1); qDDot(2); 0]; 71 | Dp_omegaDot_DpDs = [0; 0; qDDot(3)]; 72 | end 73 | function [S, R] = JointJacobian(obj, q, qIndex, nq) 74 | % Overwrite generic JointJacobian: 75 | S = zeros(3,nq); 76 | R = zeros(3,nq); 77 | S(:,qIndex) = [1,0,0;0,1,0;0,0,0]; 78 | R(:,qIndex) = [0,0,0;0,0,0;0,0,1]; 79 | end 80 | end 81 | end 82 | 83 | -------------------------------------------------------------------------------- /Exercises/Exercise_V/Solutions/Solution_30.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_V/Solutions/Solution_30.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_V/Solutions/Solution_35.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_V/Solutions/Solution_35.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_V/Solutions/Solution_36.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_V/Solutions/Solution_36.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_V/Solutions/Solution_36_DoublePendulum.m: -------------------------------------------------------------------------------- 1 | %% Define symbolic variables and the vector of generalized coordinates: 2 | clear all 3 | clc 4 | 5 | syms q1 q2 real 6 | q = [q1; q2]; 7 | syms q1_dot q2_dot real 8 | q_dot = [q1_dot; q2_dot]; 9 | 10 | syms l_1 l_2 m_1 m_2 g real 11 | 12 | Fg_1 = [0; -m_1*g]; 13 | Fg_2 = [0; -m_2*g]; 14 | 15 | %% Kinematics: 16 | % We could also do this with our recursive toolbox, but for such a simple 17 | % system it's easier to do it by hand: 18 | 19 | x_1 = [+l_1*sin(q1) 20 | -l_1*cos(q1)]; 21 | x_2 = x_1 + [+l_2*sin(q1+q2) 22 | -l_2*cos(q1+q2)]; 23 | 24 | % Get jacobians via partial derivatives: 25 | Jf_1 = jacobian(x_1, q); 26 | disp('J_1:') 27 | disp(Jf_1); 28 | Jf_2 = jacobian(x_2, q); 29 | disp('J_2:') 30 | disp(Jf_2); 31 | 32 | % Get bias acceleration via taking the partial derivative of "J_f*q_dot" 33 | % and multiplying it with q_dot: 34 | sigma_1 = simplify(expand(jacobian(Jf_1*q_dot, q)*q_dot)); 35 | disp('sigma_1:') 36 | disp(sigma_1); 37 | sigma_2 = simplify(expand(jacobian(Jf_2*q_dot, q)*q_dot)); 38 | disp('sigma_2:') 39 | disp(sigma_2); 40 | 41 | %% Put together the components of the EOM: 42 | 43 | M = simplify(expand( Jf_1'*m_1*Jf_1 + Jf_2'*m_2*Jf_2 )); 44 | disp('M(q):') 45 | disp(M) 46 | 47 | f = simplify(expand( -Jf_1'*m_1*sigma_1 + -Jf_2'*m_2*sigma_2 )); 48 | disp('f(q, q_dot):') 49 | disp(f) 50 | 51 | g = simplify(expand( Jf_1'*Fg_1 + Jf_2'*Fg_2 )); 52 | disp('g(q):') 53 | disp(g) 54 | 55 | %% Show that the constraint forces are 56 | % Since we know the direction of the constraint forces, we can express them 57 | % as a combination of these directions multiplied by a scalar: 58 | syms c_1 c_2 real 59 | Fc_2 = c_2*[-sin(q1+q2); 60 | +cos(q1+q2)]; 61 | Fc_1 = -Fc_2 + c_1*[-sin(q1); 62 | +cos(q1)]; 63 | Fc = [Fc_1; Fc_2]; 64 | disp('Fc (scaled):') 65 | disp(Fc); 66 | 67 | % Show that constraint forces and velocities are perpendicular to each 68 | % other: 69 | Jf = [Jf_1; Jf_2]; 70 | disp('Jf''*Fc:') 71 | disp(simplify(expand(Jf'*Fc))) 72 | 73 | 74 | -------------------------------------------------------------------------------- /Exercises/Exercise_V/Solutions/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_V/Solutions/Thumbs.db -------------------------------------------------------------------------------- /Exercises/Exercise_V/Templates/Template_34_LinkedTranslationalJointVeAcJaCLASS.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedTranslationalJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 2 | % 3 | % Defines a translational joint in a kinematic tree 4 | % that is implemented as a set of linked objects. 5 | % This class allows the computation of Positions/Orientations, Velocities, 6 | % Accelerations, and Jacobians. 7 | % 8 | % Public Methods: 9 | % joint = LinkedTranslationalJointVeAcJaCLASS(env, predBody, sucBody) 10 | % Creates a rotational joint object that links the body 11 | % specified in 'predBody' with the body specified in 'sucBody'. 12 | % Both are objects of the type 'LinkedRigidBodyDynamicsVeAcJaCLASS', 13 | % and function as predecessor and successor in a kinematic tree. 14 | % The joint is shown in the graphical environment 'env'. 15 | % joint.delete() 16 | % Removes the joint from the graphics output and memory 17 | % + All methods inherited from LinkedGenericJointVeAcJaCLASS 18 | % 19 | % Public Properties: 20 | % All inherited from LinkedGenericJointVeAcJaCLASS 21 | % 22 | % C. David Remy remy@inm.uni-stuttgart.de 23 | % Matlab R2018 24 | % 12/21/2018 25 | % v22 26 | % 27 | % ************************************************************************* 28 | % ToDo: 29 | % Rename file since for matlab class names and file names must be the same! 30 | % ************************************************************************* 31 | classdef LinkedTranslationalJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 32 | % Public Properties 33 | properties (SetAccess = public, GetAccess = public) 34 | % All inherited 35 | end 36 | % Protected Properties 37 | properties (SetAccess = protected, GetAccess = protected) 38 | % All inherited 39 | end 40 | % Public Methods 41 | methods 42 | % Constructor 43 | function obj = LinkedTranslationalJointVeAcJaCLASS(env, predBody, sucBody) 44 | % Superclass constructors must be called explicitly, as we need 45 | % to decide which arguments we pass to each constructor. 46 | % 47 | % Invoke superclass constructor to create generic joint: 48 | obj = obj@LinkedGenericJointVeAcJaCLASS(env, predBody, sucBody); 49 | end 50 | % Destructor (empty) 51 | function delete(obj) 52 | % Superclass desctructor is called automatically 53 | end 54 | end 55 | % Protected Methods 56 | methods (Access = protected) 57 | function [Dp_r_DpDs, A_DpDs] = JointFunction(obj, q) 58 | % Overwrite generic JointFunction: 59 | deltaX = q; 60 | Dp_r_DpDs = [deltaX;0;0]; 61 | A_DpDs = eye(3); 62 | end 63 | % ************************************************************************* 64 | % ToDo: 65 | % In the three functions below, complete the specific functions to compute 66 | % velocities, accelerations, and Jacobians accross the joint, to learn how 67 | % these concepts are implemented in Matlab for a translational joint. 68 | % ************************************************************************* 69 | function [Dp_rDot_DpDs, Dp_omega_DpDs] = JointVelocity(obj, q, qDot) 70 | % Overwrite generic JointVelocity: 71 | Dp_rDot_DpDs = ; 72 | Dp_omega_DpDs = ; 73 | end 74 | function [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] = JointAcceleration(obj, q, qDot, qDDot) 75 | % Overwrite generic JointAcceleration: 76 | Dp_rDDot_DpDs = ; 77 | Dp_omegaDot_DpDs = ; 78 | end 79 | function [S, R] = JointJacobian(obj, q, qIndex, nq) 80 | % Overwrite generic JointJacobian: 81 | S = zeros(3,nq); 82 | R = zeros(3,nq); 83 | S(:,qIndex) = ; 84 | R(:,qIndex) = ; 85 | end 86 | end 87 | end 88 | 89 | -------------------------------------------------------------------------------- /Exercises/Exercise_V/TestFiles/Problem_33_Output.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_V/TestFiles/Problem_33_Output.jpg -------------------------------------------------------------------------------- /Exercises/Exercise_V/TestFiles/TestData_30_31_TriplePendulumSim.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_V/TestFiles/TestData_30_31_TriplePendulumSim.mat -------------------------------------------------------------------------------- /Exercises/Exercise_V/TestFiles/TestData_32_RobotTrajectory.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_V/TestFiles/TestData_32_RobotTrajectory.mat -------------------------------------------------------------------------------- /Exercises/Exercise_V/TestFiles/TestData_34_RunningData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_V/TestFiles/TestData_34_RunningData.mat -------------------------------------------------------------------------------- /Exercises/Exercise_V/TestFiles/TestData_34_WalkingData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_V/TestFiles/TestData_34_WalkingData.mat -------------------------------------------------------------------------------- /Exercises/Exercise_V/TestFiles/TestFile_30_RotationalJoint_v2.m: -------------------------------------------------------------------------------- 1 | %% INITIALIZE: 2 | clear all 3 | close all 4 | clc 5 | 6 | % Create a 'physical' (graphical) environment: 7 | env = EnvironmentCLASS(); 8 | 9 | 10 | % Define a triple pendulum 11 | % Define the bodies: 12 | ground = LinkedRigidBodyDynamicsCLASS_v2(env); 13 | ground.bodyName = 'Ground'; 14 | ground.autoUpdate = false; 15 | ground.m_B = 1; % Since this is ground, m and I do not matter 16 | ground.B_I_B = eye(3)*0.0001; 17 | ground.scale = 0.5; % Defines how large the CoSys will be drawn 18 | link1 = LinkedRigidBodyDynamicsCLASS_v2(env); 19 | link1.bodyName = 'Link 1'; 20 | link1.autoUpdate = false; 21 | link1.m_B = 10; % m and I just given for visualization 22 | link1.B_I_B = diag([0.0042,0.8354,0.8354]); 23 | link1.scale = 0.5; % Defines how large the CoSys will be drawn 24 | link2 = LinkedRigidBodyDynamicsCLASS_v2(env); 25 | link2.bodyName = 'Link 2'; 26 | link2.autoUpdate = false; 27 | link2.m_B = 10; % m and I just given for visualization 28 | link2.B_I_B = diag([0.0042,0.8354,0.8354]); 29 | link2.scale = 0.5; % Defines how large the CoSys will be drawn 30 | link3 = LinkedRigidBodyDynamicsCLASS_v2(env); 31 | link3.bodyName = 'Link 3'; 32 | link3.autoUpdate = false; 33 | link3.m_B = 10; % m and I just given for visualization 34 | link3.B_I_B = diag([0.0042,0.8354,0.8354]); 35 | link3.scale = 0.5; % Defines how large the CoSys will be drawn 36 | 37 | % Define the joints and the interconnection of the bodies: 38 | joint1 = LinkedRotationalJointCLASS_v2(env, ground, link1); 39 | joint1.P_r_PDp = [0;0;0]; 40 | joint1.A_PDp = [0,1,0;-1,0,0;0,0,1]; 41 | joint1.S_r_SDs = [-0.5;0;0]; 42 | joint1.A_SDs = eye(3); 43 | joint1.jointName = 'Joint 1'; 44 | joint1.autoUpdate = false; 45 | joint1.scale = 0.2; 46 | 47 | joint2 = LinkedRotationalJointCLASS_v2(env, link1, link2); 48 | joint2.P_r_PDp = [+0.5;0;0]; 49 | joint2.A_PDp = eye(3); 50 | joint2.S_r_SDs = [-0.5;0;0]; 51 | joint2.A_SDs = eye(3); 52 | joint2.jointName = 'Joint 2'; 53 | joint2.autoUpdate = false; 54 | joint2.scale = 0.2; 55 | 56 | joint3 = LinkedRotationalJointCLASS_v2(env, link2, link3); 57 | joint3.P_r_PDp = [+0.5;0;0]; 58 | joint3.A_PDp = eye(3); 59 | joint3.S_r_SDs = [-0.5;0;0]; 60 | joint3.A_SDs = eye(3); 61 | joint3.jointName = 'Joint 3'; 62 | joint3.autoUpdate = false; 63 | joint3.scale = 0.2; 64 | 65 | % Show the topology: 66 | ground.recursiveOutput(''); 67 | 68 | % Define the joint angles: 69 | joint1.q = +pi/6; 70 | joint2.q = -pi/4; 71 | joint3.q = +pi/3; 72 | 73 | % Compute the forward kinematics starting with a root at position and 74 | % orientation zero: 75 | ground.recursiveForwardKinematics(); 76 | % Update the graphics 77 | ground.recursiveGraphicsUpdate(); 78 | 79 | % Save final screen shot: 80 | axis([-1,1,-3,3,-3.5,0.5]); 81 | print(gcf,'-r600','-djpeg','Problem_30_Output.jpg'); 82 | 83 | 84 | 85 | %% Animation: 86 | pause(3); 87 | data = load('TestData_30_31_TriplePendulumSim.mat'); 88 | 89 | delta_t_show = 0.05; 90 | next_t_output = data.t(1); 91 | 92 | while next_t_output < data.t(end) 93 | ind = find( data.t>=next_t_output, 1); 94 | 95 | % Update the joint angles: 96 | joint1.q = pi/2 + data.q1(ind); 97 | joint2.q = data.q2(ind); 98 | joint3.q = data.q3(ind); 99 | 100 | % Compute the forward kinematics starting with a root at position and 101 | % orientation zero: 102 | ground.recursiveForwardKinematics(); 103 | % Update the graphics 104 | ground.recursiveGraphicsUpdate(); 105 | drawnow; 106 | 107 | next_t_output = next_t_output + delta_t_show; 108 | end 109 | 110 | -------------------------------------------------------------------------------- /Exercises/Exercise_V/TestFiles/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_V/TestFiles/Thumbs.db -------------------------------------------------------------------------------- /Exercises/Exercise_V/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_V/Thumbs.db -------------------------------------------------------------------------------- /Exercises/Exercise_VI/Solutions/Solution_39.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_VI/Solutions/Solution_39.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_VI/Solutions/Solution_40.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_VI/Solutions/Solution_40.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_VI/Solutions/Solution_40_DoublePendulumEquations.m: -------------------------------------------------------------------------------- 1 | %% Define symbolic variables and the vector of generalized coordinates: 2 | clear all 3 | clc 4 | 5 | syms q1 q2 real 6 | q = [q1; q2]; 7 | syms q1_dot q2_dot real 8 | q_dot = [q1_dot; q2_dot]; 9 | 10 | syms l_1 l_2 m_1 m_2 g real 11 | 12 | Fg_1 = [0; -m_1*g]; 13 | Fg_2 = [0; -m_2*g]; 14 | 15 | %% Kinematics: 16 | % We could also do this with our recursive toolbox, but for such a simple 17 | % system it's easier to do it by hand: 18 | 19 | x_1 = [+sin(q1)*l_1 20 | -cos(q1)*l_1]; 21 | x_2 = [+sin(q1)*l_1 + sin(q1+q2)*l_2 22 | -cos(q1)*l_1 - cos(q1+q2)*l_2]; 23 | 24 | % Get jacobians via partial derivatives: 25 | Jf_1 = jacobian(x_1, q); 26 | disp('J_1:') 27 | disp(Jf_1); 28 | Jf_2 = jacobian(x_2, q); 29 | disp('J_2:') 30 | disp(Jf_2); 31 | 32 | % Get bias acceleration via taking the partial derivative of "J_f*q_dot" 33 | % and multiplying it with q_dot: 34 | sigma_1 = simplify(expand(jacobian(Jf_1*q_dot, q)*q_dot)); 35 | disp('sigma_1:') 36 | disp(sigma_1); 37 | sigma_2 = simplify(expand(jacobian(Jf_2*q_dot, q)*q_dot)); 38 | disp('sigma_2:') 39 | disp(sigma_2); 40 | 41 | %% Put together the components of the EOM: 42 | 43 | M = simplify(expand( Jf_1'*m_1*Jf_1 + Jf_2'*m_2*Jf_2 )); 44 | disp('M(q):') 45 | disp(M) 46 | 47 | f = simplify(expand( -Jf_1'*m_1*sigma_1 + -Jf_2'*m_2*sigma_2 )); 48 | disp('f(q, q_dot):') 49 | disp(f) 50 | 51 | g = simplify(expand( Jf_1'*Fg_1 + Jf_2'*Fg_2 )); 52 | disp('g(q):') 53 | disp(g) 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /Exercises/Exercise_VI/Solutions/Solution_41.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_VI/Solutions/Solution_41.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_VI/Solutions/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_VI/Solutions/Thumbs.db -------------------------------------------------------------------------------- /Exercises/Exercise_VI/TestFiles/BetterGraphics/CoGPositions.m: -------------------------------------------------------------------------------- 1 | function CoGs = CoGPositions(gamma,alpha,l_0,l2x,l2y,rFoot,gx,gy,m1,m2,j2) 2 | %COGPOSITIONS 3 | % COGS = COGPOSITIONS(GAMMA,ALPHA,L_0,L2X,L2Y,RFOOT,GX,GY,M1,M2,J2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 5.4. 6 | % 10-Apr-2011 10:49:54 7 | 8 | t764 = sin(gamma); 9 | t765 = l_0-rFoot; 10 | t766 = alpha+gamma; 11 | t767 = cos(gamma); 12 | t768 = t765.*t767; 13 | t769 = cos(t766); 14 | t770 = sin(t766); 15 | CoGs = reshape([-gamma.*rFoot-t764.*t765,rFoot+t768,0.0,-gamma.*rFoot+l2x.*t764+l2y.*t767-t764.*t765,rFoot+t768-l2x.*t767+l2y.*t764,gamma,-gamma.*rFoot+l2x.*t770+l2y.*t769-t764.*t765,rFoot+t768-l2x.*t769+l2y.*t770,t766],[3, 3]); 16 | -------------------------------------------------------------------------------- /Exercises/Exercise_VI/TestFiles/BetterGraphics/ContStateDefinition.m: -------------------------------------------------------------------------------- 1 | % ************************************************************************* 2 | % 3 | % function [contStateVec, contStateNames, contStateIndices] = ContStateDefinition() 4 | % function y = ContStateDefinition() 5 | % 6 | % This MATLAB function defines the continuous state vector 'y' for a 7 | % passive dynamic biped in 2D. Besides serving as initial configuration of 8 | % the model, this file provides a definition of the individual components 9 | % of the continuous state vector and an index struct that allows name-based 10 | % access to its values. 11 | % 12 | % NOTE: This function is relatively slow and should not be executed within 13 | % the simulation loop. 14 | % 15 | % Input: - NONE 16 | % Output: - The initial continuous states as the vector 'contStateVec' (or 'y') 17 | % - The corresponding state names in the cell array 'contStateNames' 18 | % - The struct 'contStateIndices' that maps these names into indices 19 | % 20 | % Created by C. David Remy on 03/14/2011 21 | % MATLAB 2010a 22 | % 23 | % Documentation: 24 | % 'A MATLAB Framework For Gait Creation', 2011, C. David Remy (1), Keith 25 | % Buffinton (2), and Roland Siegwart (1), International Conference on 26 | % Intelligent Robots and Systems, September 25-30, San Francisco, USA 27 | % 28 | % (1) Autonomous Systems Lab, Institute of Robotics and Intelligent Systems, 29 | % Swiss Federal Institute of Technology (ETHZ) 30 | % Tannenstr. 3 / CLA-E-32.1 31 | % 8092 Zurich, Switzerland 32 | % cremy@ethz.ch; rsiegwart@ethz.ch 33 | % 34 | % (2) Department of Mechanical Engineering, 35 | % Bucknell University 36 | % 701 Moore Avenue 37 | % Lewisburg, PA-17837, USA 38 | % buffintk@bucknell.edu 39 | % 40 | % See also HYBRIDDYNAMICS, FLOWMAP, JUMPMAP, JUMPSET, 41 | % DISCSTATEDEFINITION, SYSTPARAMDEFINITION, 42 | % VEC2STRUCT, STRUCT2VEC. 43 | % 44 | function [contStateVec, contStateNames, contStateIndices] = ContStateDefinition() 45 | 46 | % All units are normalized to gravity g, total mass m_0, and leg length 47 | % l_0. 48 | 49 | contState.gamma = + 0.3; % [rad] angle of the stance leg wrt the ground (0 = straight up) 50 | contState.dgamma = - 0.5; % [rad/sqrt(l_0/g)] ... angular velocity thereof 51 | contState.alpha = - 0.6; % [rad] inter leg angle 52 | contState.dalpha = + 0.5; % [rad/sqrt(l_0/g)] ... angular velocity thereof 53 | 54 | [contStateVec, contStateNames] = Struct2Vec(contState); 55 | contStateIndices = Vec2Struct(1:1:length(contStateVec),contStateNames); 56 | end 57 | -------------------------------------------------------------------------------- /Exercises/Exercise_VI/TestFiles/BetterGraphics/FootPtPositions.m: -------------------------------------------------------------------------------- 1 | function footPts = FootPtPositions(gamma,alpha,l_0,l2x,l2y,rFoot,gx,gy,m1,m2,j2) 2 | %FOOTPTPOSITIONS 3 | % FOOTPTS = FOOTPTPOSITIONS(GAMMA,ALPHA,L_0,L2X,L2Y,RFOOT,GX,GY,M1,M2,J2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 5.4. 6 | % 10-Apr-2011 10:49:54 7 | 8 | t778 = l_0-rFoot; 9 | t779 = alpha+gamma; 10 | footPts = reshape([-gamma.*rFoot,rFoot,-gamma.*rFoot-t778.*sin(gamma)+t778.*sin(t779),rFoot+t778.*cos(gamma)-t778.*cos(t779)],[2, 2]); 11 | -------------------------------------------------------------------------------- /Exercises/Exercise_VI/TestFiles/BetterGraphics/GraphicalKinematicsWrapper.m: -------------------------------------------------------------------------------- 1 | % ************************************************************************* 2 | % 3 | % function [CoGs, links, footPts] = GraphicalKinematicsWrapper(y, p) 4 | % 5 | % This MATLAB function calls the automatically generated function code that 6 | % computes the positions of all CoGs, links, and the foot points, to 7 | % facilitate graphical output. This function is not called directly to 8 | % reflect the definition of the continuous states 'y'. It is written for 9 | % the model of a passive dynamic biped. 10 | % 11 | % Input: - A vector of continuous states 'y' 12 | % - A vector of model system parameters 'p' 13 | % Output: - The position and orientation (in rows of [x;y;ang]) of all CoGs 14 | % - A series of line segments, that represent the links of the 15 | % robot 16 | % - All foot points of the robot. 17 | % 18 | % 19 | % Created by C. David Remy on 03/14/2011 20 | % MATLAB 2010a 21 | % 22 | % Documentation: 23 | % 'A MATLAB Framework For Gait Creation', 2011, C. David Remy (1), Keith 24 | % Buffinton (2), and Roland Siegwart (1), International Conference on 25 | % Intelligent Robots and Systems, September 25-30, San Francisco, USA 26 | % 27 | % (1) Autonomous Systems Lab, Institute of Robotics and Intelligent Systems, 28 | % Swiss Federal Institute of Technology (ETHZ) 29 | % Tannenstr. 3 / CLA-E-32.1 30 | % 8092 Zurich, Switzerland 31 | % cremy@ethz.ch; rsiegwart@ethz.ch 32 | % 33 | % (2) Department of Mechanical Engineering, 34 | % Bucknell University 35 | % 701 Moore Avenue 36 | % Lewisburg, PA-17837, USA 37 | % buffintk@bucknell.edu 38 | % 39 | % See also SYMBOLICCOMPUTATIONOFEQM, CONTSTATEDEFINITION, 40 | % SYSTPARAMDEFINITION. 41 | % 42 | function [CoGs, links, footPts] = GraphicalKinematicsWrapper(y, p) 43 | % Map the generalized coordinates: 44 | % Keep the index-structs in memory to speed up processing 45 | persistent contStateIndices 46 | if isempty(contStateIndices) 47 | [~, ~, contStateIndices] = ContStateDefinition(); 48 | end 49 | gamma = y(contStateIndices.gamma); 50 | alpha = y(contStateIndices.alpha); 51 | 52 | % Map the system parameters: 53 | % Keep the index-structs in memory to speed up processing 54 | persistent systParamIndices 55 | if isempty(systParamIndices) 56 | [~, ~, systParamIndices] = SystParamDefinition(); 57 | end 58 | gx = p(systParamIndices.gx); 59 | gy = p(systParamIndices.gy); 60 | l_0 = p(systParamIndices.l_0); 61 | m1 = p(systParamIndices.m1); 62 | m2 = p(systParamIndices.m2); 63 | l2x = p(systParamIndices.l2x); 64 | l2y = p(systParamIndices.l2y); 65 | rFoot = p(systParamIndices.rFoot); 66 | j2 = p(systParamIndices.j2); 67 | 68 | % Call the auto-generated functions 69 | CoGs = CoGPositions(gamma,alpha,l_0,l2x,l2y,rFoot,gx,gy,m1,m2,j2); 70 | links = LinkPositions(gamma,alpha,l_0,l2x,l2y,rFoot,gx,gy,m1,m2,j2); 71 | footPts = FootPtPositions(gamma,alpha,l_0,l2x,l2y,rFoot,gx,gy,m1,m2,j2); 72 | end -------------------------------------------------------------------------------- /Exercises/Exercise_VI/TestFiles/BetterGraphics/LinkPositions.m: -------------------------------------------------------------------------------- 1 | function links = LinkPositions(gamma,alpha,l_0,l2x,l2y,rFoot,gx,gy,m1,m2,j2) 2 | %LINKPOSITIONS 3 | % LINKS = LINKPOSITIONS(GAMMA,ALPHA,L_0,L2X,L2Y,RFOOT,GX,GY,M1,M2,J2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 5.4. 6 | % 10-Apr-2011 10:49:54 7 | 8 | t772 = l_0-rFoot; 9 | t773 = sin(gamma); 10 | t774 = alpha+gamma; 11 | t775 = cos(gamma); 12 | t776 = t772.*t775; 13 | links = reshape([-gamma.*rFoot,rFoot,-gamma.*rFoot-t772.*t773,rFoot+t776,-gamma.*rFoot-t772.*t773+t772.*sin(t774),rFoot+t776-t772.*cos(t774)],[2, 3]); 14 | -------------------------------------------------------------------------------- /Exercises/Exercise_VI/TestFiles/BetterGraphics/SystParamDefinition.m: -------------------------------------------------------------------------------- 1 | % ************************************************************************* 2 | % 3 | % function [systParamVec, systParamNames, systParamIndices] = SystParamDefinition() 4 | % function p = SystParamDefinition() 5 | % 6 | % This MATLAB function defines the physical system parameter vector 'p' for 7 | % a passive dynamic biped in 2D. Besides serving as initial configuration 8 | % of the model, this file provides a definition of the individual 9 | % components of the system parameter vector and an index struct that allows 10 | % name-based access to its values. 11 | % 12 | % NOTE: This function is relatively slow and should not be executed within 13 | % the simulation loop. 14 | % 15 | % Input: - NONE 16 | % Output: - The initial system parameters as the vector 'systParamVec' (or 'p') 17 | % - The corresponding parameter names in the cell array 'systParamNames' 18 | % - The struct 'systParamIndices' that maps these names into indices 19 | % 20 | % Created by C. David Remy on 03/14/2011 21 | % MATLAB 2010a 22 | % 23 | % Documentation: 24 | % 'A MATLAB Framework For Gait Creation', 2011, C. David Remy (1), Keith 25 | % Buffinton (2), and Roland Siegwart (1), International Conference on 26 | % Intelligent Robots and Systems, September 25-30, San Francisco, USA 27 | % 28 | % (1) Autonomous Systems Lab, Institute of Robotics and Intelligent Systems, 29 | % Swiss Federal Institute of Technology (ETHZ) 30 | % Tannenstr. 3 / CLA-E-32.1 31 | % 8092 Zurich, Switzerland 32 | % cremy@ethz.ch; rsiegwart@ethz.ch 33 | % 34 | % (2) Department of Mechanical Engineering, 35 | % Bucknell University 36 | % 701 Moore Avenue 37 | % Lewisburg, PA-17837, USA 38 | % buffintk@bucknell.edu 39 | % 40 | % See also HYBRIDDYNAMICS, FLOWMAP, JUMPMAP, JUMPSET, 41 | % CONTSTATEDEFINITION, DISCSTATEDEFINITION, 42 | % VEC2STRUCT, STRUCT2VEC. 43 | % 44 | function [systParamVec, systParamNames, systParamIndices] = SystParamDefinition() 45 | 46 | % All units are normalized to gravity g, total mass m_0, and 47 | % uncompressed leg length l_0. 48 | 49 | % Physics: 50 | % Gravity is pointing to the right (simulating an inclination of 1 deg): 51 | delta = 1*pi/180; 52 | systParam.gx = +1*sin(delta); % [g] gravity in horizontal direction 53 | systParam.gy = -1*cos(delta); % [g] gravity in vertical direction 54 | % Parameter of the model 55 | systParam.l_0 = 1; % [l_0] leg length 56 | systParam.m1 = 0.20; % [m_0] mass of the main body 57 | systParam.m2 = 0.40; % [m_0] mass of the legs 58 | systParam.l2x = 0.5; % [l_0] distance between hip joint and CoG of the legs (along the legs) 59 | systParam.l2y = 0; % [l_0] distance between hip joint and CoG of the legs (perpendicular to the legs) 60 | systParam.rFoot = 0.5; % [l_0] foot radius 61 | systParam.j2 = 0.002; % [m_0*l_0^2] inertia of the legs 62 | 63 | [systParamVec, systParamNames] = Struct2Vec(systParam); 64 | systParamIndices = Vec2Struct(1:1:length(systParamVec),systParamNames); 65 | end 66 | -------------------------------------------------------------------------------- /Exercises/Exercise_VI/TestFiles/BetterGraphics/TransformVertices.m: -------------------------------------------------------------------------------- 1 | % ************************************************************************* 2 | % 3 | % function vTrans = TransformVertices(v, dirCosine, translation) 4 | % 5 | % This function transforms the coordinates of the vertices given in 'v'. 6 | % 'dirCosine' is a rotation 3 x 3 matrix about the origin of the coordinate 7 | % system, 'translation' is a translational 3-vector which is applied 8 | % subsequently. Both are applied to every element in 'v'. 'v' is a matrix 9 | % containing vertices, as they are used in patch objects. The return value 10 | % 'vTrans' contains the coordinates of the transformed vertices. 11 | % 12 | % 13 | % Input: - A nx3 matrix of vertices 'v' stored in rows 14 | % - A 3x3 rotation/scale matrix 'dirCosine' 15 | % - A 1x3 translation vector 16 | % 17 | % Output: - A nx3 matrix of transformed vertices. 18 | % 19 | % Created by C. David Remy on 03/14/2011 20 | % MATLAB 2010a 21 | % 22 | % Documentation: 23 | % 'A MATLAB Framework For Gait Creation', 2011, C. David Remy (1), Keith 24 | % Buffinton (2), and Roland Siegwart (1), International Conference on 25 | % Intelligent Robots and Systems, September 25-30, San Francisco, USA 26 | % 27 | % (1) Autonomous Systems Lab, Institute of Robotics and Intelligent Systems, 28 | % Swiss Federal Institute of Technology (ETHZ) 29 | % Tannenstr. 3 / CLA-E-32.1 30 | % 8092 Zurich, Switzerland 31 | % cremy@ethz.ch; rsiegwart@ethz.ch 32 | % 33 | % (2) Department of Mechanical Engineering, 34 | % Bucknell University 35 | % 701 Moore Avenue 36 | % Lewisburg, PA-17837, USA 37 | % buffintk@bucknell.edu 38 | % 39 | % See also PATCH. 40 | % 41 | function vTrans = TransformVertices(v, dirCosine, translation) 42 | if isempty(v) 43 | vTrans = []; 44 | return 45 | end 46 | % rotation 47 | vTrans = (dirCosine*v')'; 48 | % translation 49 | vTrans = vTrans + repmat(translation(:)', size(vTrans,1),1); 50 | end 51 | % ************************************************************************* 52 | % ************************************************************************* 53 | -------------------------------------------------------------------------------- /Exercises/Exercise_VI/TestFiles/BetterGraphics/Vec2Struct.m: -------------------------------------------------------------------------------- 1 | % ************************************************************************* 2 | % 3 | % function struct = Vec2Struct(vec, names) 4 | % 5 | % Creates a MATLAB struct from a vector and a cell array of variable names. 6 | % 7 | % Input: - 'vec', a one dimensional column vector that contains the 8 | % numeric information that will be placed in the struct. 9 | % - 'names', a string cell array that contains the names (and if 10 | % necessary) indexes to all elements in 'vec' 11 | % Output: - A struct that contains doubles, or multidimensional (max number 12 | % of dimensions = 2) arrays of doubles. 13 | % 14 | % Example: 15 | % names = {'a' 'b(1,1)' 'b(1,2)'}; 16 | % vec = [1; 2; 3]; 17 | % struct = Vec2Struct(vec,names) 18 | % 19 | % Created by C. David Remy on 03/14/2011 20 | % MATLAB 2010a 21 | % 22 | % Documentation: 23 | % 'A MATLAB Framework For Gait Creation', 2011, C. David Remy (1), Keith 24 | % Buffinton (2), and Roland Siegwart (1), International Conference on 25 | % Intelligent Robots and Systems, September 25-30, San Francisco, USA 26 | % 27 | % (1) Autonomous Systems Lab, Institute of Robotics and Intelligent Systems, 28 | % Swiss Federal Institute of Technology (ETHZ) 29 | % Tannenstr. 3 / CLA-E-32.1 30 | % 8092 Zurich, Switzerland 31 | % cremy@ethz.ch; rsiegwart@ethz.ch 32 | % 33 | % (2) Department of Mechanical Engineering, 34 | % Bucknell University 35 | % 701 Moore Avenue 36 | % Lewisburg, PA-17837, USA 37 | % buffintk@bucknell.edu 38 | % 39 | % See also STRUCT2VEC. 40 | % 41 | function struct = Vec2Struct(vec,names) 42 | 43 | % Check input values: 44 | if (length(vec) ~= length(names)) 45 | error('GaitCreation:Vec2Struct:LengthMissmatch','Vector and name array must be of the same length'); 46 | end 47 | % Start with an empty struct... 48 | struct = []; 49 | % ... and process items one by one: 50 | for i = 1:length(vec) 51 | % Use the eval command to map the struct back: 52 | eval(['struct.',names{i},' = vec(i);']) 53 | end 54 | end 55 | % ************************************************************************* 56 | % ************************************************************************* -------------------------------------------------------------------------------- /Exercises/Exercise_VI/TestFiles/BetterGraphics/rndread.m: -------------------------------------------------------------------------------- 1 | % 2 | % from MATLAB central 3 | % 4 | % Written by Don Riley 5 | % 6 | % 7 | function [fout, vout, cout] = rndread(filename) 8 | % Reads CAD STL ASCII files, which most CAD programs can export. 9 | % Used to create Matlab patches of CAD 3D data. 10 | % Returns a vertex list and face list, for Matlab patch command. 11 | % 12 | % 13 | fid=fopen(filename, 'r'); %Open the file, assumes STL ASCII format. 14 | if fid == -1 15 | error('File could not be opened, check name or path.') 16 | end 17 | % 18 | % Render files take the form: 19 | % 20 | %solid BLOCK 21 | % color 1.000 1.000 1.000 22 | % facet 23 | % normal 0.000000e+00 0.000000e+00 -1.000000e+00 24 | % normal 0.000000e+00 0.000000e+00 -1.000000e+00 25 | % normal 0.000000e+00 0.000000e+00 -1.000000e+00 26 | % outer loop 27 | % vertex 5.000000e-01 -5.000000e-01 -5.000000e-01 28 | % vertex -5.000000e-01 -5.000000e-01 -5.000000e-01 29 | % vertex -5.000000e-01 5.000000e-01 -5.000000e-01 30 | % endloop 31 | % endfacet 32 | % 33 | % The first line is object name, then comes multiple facet and vertex lines. 34 | % A color specifier is next, followed by those faces of that color, until 35 | % next color line. 36 | % 37 | CAD_object_name = sscanf(fgetl(fid), '%*s %s'); %CAD object name, if needed. 38 | % %Some STLs have it, some don't. 39 | vnum=0; %Vertex number counter. 40 | report_num=0; %Report the status as we go. 41 | VColor = 0; 42 | % 43 | while feof(fid) == 0 % test for end of file, if not then do stuff 44 | tline = fgetl(fid); % reads a line of data from file. 45 | fword = sscanf(tline, '%s '); % make the line a character string 46 | % Check for color 47 | if strncmpi(fword, 'c',1) == 1; % Checking if a "C"olor line, as "C" is 1st char. 48 | VColor = sscanf(tline, '%*s %f %f %f'); % & if a C, get the RGB color data of the face. 49 | end % Keep this color, until the next color is used. 50 | if strncmpi(fword, 'v',1) == 1; % Checking if a "V"ertex line, as "V" is 1st char. 51 | vnum = vnum + 1; % If a V we count the # of V's 52 | v(:,vnum) = sscanf(tline, '%*s %f %f %f'); % & if a V, get the XYZ data of it. 53 | c(:,vnum) = VColor; % A color for each vertex, which will color the faces. 54 | end % we "*s" skip the name "color" and get the data. 55 | end 56 | % Build face list; The vertices are in order, so just number them. 57 | % 58 | fnum = vnum/3; %Number of faces, vnum is number of vertices. STL is triangles. 59 | flist = 1:vnum; %Face list of vertices, all in order. 60 | F = reshape(flist, 3,fnum); %Make a "3 by fnum" matrix of face list data. 61 | % 62 | % Return the faces and vertexs. 63 | % 64 | fout = F'; %Orients the array for direct use in patch. 65 | vout = v'; % " 66 | cout = c'; 67 | % 68 | fclose(fid); -------------------------------------------------------------------------------- /Exercises/Exercise_VI/TestFiles/TestData_37_ControllerEvaluation.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_VI/TestFiles/TestData_37_ControllerEvaluation.mat -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Examples/Example_11_CreateVelLambdaFigure.m: -------------------------------------------------------------------------------- 1 | function Example_11_CreateVelLambdaFigure(FigNr, titleString, phi_vec, c_dot_PLUS, Lambda) 2 | % Show output: 3 | fig = figure(FigNr); 4 | clf 5 | set(fig,'Name',titleString) 6 | % Velocities: 7 | subplot(211) 8 | hold on 9 | grid on 10 | xlabel('Leg Angle at Impact $\varphi$ [deg]','interpreter','latex') 11 | ylabel('Post Impact Vel. [$l \dot{\varphi}_{tr}$]','interpreter','latex') 12 | plot(phi_vec,c_dot_PLUS(1,:),'m') 13 | plot(phi_vec,c_dot_PLUS(2,:),'r') 14 | plot(phi_vec,c_dot_PLUS(3,:),'c') 15 | plot(phi_vec,c_dot_PLUS(4,:),'b') 16 | L1 = legend('$\dot{x}_{tr}$',... 17 | '$\dot{y}_{tr}$',... 18 | '$\dot{x}_{le}$',... 19 | '$\dot{y}_{le}$'); 20 | set(L1,'interpreter','latex','FontSize',11); 21 | axis([phi_vec(1), phi_vec(end), -2, 2]) 22 | set(L1,'location','best'); 23 | % Impulses: 24 | subplot(212) 25 | hold on 26 | grid on 27 | xlabel('Leg Angle at Impact $\varphi$ [deg]','interpreter','latex') 28 | ylabel('Impulse [$m l \dot{\varphi}_{tr}$]','interpreter','latex') 29 | plot(phi_vec,Lambda(1,:),'m') 30 | plot(phi_vec,Lambda(2,:),'r') 31 | plot(phi_vec,Lambda(3,:),'c') 32 | plot(phi_vec,Lambda(4,:),'b') 33 | L2 = legend('$\Lambda_{tr,x}$',... 34 | '$\Lambda_{tr,y}$',... 35 | '$\Lambda_{le,x}$',... 36 | '$\Lambda_{le,y}$'); 37 | set(L2,'interpreter','latex','FontSize',11); 38 | axis([phi_vec(1), phi_vec(end), -2, 2]) 39 | set(L1,'location','best'); 40 | end -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Examples/Example_15_PerformanceComparisonAnalytical.m: -------------------------------------------------------------------------------- 1 | function Example_15_PerformanceComparisonAnalytical 2 | clc 3 | %% Example for efficient and inefficient code generation: 4 | % This example shows how to compute the Mass Matrix for a simple 5 | % nq-link robot that consists of segments of length l that are all 6 | % connected by rotational joints. The COG of each segment (with mass m 7 | % and inertia j) is located at a distance s from the joint 8 | 9 | % Define kinematics recursively for nq joints in the systems 10 | nq = 6; 11 | n_trials = 100000; 12 | 13 | % Generalized Coordinates 14 | q = sym('q',[nq 1]); 15 | assume(q,'real'); 16 | % Parameters 17 | l = sym('l','real'); 18 | s = sym('s','real'); 19 | m = sym('m','real'); 20 | j = sym('j','real'); 21 | p = [l s m j]'; 22 | % Kinematics (planar): 23 | x = sym('x', [2*nq, 1]); 24 | phi = sym('phi', [nq, 1]); 25 | % Start of the chain: 26 | phi(1) = q(1); 27 | x(1:2) = s*[-sin(q(1)); cos(q(1))]; 28 | % Add the chain recursively: 29 | for i = 2:nq 30 | phi(i) = phi(i-1) + q(i); 31 | x(2*i-1:2*i) = x(2*i-3:2*i-2) + ... 32 | (l-s)*[-sin(phi(i-1)); cos(phi(i-1))] +... 33 | s*[-sin(phi(i)); cos(phi(i))]; 34 | phi(i) = phi(i); 35 | x(2*i-1:2*i) = x(2*i-1:2*i); 36 | %x = simplify(expand(x)); 37 | %phi = simplify(expand(phi)); 38 | end 39 | disp('phi:') 40 | disp(phi) 41 | disp('x:') 42 | disp(x) 43 | 44 | % Jacobians: 45 | J_R = jacobian(phi,q); 46 | J_S = jacobian(x,q); 47 | 48 | % Mass matrix: 49 | M = sym(zeros(nq)); 50 | for i = 1:nq %nq = nN (# bodies) 51 | M = M + J_R(i,:)'*j*J_R(i,:) + J_S(i*2-1:i*2,:)'*m*J_S(i*2-1:i*2,:); 52 | end 53 | disp('M:') 54 | disp(M) 55 | 56 | % Just create a handle to the function without code optimization 57 | M_fct = matlabFunction(M,'vars', {p, q}); 58 | 59 | % Use Matlab to generate optimized code: 60 | matlabFunction(M,'file','M_mat','vars', {p, q}); 61 | 62 | disp('Created Matlab functions.') 63 | 64 | % Compare performances for random configurations of many trials: 65 | l_num = 1; 66 | s_num = 0.5; 67 | m_num = 1; 68 | j_num = 1; 69 | p_num = [l_num s_num m_num j_num]'; 70 | 71 | %% Use solve 72 | for k = 1:n_trials 73 | q_num = rand([nq,1])*0; 74 | M_eval = DirectEval(p_num, q_num); 75 | end 76 | disp('Direct Solve:') 77 | disp(M_eval) 78 | 79 | %% Use the Matlab Function (which exploits recursion): 80 | for k = 1:n_trials 81 | q_num = rand([nq,1])*0; 82 | M_eval = MatlabFct(p_num, q_num); 83 | end 84 | disp('Matlab Function:') 85 | disp(M_eval) 86 | 87 | 88 | 89 | %% Embedded Functions: 90 | % Direct evalutation of the symbolically derived function: 91 | function M_num1 = DirectEval(p_num, q_num) 92 | M_num1 = M_fct(p_num, q_num); 93 | end 94 | 95 | % Using the optimized code from 'MatlabFunction' 96 | function M_num2 = MatlabFct(p_num, q_num) 97 | M_num2 = M_mat(p_num, q_num); 98 | end 99 | end 100 | -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Solutions/Solution_43.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_VII/Solutions/Solution_43.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Solutions/Solution_44_EventDetection.m: -------------------------------------------------------------------------------- 1 | function Solution_44_EventDetection() 2 | % Define timing and intial parameters: 3 | t_end = 1.0; 4 | delta_t = 0.01; 5 | q_0 = [1; 1; pi/4]; 6 | q_dot_0 = [0; 0; 0]; 7 | m = 0.5; 8 | theta = 1; 9 | grav = 9.81; 10 | d = 0; 11 | s = 0.5; % [m] 12 | % Combined vector of initial generalized positions and velocities: 13 | y_0 = [q_0; q_dot_0]; 14 | % Set up options with our event function: 15 | options = odeset('Events',@events); 16 | 17 | % run simulation/integration 18 | [t, y] = ode45(@ODE, 0:delta_t:t_end, y_0, options); 19 | 20 | % Plot results: 21 | figure; 22 | hold on; 23 | grid on; 24 | box on 25 | plot(t, y); 26 | print(gcf,'-r600','-djpeg','Problem_44_Output.jpg'); 27 | 28 | % This function implements the right hand side of the differential 29 | % equation. 30 | function y_dot = ODE(~, y) 31 | q_ = y(1:3); 32 | q_dot_ = y(4:end); 33 | A_num_ = A_fct(q_(1), q_(2), q_(3), q_dot_(1), q_dot_(2), q_dot_(3), m, theta, grav, d); 34 | b_num_ = b_fct(q_(1), q_(2), q_(3), q_dot_(1), q_dot_(2), q_dot_(3), m, theta, grav, d); 35 | x_ = A_num_\b_num_; 36 | % Extract accelerations: 37 | q_ddot_ = x_(1:3); 38 | % Prepare the output variable y_dot: 39 | y_dot = zeros(6,1); 40 | y_dot(1:3) = q_dot_; 41 | y_dot(4:end) = q_ddot_; 42 | end 43 | 44 | % This function is called at each integraton step to check whether an 45 | % event has happend (i.e., 'value' has changed it's sign). 46 | function [value, isterminal, direction] = events(~, y) 47 | % The precise definition for the constraint distance of the second 48 | % contact is "y(2)*cos(y(3))-(y(1)-s)*sin(y(3))-d = 0". Note that 49 | % this can be simplified to "y(3) = 0", since the first contact is 50 | % already enforced. 51 | value = y(2)*cos(y(3))-(y(1)-s)*sin(y(3))-d; 52 | isterminal = 1; % Abort the integration 53 | direction = -1; % Only register a downward motion 54 | end 55 | end 56 | 57 | -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Solutions/Solution_45.m: -------------------------------------------------------------------------------- 1 | clear all; clc; close all; 2 | 3 | % Define parameters: 4 | syms m theta grav d real 5 | 6 | % Define coords: 7 | syms x y phi real 8 | q = [x, y, phi]'; 9 | 10 | % Define velocities: 11 | syms dx dy dphi real 12 | dq = [dx, dy, dphi]'; 13 | 14 | %% Define equations of motion 15 | 16 | A_BI = [ cos(phi) sin(phi) 0; 17 | -sin(phi) cos(phi) 0 18 | 0 0 1]; 19 | 20 | % translational jacobian 21 | fc_S = [x;y;0]; 22 | J_S = jacobian(fc_S,q); 23 | B_J_S = A_BI * J_S; 24 | % translational bias acceleration 25 | sigma_S = arrayfun(@(x) jacobian(x,q)*dq, J_S) * dq; 26 | B_sigma_S = A_BI * sigma_S; 27 | 28 | % rotational jacobian 29 | fc_R = [0;0;phi]; 30 | J_R = jacobian(fc_R,q); 31 | B_J_R = A_BI * J_R; 32 | % rotational bias acceleration 33 | sigma_R = arrayfun(@(x) jacobian(x,q)*dq, J_R) * dq; 34 | B_sigma_R = A_BI * sigma_R; 35 | 36 | % inertia 37 | B_I_B = diag([0 0 theta]); 38 | 39 | % body rotation 40 | B_omega_B = [0;0;dphi]; 41 | 42 | % graviational force 43 | I_Fgrav = [0;-9.81;0]*m; 44 | B_Fgrav = A_BI * I_Fgrav; 45 | % graviational moment 46 | I_Mgrav = [0;0;0]; 47 | B_Mgrav = A_BI * I_Mgrav; 48 | 49 | M = B_J_S' * m * B_J_S + ... 50 | B_J_R' * B_I_B * B_J_R; 51 | M = simplify(M); 52 | 53 | f = - ( B_J_S' * m * B_sigma_S + ... 54 | B_J_R' * ( B_I_B * B_sigma_R + skew(B_omega_B) * B_I_B * B_omega_B) ); 55 | f = simplify(f); 56 | 57 | g = B_J_S' * B_Fgrav + ... 58 | B_J_R' * B_Mgrav; 59 | g = simplify(g); 60 | 61 | tau = [0;0;0]; 62 | 63 | 64 | %% Define the constraint violation and contact forces: 65 | 66 | syms s l real 67 | 68 | c = [ [0 1 0] * ( A_BI*[x;y;0] - [0;d;0] ); 69 | [0 1 0] * ( A_BI * ( [x;y;0] - [s;0;0] ) - [0;d;0]) ]; 70 | 71 | J_lambda = jacobian(c,q); 72 | 73 | sigma_lambda = arrayfun(@(x) jacobian(x,q)*dq, J_lambda) * dq; 74 | sigma_lambda = simplify(expand(sigma_lambda)); 75 | 76 | 77 | % Define the terms for the constrained equations of motion: 78 | A = [M -J_lambda'; 79 | J_lambda 0*J_lambda*J_lambda' ]; 80 | b = [f + g + tau; -sigma_lambda]; 81 | result = simplify(expand(A\b)); 82 | 83 | % calculate constraint forces 84 | lambda = result(4:5); 85 | 86 | 87 | %% Calculate Impact hypothesis (bar stops after contact) 88 | 89 | % position at impact 90 | q_m = [l;d;0]; 91 | % constraint forces at impact 92 | lambda = subs( lambda, {q(1),q(2),q(3)}, {q_m(1), q_m(2), q_m(3)}) 93 | 94 | % Jacobian before impact 95 | J_lambda_m = subs(J_lambda, {q(1),q(2),q(3)}, {q_m(1), q_m(2), q_m(3)}) 96 | 97 | % Define velocities before impact: 98 | syms dx_m dy_m dphi_m real 99 | dq_m = [dx_m, dy_m, dphi_m]'; 100 | dc_m = J_lambda_m * dq_m; 101 | 102 | % since first constraint is supposed to be closed before impact, then: 103 | dc_m(1) = 0; 104 | 105 | % hypothesis: bar stops after contact 106 | dc_p = [0;0]; 107 | 108 | M_lambda = simplify(inv(J_lambda_m / M * J_lambda_m')) 109 | 110 | impulse = simplify(expand( M_lambda * ( dc_p - dc_m ) )); 111 | 112 | 113 | % The first contact will open at the impact moment, if the first impulse is 114 | % negative: 115 | 116 | solve( impulse(1)==0, s) % check impulse(1) < 0 117 | 118 | % ANSWER: 119 | % s < l- + theta-/(m*l-) 120 | % 121 | % since theta- = 0 122 | % 123 | % s < l 124 | 125 | 126 | 127 | 128 | 129 | -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Solutions/Solution_45.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_VII/Solutions/Solution_45.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Solutions/Solution_46.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_VII/Solutions/Solution_46.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Solutions/Solution_47.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_VII/Solutions/Solution_47.pdf -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Solutions/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_VII/Solutions/Thumbs.db -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Solutions/skew.m: -------------------------------------------------------------------------------- 1 | function M = skew(w) 2 | % Generates a skew-symmetric matrix given a vector w 3 | % The initialization with zeros(3,3)*w, is done, such that this matrix 4 | % is symbolic if w is symbolic. 5 | M = zeros(3,3)*w; 6 | 7 | M(1,2) = -w(3); 8 | M(1,3) = w(2); 9 | M(2,3) = -w(1); 10 | 11 | M(2,1) = w(3); 12 | M(3,1) = -w(2); 13 | M(3,2) = w(1); 14 | end -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Templates/Template_43_ImplicitConstraints.m: -------------------------------------------------------------------------------- 1 | % Define parameters: 2 | syms m theta grav d real 3 | 4 | % Define coords: 5 | syms x y phi real 6 | q = [x, y, phi]'; 7 | 8 | % Define velocities: 9 | syms dx dy dphi real 10 | dq = [dx, dy, dphi]'; 11 | 12 | % Define the terms of the unconstrained EOMs (Part a): 13 | % ************************************************************************* 14 | % ToDo: 15 | % Derive on paper and then implement here the equations of motion by 16 | % defining the terms M, f, g, and tau 17 | % ************************************************************************* 18 | 19 | A_BI = [ cos(phi) sin(phi) 0; 20 | -sin(phi) cos(phi) 0 21 | 0 0 1]; 22 | 23 | % translational jacobian 24 | fc_S = [x;y;0]; 25 | J_S = jacobian(fc_S,q); 26 | B_J_S = A_BI * J_S; 27 | % translational bias acceleration 28 | sigma_S = arrayfun(@(x) jacobian(x,q)*dq, J_S) * dq; 29 | B_sigma_S = A_BI * sigma_S; 30 | 31 | % rotational jacobian 32 | fc_R = [0;0;phi]; 33 | J_R = jacobian(fc_R,q); 34 | B_J_R = A_BI * J_R; 35 | % rotational bias acceleration 36 | sigma_R = arrayfun(@(x) jacobian(x,q)*dq, J_R) * dq; 37 | B_sigma_R = A_BI * sigma_R; 38 | 39 | % inertia 40 | B_I_B = diag([0 0 theta]); 41 | 42 | % body rotation 43 | B_omega_B = [0;0;dphi]; 44 | 45 | % graviational force 46 | I_Fgrav = [0;-9.81;0]*m; 47 | B_Fgrav = A_BI * I_Fgrav; 48 | % graviational moment 49 | I_Mgrav = [0;0;0]; 50 | B_Mgrav = A_BI * I_Mgrav; 51 | 52 | M = B_J_S' * m * B_J_S + ... 53 | B_J_R' * B_I_B * B_J_R; 54 | M = simplify(M); 55 | 56 | f = - ( B_J_S' * m * B_sigma_S + ... 57 | B_J_R' * ( B_I_B * B_sigma_R + skew(B_omega_B) * B_I_B * B_omega_B) ); 58 | f = simplify(f); 59 | 60 | g = B_J_S' * B_Fgrav + ... 61 | B_J_R' * B_Mgrav; 62 | g = simplify(g); 63 | 64 | tau = [0;0;0]; 65 | 66 | 67 | % Define the constraint violation (Part b): 68 | % ************************************************************************* 69 | % ToDo: 70 | % Derive on paper and then implement here an equation that describes the 71 | % constraint violation c as a function of q. Then compute the associated 72 | % Jacobian and bias acceleration. Note that this is a scleronomic system, 73 | % expresssed in inertial coordinates. So you can use partial derivatives 74 | % to get J and use J_dot*q_dot for the bias accelerations. 75 | % ************************************************************************* 76 | 77 | c = [0 1 0] * ( A_BI*[x;y;0] - [0;d;0] ); 78 | J_lambda = jacobian(c,q); 79 | sigma_lambda = arrayfun(@(x) jacobian(x,q)*dq, J_lambda) * dq; 80 | sigma_lambda = simplify(expand(sigma_lambda)); 81 | 82 | % Define the terms for the constrained equations of motion: 83 | A = [M -J_lambda'; 84 | J_lambda 0 ]; 85 | b = [f + g + tau; -sigma_lambda]; 86 | result = simplify(expand(A\b)); 87 | disp('q_ddot:'); 88 | disp(result(1:3)); 89 | disp('lambda:'); 90 | disp(result(4)); 91 | 92 | % And create a matlab function for this matrix, as well as for the transformation A_IC: 93 | matlabFunction(A,'file','A_fct.m', 'vars', [x, y, phi, dx, dy, dphi, m, theta, grav, d]); 94 | matlabFunction(b,'file','b_fct.m', 'vars', [x, y, phi, dx, dy, dphi, m, theta, grav, d]); 95 | 96 | % Evaluate the EOMs with specific values (Part c): 97 | A_num = A_fct(1, 1, pi/4, 0, 0, 0, 0.5, 1, 9.81, 0); 98 | b_num = b_fct(1, 1, pi/4, 0, 0, 0, 0.5, 1, 9.81, 0); 99 | result = A_num\b_num; 100 | disp('q_ddot:'); 101 | disp(result(1:3)); 102 | disp('lambda:'); 103 | disp(result(4)); 104 | -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Templates/Template_44_EventDetection.m: -------------------------------------------------------------------------------- 1 | function TestFile_44_EventDetection() 2 | % Define timing and intial parameters: 3 | t_end = 1.0; 4 | delta_t = 0.01; 5 | q_0 = [1; 1; pi/4]; 6 | q_dot_0 = [0; 0; 0]; 7 | m = 0.5; 8 | theta = 1; 9 | grav = 9.81; 10 | d = 0; 11 | s = 0.5; % [m] 12 | % Combined vector of initial generalized positions and velocities: 13 | y_0 = [q_0; q_dot_0]; 14 | % Set up options with our event function: 15 | options = odeset('Events',@events); 16 | 17 | % run simulation/integration 18 | [t, y] = ode45(@ODE, 0:delta_t:t_end, y_0, options); 19 | 20 | % Plot results: 21 | figure; 22 | hold on; 23 | grid on; 24 | box on 25 | plot(t, y); 26 | legend('x','y','\phi','dx','dy','d\phi'); 27 | print(gcf,'-r600','-djpeg','Problem_44_Output.jpg'); 28 | 29 | % This function implements the right hand side of the differential 30 | % equation. 31 | function y_dot = ODE(~, y) 32 | q_ = y(1:3); 33 | q_dot_ = y(4:end); 34 | % ************************************************************************* 35 | % ToDo: 36 | % compute q_ddot by solving the constraint equations of motion. You can 37 | % use the autogenerated functions from problem 43 to implement this quick 38 | % ************************************************************************* 39 | A = A_fct(q_(1), q_(2), q_(3), q_dot_(1), q_dot_(2), q_dot_(3), m, theta, grav, d); 40 | b = b_fct(q_(1), q_(2), q_(3), q_dot_(1), q_dot_(2), q_dot_(3), m, theta, grav, d); 41 | result = A\b; 42 | q_ddot_ = result(1:3); 43 | % Prepare the output variable y_dot: 44 | y_dot = zeros(6,1); 45 | y_dot(1:3) = q_dot_; 46 | y_dot(4:end) = q_ddot_; 47 | end 48 | 49 | % This function is called at each integraton step to check whether an 50 | % event has happend (i.e., 'value' has changed it's sign). 51 | function [value, isterminal, direction] = events(~, y) 52 | % ************************************************************************* 53 | % ToDo: 54 | % Finish this event function. First derive on paper which function you 55 | % need to implement for "value". This is the functions that is checked for 56 | % a change in sign. Then determine wether this change needs to happen from 57 | % '-' to '+' or the other way around and set the 'direction' flag 58 | % accordingly. 'isterminal' is set to 1, such that the integration is not 59 | % continued. 60 | % ***************************** ******************************************** 61 | x_ = y(1); 62 | y_ = y(2); 63 | phi = y(3); 64 | A_BI = [ cos(phi) sin(phi) 0; 65 | -sin(phi) cos(phi) 0 66 | 0 0 1]; 67 | 68 | value = [0 1 0] * ( A_BI * ( [x_;y_;0] - [s;0;0] ) - [0;d;0]); 69 | isterminal = 1; % Abort the integration 70 | direction = -1; 71 | end 72 | end 73 | 74 | -------------------------------------------------------------------------------- /Exercises/Exercise_VII/Templates/skew.m: -------------------------------------------------------------------------------- 1 | function M = skew(w) 2 | % Generates a skew-symmetric matrix given a vector w 3 | % The initialization with zeros(3,3)*w, is done, such that this matrix 4 | % is symbolic if w is symbolic. 5 | M = zeros(3,3)*w; 6 | 7 | M(1,2) = -w(3); 8 | M(1,3) = w(2); 9 | M(2,3) = -w(1); 10 | 11 | M(2,1) = w(3); 12 | M(3,1) = -w(2); 13 | M(3,2) = w(1); 14 | end -------------------------------------------------------------------------------- /Exercises/Exercise_VII/TestFiles/TestData_46_RoboticArmCollision.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Exercise_VII/TestFiles/TestData_46_RoboticArmCollision.mat -------------------------------------------------------------------------------- /Exercises/Schedule.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Exercises/Schedule.pdf -------------------------------------------------------------------------------- /LectureNotes/0_Introduction.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/LectureNotes/0_Introduction.pdf -------------------------------------------------------------------------------- /LectureNotes/1_Fundamentals.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/LectureNotes/1_Fundamentals.pdf -------------------------------------------------------------------------------- /LectureNotes/2_RigidBodyDynamics.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/LectureNotes/2_RigidBodyDynamics.pdf -------------------------------------------------------------------------------- /LectureNotes/3_Constraints_Joints.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/LectureNotes/3_Constraints_Joints.pdf -------------------------------------------------------------------------------- /LectureNotes/4_RecursiveKinematics.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/LectureNotes/4_RecursiveKinematics.pdf -------------------------------------------------------------------------------- /LectureNotes/5_MultiBodyDynamics.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/LectureNotes/5_MultiBodyDynamics.pdf -------------------------------------------------------------------------------- /LectureNotes/6_LoopsAndContact.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/LectureNotes/6_LoopsAndContact.pdf -------------------------------------------------------------------------------- /LectureNotes/Exercise_I.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/LectureNotes/Exercise_I.pdf -------------------------------------------------------------------------------- /LectureNotes/Exercise_II.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/LectureNotes/Exercise_II.pdf -------------------------------------------------------------------------------- /LectureNotes/Exercise_III.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/LectureNotes/Exercise_III.pdf -------------------------------------------------------------------------------- /LectureNotes/Exercise_IV.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/LectureNotes/Exercise_IV.pdf -------------------------------------------------------------------------------- /LectureNotes/Exercise_V.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/LectureNotes/Exercise_V.pdf -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/UML.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Multibodydynamics_engine/matlab/UML.pdf -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/classes/EnvironmentCLASS.m: -------------------------------------------------------------------------------- 1 | % EnvironmentCLASS < handle 2 | % 3 | % Defines a 'physical' environment in which all further components are 4 | % embedded. Since this is a purely numerical tool, all 'physical' 5 | % definitions refer to vectors and coordinates given in the (inertial) 6 | % coordinates of this graphics window. 7 | % 8 | % Methods: 9 | % Env = EnvironmentCLASS() % Creates a new physical (graphical) 10 | % environment 11 | % Env.delete() % Closes the environment and removes it from 12 | % the memory 13 | % Env.toggleAxis() % Toggles between showing and hiding the axis 14 | % of the environment 15 | % Env.resetOutput() % Fits the axis to the size of the objects in 16 | % it and resets all view parameter 17 | % 18 | % Properties: 19 | % fig % Handle to the figure that stores the 20 | % graphical output 21 | % 22 | % C. David Remy remy@inm.uni-stuttgart.de 23 | % Matlab R2018 24 | % 10/12/2018 25 | % v21 26 | % 27 | classdef EnvironmentCLASS < handle 28 | % Public Properties 29 | properties 30 | fig % handle to the figure window where the graphical output happens 31 | end 32 | % Private Properties 33 | properties (SetAccess = private, GetAccess = private) 34 | % Handles to the graphical output 35 | ax 36 | light_handle 37 | % Indicates whether the graphical (inertial) axis are visible or 38 | % not. 39 | axVisible = 1; 40 | end 41 | 42 | % Public Methods 43 | methods 44 | % Constructor creates a graphics environment 45 | function obj = EnvironmentCLASS() 46 | obj.fig = figure; 47 | set(obj.fig, 'Color', [0.95 0.95 0.95]); 48 | obj.ax = axes; 49 | obj.light_handle = camlight('right'); 50 | resetOutput(obj); 51 | end 52 | % Destructor closes the figure upon deletion 53 | function delete(obj) 54 | close(obj.fig); 55 | end 56 | % Update output 57 | function resetOutput(obj) 58 | figure(obj.fig); 59 | if obj.axVisible 60 | axis(obj.ax, 'on'); 61 | else 62 | axis(obj.ax, 'off'); 63 | end 64 | axis(obj.ax, 'equal'); 65 | view(obj.ax, [-37.5,30]) 66 | axis(obj.ax, 'tight'); 67 | a = axis(obj.ax); 68 | axis(obj.ax, a+[-1,1,-1,1,-1,1]); 69 | box(obj.ax, 'on') 70 | grid(obj.ax, 'on') 71 | camproj(obj.ax, 'perspective'); 72 | camlight(obj.light_handle, 'right') 73 | view(obj.ax, [125, 25]) 74 | end 75 | % Toggle the visibility of the graphical axis: 76 | function toggleAxis(obj) 77 | obj.axVisible = 1-obj.axVisible; 78 | resetOutput(obj); 79 | end 80 | end 81 | % Private Methods 82 | % - none 83 | end -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/classes/LinkedJointObjectCLASS_v2.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedJointObjectCLASS_v2 < handle 2 | % 3 | % Defines an abstract joint object that is part of a linked list of bodies 4 | % and joints. 5 | % 6 | % Public Methods: 7 | % joint = LinkedJointObjectCLASS_v2(predBody, sucBody) 8 | % Creates an abstract joint object that links the body 9 | % specified in 'predBody' with the body specified in 'sucBody'. 10 | % Both are objects of the type 'LinkedBodyObjectCLASS', 11 | % and function as predecessor and successor in a kinematic tree. 12 | % joint.delete() 13 | % Removes the joint from the graphics output and memory 14 | % joint.recursiveOutput(spaceString) 15 | % This function recursively displays the object properties in 16 | % the matlab command window. Then calls the 'recursiveOutput' 17 | % routine of the successor body. Display starts with an 18 | % identation of 'spaceString'. 19 | % sucBody = joint.getSuccessorBody() 20 | % Returns the successor of this joint. Can be used to write 21 | % recursive algorithms that simply jump over joints. 22 | % 23 | % Public Properties: 24 | % description % A string describing the joint 25 | % 26 | % C. David Remy remy@inm.uni-stuttgart.de 27 | % Matlab R2018 28 | % 12/21/2018 29 | % v23 30 | % 31 | classdef LinkedJointObjectCLASS_v2 < handle 32 | % Public Properties 33 | properties 34 | description = ''; % A string describing the joint 35 | end 36 | % Protected Properties 37 | properties (SetAccess = protected, GetAccess = protected) 38 | predBody; % This is the predecessor of the joint in the kinematic 39 | % tree 40 | sucBody; % This is the successor of the joint in the kinematic 41 | % tree 42 | end 43 | % Public Methods 44 | methods 45 | % Constructor 46 | function obj = LinkedJointObjectCLASS_v2(predBody, sucBody) 47 | % Store the information about the predecessor and successor of 48 | % this joint: 49 | obj.predBody = predBody; 50 | obj.sucBody = sucBody; 51 | % Now link this body outward and inward, by registering it 52 | % with the parent and child bodies: 53 | sucBody.setParentJoint(obj); 54 | predBody.addChildJoint(obj); 55 | end 56 | % Destructor (empty) 57 | function delete(obj) 58 | end 59 | function recursiveOutput(obj, spaceString) 60 | disp([spaceString,'###### BEGIN JOINT #####']); 61 | % Display some information about this joint: 62 | disp([spaceString, '" ',obj.description,' "']); 63 | % recursively call the successor body and add three spaces so 64 | % we get a nice indentation: 65 | obj.sucBody.recursiveOutput([spaceString,' ']); 66 | disp([spaceString,'###### END JOINT #######']); 67 | end 68 | % This function allows accessing the private property sucBody: 69 | function sucBody = getSuccessorBody(obj) 70 | sucBody = obj.sucBody; 71 | end 72 | end 73 | % Protected Methods 74 | methods (Access = protected) 75 | % - none 76 | end 77 | end 78 | 79 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/classes/LinkedRigidBodyDynamicsCLASS_v2.m: -------------------------------------------------------------------------------- 1 | % LinkedRigidBodyDynamicsCLASS_v2 < RigidBodyDynamicsCLASS_v2 & LinkedBodyObjectCLASS_v2 2 | % 3 | % Defines a rigid body that is represented by all its 4 | % kinematic and dynamic properties and that is part of a linked list. 5 | % 6 | % Public Methods: 7 | % B = LinkedRigidBodyDynamicsCLASS_v2(env) 8 | % Creates a body in the graphical environment 'env'. The body's 9 | % initial position, velocity, and acceleration are set to 10 | % standard values. It is not connected to any joints. 11 | % B.delete() 12 | % Removes the body from the graphics output and memory 13 | % B.recursiveForwardKinematics(obj, B_r_IB, A_IB) 14 | % This function recieves the bodies position and orientation 15 | % from the parent joint, saves it, and passes it on to the child 16 | % joints. 17 | % B.recursiveGraphicsUpdate(obj) 18 | % Calls the internal 'update' function to force an update of all 19 | % graphics objects. Then recursively calls the child joints. 20 | % + All methods inherited from RigidBodyDynamicsCLASS_v2 & 21 | % LinkedBodyObjectCLASS_v2 22 | % 23 | % Public Properties: 24 | % All inherited from RigidBodyDynamicsCLASS_v2 & LinkedBodyObjectCLASS_v2 25 | % 26 | % C. David Remy remy@inm.uni-stuttgart.de 27 | % Matlab R2018 28 | % 12/21/2018 29 | % v22 30 | % 31 | classdef LinkedRigidBodyDynamicsCLASS_v2 < RigidBodyDynamicsCLASS_v2 & LinkedBodyObjectCLASS_v2 32 | % Public Properties 33 | properties 34 | % All inherited 35 | end 36 | % Protected Properties 37 | properties (SetAccess = protected, GetAccess = protected) 38 | % All inherited 39 | end 40 | % Public Methods 41 | methods 42 | % Constructor (calls superclass constructors) 43 | function obj = LinkedRigidBodyDynamicsCLASS_v2(env) 44 | % Superclass constructors must be called explicitly, as we need 45 | % to decide which arguments we pass to each constructor. 46 | % 47 | % Call superclass constructors to create a rigid dynamic body 48 | % and a body object that is part of a linked list: 49 | obj@RigidBodyDynamicsCLASS_v2(env); 50 | obj@LinkedBodyObjectCLASS_v2(); 51 | end 52 | % Destructor (empty) 53 | function delete(obj) 54 | % Superclass desctructor is called automatically 55 | end 56 | function recursiveForwardKinematics(obj, B_r_IB, A_IB) 57 | % Position and orientation, as well as velocities and 58 | % accelerations are given by the parent joint and passed in its 59 | % call of 'recursiveForwardKinematics' 60 | if obj.isRoot == false 61 | obj.A_IB = A_IB; 62 | obj.B_r_IB = B_r_IB; 63 | else 64 | obj.A_IB = eye(3); 65 | obj.B_r_IB = zeros(3,1); 66 | end 67 | 68 | if obj.isLeaf == false 69 | % If this is no leaf, recursively call the child joints: 70 | for i = 1:obj.nChildren 71 | obj.childJoints{i}.recursiveForwardKinematics(obj.B_r_IB, obj.A_IB); 72 | end 73 | end 74 | end 75 | function recursiveGraphicsUpdate(obj) 76 | % Calls the 'updateGraphics()' function to force an update of 77 | % all graphics objects. Then recursively calls the child 78 | % joints. 79 | obj.updateGraphics(); 80 | if obj.isLeaf == false 81 | % If this is no leaf, recursively call the child joints: 82 | for i = 1:obj.nChildren 83 | obj.childJoints{i}.recursiveGraphicsUpdate(); 84 | end 85 | end 86 | end 87 | end 88 | % Protected Methods 89 | methods (Access = protected) 90 | % -none 91 | end 92 | end 93 | 94 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/classes/LinkedRollingContactJointVeAcJaCLASS.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedRollingContactJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 2 | % 3 | % Defines a rolling contact joint in a kinematic tree 4 | % that is implemented as a set of linked objects. 5 | % This class allows the computation of Positions/Orientations, Velocities, 6 | % Accelerations, and Jacobians. 7 | % 8 | % Public Methods: 9 | % joint = LinkedRollingContactJointVeAcJaCLASS(env, predBody, sucBody) 10 | % Creates a rotational joint object that links the body 11 | % specified in 'predBody' with the body specified in 'sucBody'. 12 | % Both are objects of the type 'LinkedRigidBodyDynamicsVeAcJaCLASS', 13 | % and function as predecessor and successor in a kinematic tree. 14 | % The joint is shown in the graphical environment 'env'. 15 | % joint.delete() 16 | % Removes the joint from the graphics output and memory 17 | % + All methods inherited from LinkedGenericJointVeAcJaCLASS 18 | % 19 | % Public Properties: 20 | % All inherited from LinkedGenericJointVeAcJaCLASS 21 | % 22 | % C. David Remy remy@inm.uni-stuttgart.de 23 | % Matlab R2018 24 | % 12/21/2018 25 | % v22 26 | % 27 | classdef LinkedRollingContactJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 28 | % Public Properties 29 | properties (SetAccess = public, GetAccess = public) 30 | r = 0; % Rolling contact radius 31 | end 32 | % Protected Properties 33 | properties (SetAccess = protected, GetAccess = protected) 34 | % All inherited 35 | end 36 | % Public Methods 37 | methods 38 | % Constructor 39 | function obj = LinkedRollingContactJointVeAcJaCLASS(env, predBody, sucBody) 40 | % Superclass constructors must be called explicitly, as we need 41 | % to decide which arguments we pass to each constructor. 42 | % 43 | % Invoke superclass constructor to create generic joint: 44 | obj = obj@LinkedGenericJointVeAcJaCLASS(env, predBody, sucBody); 45 | end 46 | % Destructor (empty) 47 | function delete(obj) 48 | % Superclass desctructor is called automatically 49 | end 50 | end 51 | % Protected Methods 52 | methods (Access = protected) 53 | function [Dp_r_DpDs, A_DpDs] = JointFunction(obj, q) 54 | % Overwrite generic JointFunction: 55 | gamma = q; 56 | Dp_r_DpDs = [-obj.r*gamma;0;0]; 57 | A_DpDs = [+cos(gamma), -sin(gamma), 0; 58 | +sin(gamma), +cos(gamma), 0; 59 | +0 , +0 , 1]; 60 | end 61 | function [Dp_rDot_DpDs, Dp_omega_DpDs] = JointVelocity(obj, q, qDot) 62 | % Overwrite generic JointVelocity: 63 | Dp_rDot_DpDs = [-obj.r*qDot; 0; 0]; 64 | Dp_omega_DpDs = [0; 0; qDot]; 65 | end 66 | function [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] = JointAcceleration(obj, q, qDot, qDDot) 67 | % Overwrite generic JointAcceleration: 68 | Dp_rDDot_DpDs = [-obj.r*qDDot; 0; 0]; 69 | Dp_omegaDot_DpDs = [0; 0; qDDot]; 70 | end 71 | function [S, R] = JointJacobian(obj, q, qIndex, nq) 72 | % Overwrite generic JointJacobian: 73 | S = zeros(3,nq); 74 | R = zeros(3,nq); 75 | S(:,qIndex) = [-obj.r;0;0]; 76 | R(:,qIndex) = [0;0;1]; 77 | end 78 | end 79 | end 80 | 81 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/classes/LinkedRotationalJointCLASS_v2.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedRotationalJointCLASS_v2 < LinkedGenericJointCLASS_v2 2 | % 3 | % Defines a rotational joint in a kinematic tree that is implemented as a 4 | % set of linked objects. 5 | % 6 | % Public Methods: 7 | % joint = LinkedRotationalJointCLASS_v2(env, predBody, sucBody) 8 | % Creates a rotational joint object that links the body 9 | % specified in 'predBody' with the body specified in 'sucBody'. 10 | % Both are objects of the type 'LinkedRigidBodyDynamicsCLASS_v2', 11 | % and function as predecessor and successor in a kinematic tree. 12 | % The joint is shown in the graphical environment 'env'. 13 | % joint.delete() 14 | % Removes the joint from the graphics output and memory 15 | % + All methods inherited from LinkedGenericJointCLASS_v2 16 | % 17 | % Public Properties: 18 | % All inherited from LinkedGenericJointCLASS_v2 19 | % 20 | % C. David Remy remy@inm.uni-stuttgart.de 21 | % Matlab R2018 22 | % 12/21/2018 23 | % v22 24 | % 25 | classdef LinkedRotationalJointCLASS_v2 < LinkedGenericJointCLASS_v2 26 | % Public Properties 27 | properties (SetAccess = public, GetAccess = public) 28 | % All inherited 29 | end 30 | % Protected Properties 31 | properties (SetAccess = protected, GetAccess = protected) 32 | % All inherited 33 | end 34 | % Public Methods 35 | methods 36 | % Constructor 37 | function obj = LinkedRotationalJointCLASS_v2(env, predBody, sucBody) 38 | % Superclass constructors must be called explicitly, as we need 39 | % to decide which arguments we pass to each constructor. 40 | % 41 | % Invoke superclass constructor to create generic joint: 42 | obj = obj@LinkedGenericJointCLASS_v2(env, predBody, sucBody); 43 | end 44 | % Destructor (empty) 45 | function delete(obj) 46 | % Superclass desctructor is called automatically 47 | end 48 | end 49 | % Protected Methods 50 | methods (Access = protected) 51 | function [Dp_r_DpDs, A_DpDs] = JointFunction(obj, q) 52 | % Overwrite generic JointFunction: 53 | gamma = q; 54 | Dp_r_DpDs = [0;0;0]; 55 | A_DpDs = [+cos(gamma), -sin(gamma), 0; 56 | +sin(gamma), +cos(gamma), 0; 57 | +0 , +0 , 1]; 58 | end 59 | end 60 | end 61 | 62 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/classes/LinkedRotationalJointVeAcJaCLASS.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedRotationalJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 2 | % 3 | % Defines a rotational joint in a kinematic tree 4 | % that is implemented as a set of linked objects. 5 | % This class allows the computation of Positions/Orientations, Velocities, 6 | % Accelerations, and Jacobians. 7 | % 8 | % Public Methods: 9 | % joint = LinkedRotationalJointVeAcJaCLASS(env, predBody, sucBody) 10 | % Creates a rotational joint object that links the body 11 | % specified in 'predBody' with the body specified in 'sucBody'. 12 | % Both are objects of the type 'LinkedRigidBodyDynamicsVeAcJaCLASS', 13 | % and function as predecessor and successor in a kinematic tree. 14 | % The joint is shown in the graphical environment 'env'. 15 | % joint.delete() 16 | % Removes the joint from the graphics output and memory 17 | % + All methods inherited from LinkedGenericJointVeAcJaCLASS 18 | % 19 | % Public Properties: 20 | % All inherited from LinkedGenericJointVeAcJaCLASS 21 | % 22 | % C. David Remy remy@inm.uni-stuttgart.de 23 | % Matlab R2018 24 | % 12/21/2018 25 | % v22 26 | % 27 | classdef LinkedRotationalJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 28 | % Public Properties 29 | properties (SetAccess = public, GetAccess = public) 30 | % All inherited 31 | end 32 | % Protected Properties 33 | properties (SetAccess = protected, GetAccess = protected) 34 | % All inherited 35 | end 36 | % Public Methods 37 | methods 38 | % Constructor 39 | function obj = LinkedRotationalJointVeAcJaCLASS(env, predBody, sucBody) 40 | % Superclass constructors must be called explicitly, as we need 41 | % to decide which arguments we pass to each constructor. 42 | % 43 | % Invoke superclass constructor to create generic joint: 44 | obj = obj@LinkedGenericJointVeAcJaCLASS(env, predBody, sucBody); 45 | end 46 | % Destructor (empty) 47 | function delete(obj) 48 | % Superclass desctructor is called automatically 49 | end 50 | end 51 | % Protected Methods 52 | methods (Access = protected) 53 | function [Dp_r_DpDs, A_DpDs] = JointFunction(obj, q) 54 | % Overwrite generic JointFunction: 55 | gamma = q; 56 | Dp_r_DpDs = [0;0;0]; 57 | A_DpDs = [+cos(gamma), -sin(gamma), 0; 58 | +sin(gamma), +cos(gamma), 0; 59 | +0 , +0 , 1]; 60 | end 61 | function [Dp_rDot_DpDs, Dp_omega_DpDs] = JointVelocity(obj, q, qDot) 62 | % Overwrite generic JointVelocity: 63 | Dp_rDot_DpDs = zeros(3,1); 64 | Dp_omega_DpDs = [0; 0; qDot]; 65 | end 66 | function [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] = JointAcceleration(obj, q, qDot, qDDot) 67 | % Overwrite generic JointAcceleration: 68 | Dp_rDDot_DpDs = zeros(3,1); 69 | Dp_omegaDot_DpDs = [0; 0; qDDot]; 70 | end 71 | function [S, R] = JointJacobian(obj, q, qIndex, nq) 72 | % Overwrite generic JointJacobian: 73 | S = zeros(3,nq); 74 | R = zeros(3,nq); 75 | S(:,qIndex) = [0;0;0]; 76 | R(:,qIndex) = [0;0;1]; 77 | end 78 | end 79 | end 80 | 81 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/classes/LinkedTranslationalJointVeAcJaCLASS.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedTranslationalJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 2 | % 3 | % Defines a translational joint in a kinematic tree 4 | % that is implemented as a set of linked objects. 5 | % This class allows the computation of Positions/Orientations, Velocities, 6 | % Accelerations, and Jacobians. 7 | % 8 | % Public Methods: 9 | % joint = LinkedTranslationalJointVeAcJaCLASS(env, predBody, sucBody) 10 | % Creates a rotational joint object that links the body 11 | % specified in 'predBody' with the body specified in 'sucBody'. 12 | % Both are objects of the type 'LinkedRigidBodyDynamicsVeAcJaCLASS', 13 | % and function as predecessor and successor in a kinematic tree. 14 | % The joint is shown in the graphical environment 'env'. 15 | % joint.delete() 16 | % Removes the joint from the graphics output and memory 17 | % + All methods inherited from LinkedGenericJointVeAcJaCLASS 18 | % 19 | % Public Properties: 20 | % All inherited from LinkedGenericJointVeAcJaCLASS 21 | % 22 | % C. David Remy remy@inm.uni-stuttgart.de 23 | % Matlab R2018 24 | % 12/21/2018 25 | % v22 26 | % 27 | classdef LinkedTranslationalJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 28 | % Public Properties 29 | properties (SetAccess = public, GetAccess = public) 30 | % All inherited 31 | end 32 | % Protected Properties 33 | properties (SetAccess = protected, GetAccess = protected) 34 | % All inherited 35 | end 36 | % Public Methods 37 | methods 38 | % Constructor 39 | function obj = LinkedTranslationalJointVeAcJaCLASS(env, predBody, sucBody) 40 | % Superclass constructors must be called explicitly, as we need 41 | % to decide which arguments we pass to each constructor. 42 | % 43 | % Invoke superclass constructor to create generic joint: 44 | obj = obj@LinkedGenericJointVeAcJaCLASS(env, predBody, sucBody); 45 | end 46 | % Destructor (empty) 47 | function delete(obj) 48 | % Superclass desctructor is called automatically 49 | end 50 | end 51 | % Protected Methods 52 | methods (Access = protected) 53 | function [Dp_r_DpDs, A_DpDs] = JointFunction(obj, q) 54 | % Overwrite generic JointFunction: 55 | deltaX = q; 56 | Dp_r_DpDs = [deltaX;0;0]; 57 | A_DpDs = eye(3); 58 | end 59 | function [Dp_rDot_DpDs, Dp_omega_DpDs] = JointVelocity(obj, q, qDot) 60 | % Overwrite generic JointVelocity: 61 | Dp_rDot_DpDs = [qDot; 0; 0]; 62 | Dp_omega_DpDs = zeros(3,1); 63 | end 64 | function [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] = JointAcceleration(obj, q, qDot, qDDot) 65 | % Overwrite generic JointAcceleration: 66 | Dp_rDDot_DpDs = [qDDot; 0; 0]; 67 | Dp_omegaDot_DpDs = zeros(3,1); 68 | end 69 | function [S, R] = JointJacobian(obj, q, qIndex, nq) 70 | % Overwrite generic JointJacobian: 71 | S = zeros(3,nq); 72 | R = zeros(3,nq); 73 | S(:,qIndex) = [1;0;0]; 74 | R(:,qIndex) = [0;0;0]; 75 | end 76 | end 77 | end 78 | 79 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/classes/LinkedVirtual3DOFJointVeAcJaCLASS.m: -------------------------------------------------------------------------------- 1 | % classdef LinkedVirtual3DOFJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 2 | % 3 | % Defines a virtual 3 DOF joint in a kinematic tree 4 | % that is implemented as a set of linked objects. 5 | % This class allows the computation of Positions/Orientations, Velocities, 6 | % Accelerations, and Jacobians. 7 | % 8 | % Public Methods: 9 | % joint = LinkedVirtual3DOFJointVeAcJaCLASS(env, predBody, sucBody) 10 | % Creates a rotational joint object that links the body 11 | % specified in 'predBody' with the body specified in 'sucBody'. 12 | % Both are objects of the type 'LinkedRigidBodyDynamicsVeAcJaCLASS', 13 | % and function as predecessor and successor in a kinematic tree. 14 | % The joint is shown in the graphical environment 'env'. 15 | % joint.delete() 16 | % Removes the joint from the graphics output and memory 17 | % + All methods inherited from LinkedGenericJointVeAcJaCLASS 18 | % 19 | % Public Properties: 20 | % All inherited from LinkedGenericJointVeAcJaCLASS 21 | % 22 | % C. David Remy remy@inm.uni-stuttgart.de 23 | % Matlab R2018 24 | % 12/21/2018 25 | % v22 26 | % 27 | classdef LinkedVirtual3DOFJointVeAcJaCLASS < LinkedGenericJointVeAcJaCLASS 28 | % Public Properties 29 | properties (SetAccess = public, GetAccess = public) 30 | % All inherited 31 | end 32 | % Protected Properties 33 | properties (SetAccess = protected, GetAccess = protected) 34 | % All inherited 35 | end 36 | % Public Methods 37 | methods 38 | % Constructor 39 | function obj = LinkedVirtual3DOFJointVeAcJaCLASS(env, predBody, sucBody) 40 | % Superclass constructors must be called explicitly, as we need 41 | % to decide which arguments we pass to each constructor. 42 | % 43 | % Invoke superclass constructor to create generic joint: 44 | obj = obj@LinkedGenericJointVeAcJaCLASS(env, predBody, sucBody); 45 | end 46 | % Destructor (empty) 47 | function delete(obj) 48 | % Superclass desctructor is called automatically 49 | end 50 | end 51 | % Protected Methods 52 | methods (Access = protected) 53 | function [Dp_r_DpDs, A_DpDs] = JointFunction(obj, q) 54 | % Overwrite generic JointFunction: 55 | deltaX = q(1); 56 | deltaY = q(2); 57 | gamma = q(3); 58 | Dp_r_DpDs = [deltaX; deltaY;0]; 59 | A_DpDs = [+cos(gamma), -sin(gamma), 0; 60 | +sin(gamma), +cos(gamma), 0; 61 | +0 , +0 , 1]; 62 | end 63 | function [Dp_rDot_DpDs, Dp_omega_DpDs] = JointVelocity(obj, q, qDot) 64 | % Overwrite generic JointVelocity: 65 | Dp_rDot_DpDs = [qDot(1); qDot(2); 0]; 66 | Dp_omega_DpDs = [0; 0; qDot(3)]; 67 | end 68 | function [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] = JointAcceleration(obj, q, qDot, qDDot) 69 | % Overwrite generic JointAcceleration: 70 | Dp_rDDot_DpDs = [qDDot(1); qDDot(2); 0]; 71 | Dp_omegaDot_DpDs = [0; 0; qDDot(3)]; 72 | end 73 | function [S, R] = JointJacobian(obj, q, qIndex, nq) 74 | % Overwrite generic JointJacobian: 75 | S = zeros(3,nq); 76 | R = zeros(3,nq); 77 | S(:,qIndex) = [1,0,0;0,1,0;0,0,0]; 78 | R(:,qIndex) = [0,0,0;0,0,0;0,0,1]; 79 | end 80 | end 81 | end 82 | 83 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/classes/skew.m: -------------------------------------------------------------------------------- 1 | function M = skew(w) 2 | % Generates a skew-symmetric matrix given a vector w 3 | % The initialization with zeros(3,3)*w, is done, such that this matrix 4 | % is symbolic if w is symbolic. 5 | M = zeros(3,3)*w; 6 | 7 | M(1,2) = -w(3); 8 | M(1,3) = w(2); 9 | M(2,3) = -w(1); 10 | 11 | M(2,1) = w(3); 12 | M(3,1) = -w(2); 13 | M(3,2) = w(1); 14 | end -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/testfiles/Solution_43.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Multibodydynamics_engine/matlab/testfiles/Solution_43.pdf -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/testfiles/Solution_44_EventDetection.m: -------------------------------------------------------------------------------- 1 | function Solution_44_EventDetection() 2 | % Define timing and intial parameters: 3 | t_end = 1.0; 4 | delta_t = 0.01; 5 | q_0 = [1; 1; pi/4]; 6 | q_dot_0 = [0; 0; 0]; 7 | m = 0.5; 8 | theta = 1; 9 | grav = 9.81; 10 | d = 0; 11 | s = 0.5; % [m] 12 | % Combined vector of initial generalized positions and velocities: 13 | y_0 = [q_0; q_dot_0]; 14 | % Set up options with our event function: 15 | options = odeset('Events',@events); 16 | 17 | % run simulation/integration 18 | [t, y] = ode45(@ODE, 0:delta_t:t_end, y_0, options); 19 | 20 | % Plot results: 21 | figure; 22 | hold on; 23 | grid on; 24 | box on 25 | plot(t, y); 26 | print(gcf,'-r600','-djpeg','Problem_44_Output.jpg'); 27 | 28 | % This function implements the right hand side of the differential 29 | % equation. 30 | function y_dot = ODE(~, y) 31 | q_ = y(1:3); 32 | q_dot_ = y(4:end); 33 | A_num_ = A_fct(q_(1), q_(2), q_(3), q_dot_(1), q_dot_(2), q_dot_(3), m, theta, grav, d); 34 | b_num_ = b_fct(q_(1), q_(2), q_(3), q_dot_(1), q_dot_(2), q_dot_(3), m, theta, grav, d); 35 | x_ = A_num_\b_num_; 36 | % Extract accelerations: 37 | q_ddot_ = x_(1:3); 38 | % Prepare the output variable y_dot: 39 | y_dot = zeros(6,1); 40 | y_dot(1:3) = q_dot_; 41 | y_dot(4:end) = q_ddot_; 42 | end 43 | 44 | % This function is called at each integraton step to check whether an 45 | % event has happend (i.e., 'value' has changed it's sign). 46 | function [value, isterminal, direction] = events(~, y) 47 | % The precise definition for the constraint distance of the second 48 | % contact is "y(2)*cos(y(3))-(y(1)-s)*sin(y(3))-d = 0". Note that 49 | % this can be simplified to "y(3) = 0", since the first contact is 50 | % already enforced. 51 | value = y(2)*cos(y(3))-(y(1)-s)*sin(y(3))-d; 52 | isterminal = 1; % Abort the integration 53 | direction = -1; % Only register a downward motion 54 | end 55 | end 56 | 57 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/testfiles/Solution_45.m: -------------------------------------------------------------------------------- 1 | clear all; clc; close all; 2 | 3 | % Define parameters: 4 | syms m theta grav d real 5 | 6 | % Define coords: 7 | syms x y phi real 8 | q = [x, y, phi]'; 9 | 10 | % Define velocities: 11 | syms dx dy dphi real 12 | dq = [dx, dy, dphi]'; 13 | 14 | %% Define equations of motion 15 | 16 | A_BI = [ cos(phi) sin(phi) 0; 17 | -sin(phi) cos(phi) 0 18 | 0 0 1]; 19 | 20 | % translational jacobian 21 | fc_S = [x;y;0]; 22 | J_S = jacobian(fc_S,q); 23 | B_J_S = A_BI * J_S; 24 | % translational bias acceleration 25 | sigma_S = arrayfun(@(x) jacobian(x,q)*dq, J_S) * dq; 26 | B_sigma_S = A_BI * sigma_S; 27 | 28 | % rotational jacobian 29 | fc_R = [0;0;phi]; 30 | J_R = jacobian(fc_R,q); 31 | B_J_R = A_BI * J_R; 32 | % rotational bias acceleration 33 | sigma_R = arrayfun(@(x) jacobian(x,q)*dq, J_R) * dq; 34 | B_sigma_R = A_BI * sigma_R; 35 | 36 | % inertia 37 | B_I_B = diag([0 0 theta]); 38 | 39 | % body rotation 40 | B_omega_B = [0;0;dphi]; 41 | 42 | % graviational force 43 | I_Fgrav = [0;-9.81;0]*m; 44 | B_Fgrav = A_BI * I_Fgrav; 45 | % graviational moment 46 | I_Mgrav = [0;0;0]; 47 | B_Mgrav = A_BI * I_Mgrav; 48 | 49 | M = B_J_S' * m * B_J_S + ... 50 | B_J_R' * B_I_B * B_J_R; 51 | M = simplify(M); 52 | 53 | f = - ( B_J_S' * m * B_sigma_S + ... 54 | B_J_R' * ( B_I_B * B_sigma_R + skew(B_omega_B) * B_I_B * B_omega_B) ); 55 | f = simplify(f); 56 | 57 | g = B_J_S' * B_Fgrav + ... 58 | B_J_R' * B_Mgrav; 59 | g = simplify(g); 60 | 61 | tau = [0;0;0]; 62 | 63 | 64 | %% Define the constraint violation and contact forces: 65 | 66 | syms s l real 67 | 68 | c = [ [0 1 0] * ( A_BI*[x;y;0] - [0;d;0] ); 69 | [0 1 0] * ( A_BI * ( [x;y;0] - [s;0;0] ) - [0;d;0]) ]; 70 | 71 | J_lambda = jacobian(c,q); 72 | 73 | sigma_lambda = arrayfun(@(x) jacobian(x,q)*dq, J_lambda) * dq; 74 | sigma_lambda = simplify(expand(sigma_lambda)); 75 | 76 | 77 | % Define the terms for the constrained equations of motion: 78 | A = [M -J_lambda'; 79 | J_lambda 0*J_lambda*J_lambda' ]; 80 | b = [f + g + tau; -sigma_lambda]; 81 | result = simplify(expand(A\b)); 82 | 83 | % calculate constraint forces 84 | lambda = result(4:5); 85 | 86 | 87 | %% Calculate Impact hypothesis (bar stops after contact) 88 | 89 | % position at impact 90 | q_m = [l;d;0]; 91 | % constraint forces at impact 92 | lambda = subs( lambda, {q(1),q(2),q(3)}, {q_m(1), q_m(2), q_m(3)}) 93 | 94 | % Jacobian before impact 95 | J_lambda_m = subs(J_lambda, {q(1),q(2),q(3)}, {q_m(1), q_m(2), q_m(3)}) 96 | 97 | % Define velocities before impact: 98 | syms dx_m dy_m dphi_m real 99 | dq_m = [dx_m, dy_m, dphi_m]'; 100 | dc_m = J_lambda_m * dq_m; 101 | 102 | % since first constraint is supposed to be closed before impact, then: 103 | dc_m(1) = 0; 104 | 105 | % hypothesis: bar stops after contact 106 | dc_p = [0;0]; 107 | 108 | M_lambda = simplify(inv(J_lambda_m / M * J_lambda_m')) 109 | 110 | impulse = simplify(expand( M_lambda * ( dc_p - dc_m ) )); 111 | 112 | 113 | % The first contact will open at the impact moment, if the first impulse is 114 | % negative: 115 | 116 | solve( impulse(1)==0, s) % check impulse(1) < 0 117 | 118 | % ANSWER: 119 | % s < l- + theta-/(m*l-) 120 | % 121 | % since theta- = 0 122 | % 123 | % s < l 124 | 125 | 126 | 127 | 128 | 129 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/testfiles/Solution_45.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Multibodydynamics_engine/matlab/testfiles/Solution_45.pdf -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/testfiles/Solution_46.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Multibodydynamics_engine/matlab/testfiles/Solution_46.pdf -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/testfiles/TestData_30_31_TriplePendulumSim.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Multibodydynamics_engine/matlab/testfiles/TestData_30_31_TriplePendulumSim.mat -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/testfiles/TestData_32_RobotTrajectory.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Multibodydynamics_engine/matlab/testfiles/TestData_32_RobotTrajectory.mat -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/testfiles/TestData_34_RunningData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Multibodydynamics_engine/matlab/testfiles/TestData_34_RunningData.mat -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/testfiles/TestData_34_WalkingData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Multibodydynamics_engine/matlab/testfiles/TestData_34_WalkingData.mat -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/testfiles/TestData_37_ControllerEvaluation.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Multibodydynamics_engine/matlab/testfiles/TestData_37_ControllerEvaluation.mat -------------------------------------------------------------------------------- /Multibodydynamics_engine/matlab/testfiles/TestFile_30_RotationalJoint_v2.m: -------------------------------------------------------------------------------- 1 | %% INITIALIZE: 2 | clear all 3 | close all 4 | clc 5 | 6 | % Create a 'physical' (graphical) environment: 7 | env = EnvironmentCLASS(); 8 | 9 | 10 | % Define a triple pendulum 11 | % Define the bodies: 12 | ground = LinkedRigidBodyDynamicsCLASS_v2(env); 13 | ground.bodyName = 'Ground'; 14 | ground.autoUpdate = false; 15 | ground.m_B = 1; % Since this is ground, m and I do not matter 16 | ground.B_I_B = eye(3)*0.0001; 17 | ground.scale = 0.5; % Defines how large the CoSys will be drawn 18 | link1 = LinkedRigidBodyDynamicsCLASS_v2(env); 19 | link1.bodyName = 'Link 1'; 20 | link1.autoUpdate = false; 21 | link1.m_B = 10; % m and I just given for visualization 22 | link1.B_I_B = diag([0.0042,0.8354,0.8354]); 23 | link1.scale = 0.5; % Defines how large the CoSys will be drawn 24 | link2 = LinkedRigidBodyDynamicsCLASS_v2(env); 25 | link2.bodyName = 'Link 2'; 26 | link2.autoUpdate = false; 27 | link2.m_B = 10; % m and I just given for visualization 28 | link2.B_I_B = diag([0.0042,0.8354,0.8354]); 29 | link2.scale = 0.5; % Defines how large the CoSys will be drawn 30 | link3 = LinkedRigidBodyDynamicsCLASS_v2(env); 31 | link3.bodyName = 'Link 3'; 32 | link3.autoUpdate = false; 33 | link3.m_B = 10; % m and I just given for visualization 34 | link3.B_I_B = diag([0.0042,0.8354,0.8354]); 35 | link3.scale = 0.5; % Defines how large the CoSys will be drawn 36 | 37 | % Define the joints and the interconnection of the bodies: 38 | joint1 = LinkedRotationalJointCLASS_v2(env, ground, link1); 39 | joint1.P_r_PDp = [0;0;0]; 40 | joint1.A_PDp = [0,1,0;-1,0,0;0,0,1]; 41 | joint1.S_r_SDs = [-0.5;0;0]; 42 | joint1.A_SDs = eye(3); 43 | joint1.jointName = 'Joint 1'; 44 | joint1.autoUpdate = false; 45 | joint1.scale = 0.2; 46 | 47 | joint2 = LinkedRotationalJointCLASS_v2(env, link1, link2); 48 | joint2.P_r_PDp = [+0.5;0;0]; 49 | joint2.A_PDp = eye(3); 50 | joint2.S_r_SDs = [-0.5;0;0]; 51 | joint2.A_SDs = eye(3); 52 | joint2.jointName = 'Joint 2'; 53 | joint2.autoUpdate = false; 54 | joint2.scale = 0.2; 55 | 56 | joint3 = LinkedRotationalJointCLASS_v2(env, link2, link3); 57 | joint3.P_r_PDp = [+0.5;0;0]; 58 | joint3.A_PDp = eye(3); 59 | joint3.S_r_SDs = [-0.5;0;0]; 60 | joint3.A_SDs = eye(3); 61 | joint3.jointName = 'Joint 3'; 62 | joint3.autoUpdate = false; 63 | joint3.scale = 0.2; 64 | 65 | % Show the topology: 66 | ground.recursiveOutput(''); 67 | 68 | % Define the joint angles: 69 | joint1.q = +pi/6; 70 | joint2.q = -pi/4; 71 | joint3.q = +pi/3; 72 | 73 | % Compute the forward kinematics starting with a root at position and 74 | % orientation zero: 75 | ground.recursiveForwardKinematics(); 76 | % Update the graphics 77 | ground.recursiveGraphicsUpdate(); 78 | 79 | % Save final screen shot: 80 | axis([-1,1,-3,3,-3.5,0.5]); 81 | print(gcf,'-r600','-djpeg','Problem_30_Output.jpg'); 82 | 83 | 84 | 85 | %% Animation: 86 | pause(3); 87 | data = load('TestData_30_31_TriplePendulumSim.mat'); 88 | 89 | delta_t_show = 0.05; 90 | next_t_output = data.t(1); 91 | 92 | while next_t_output < data.t(end) 93 | ind = find( data.t>=next_t_output, 1); 94 | 95 | % Update the joint angles: 96 | joint1.q = pi/2 + data.q1(ind); 97 | joint2.q = data.q2(ind); 98 | joint3.q = data.q3(ind); 99 | 100 | % Compute the forward kinematics starting with a root at position and 101 | % orientation zero: 102 | ground.recursiveForwardKinematics(); 103 | % Update the graphics 104 | ground.recursiveGraphicsUpdate(); 105 | drawnow; 106 | 107 | next_t_output = next_t_output + delta_t_show; 108 | end 109 | 110 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/.ipynb_checkpoints/Untitled-checkpoint.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [], 3 | "metadata": {}, 4 | "nbformat": 4, 5 | "nbformat_minor": 2 6 | } 7 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/.vscode/.ropeproject/objectdb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Multibodydynamics_engine/python/.vscode/.ropeproject/objectdb -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // Use IntelliSense to learn about possible attributes. 3 | // Hover to view descriptions of existing attributes. 4 | // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "Python: Current File", 9 | "type": "python", 10 | "request": "launch", 11 | "program": "${file}", 12 | "console": "integratedTerminal", 13 | // "args" : ["-m"] 14 | "cwd": "${workspaceFolder}" 15 | } 16 | ] 17 | } -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.pythonPath": "/home/lucas/anaconda3/envs/GP2/bin/python" 3 | } -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/classes/PositionBilateralConstraint.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Bilateral Constraint making 2 points in different bodies coincide (one might be ground) 3 | 4 | Created by: Lucas Rath (lucasrm25@gmail.com) 5 | ''' 6 | 7 | from numpy import append, array, size, ones, zeros, eye, ndarray, absolute 8 | from numpy.linalg import matrix_power, norm 9 | from .robotics_helpfuns import skew 10 | from .RigidBody import RigidBody 11 | 12 | from vpython import canvas, vector, color, rate, helix, arrow 13 | from classes.vpython_ext import vellipsoid 14 | 15 | 16 | class PositionBilateralConstraint(): 17 | ''' 18 | Abstract Generic Joint class 19 | ''' 20 | 21 | def __init__(self, predBody:RigidBody, sucBody:RigidBody, 22 | A_PDp = eye(3), A_SDs = eye(3), 23 | P_r_PDp = zeros([3,1]), S_r_SDs = zeros([3,1]), 24 | wn=10, ksi=1): 25 | # link pred and suc bodies to current joint 26 | self.predBody = predBody 27 | self.sucBody = sucBody 28 | 29 | # store static properties 30 | self.A_PDp = A_PDp 31 | self.A_SDs = A_SDs 32 | self.P_r_PDp = P_r_PDp 33 | self.S_r_SDs = S_r_SDs 34 | self.K = wn**2 # Baumgarte proportional stabilization constant 35 | self.D = 2*ksi*wn # Baumgarte derivative stabilization constant 36 | 37 | 38 | def getConstraintTerms(self): 39 | ''' 40 | Compute constraint terms 41 | ''' 42 | 43 | # calculate displacement and velocity constraint violation 44 | I_c = self.predBody.I_r_IQ(B_r_BQ=self.P_r_PDp) - self.sucBody.I_r_IQ( B_r_BQ = self.S_r_SDs ) 45 | I_cDot = self.predBody.I_v_Q(B_r_BQ=self.P_r_PDp) - self.sucBody.I_v_Q( B_r_BQ = self.S_r_SDs ) 46 | 47 | # calculate jacobian J_lambda, such that: cDot = J_lambda * qDot, which is the ratio of the constraint 48 | # displacement to the generalized coordinates 49 | J_lambda = self.predBody.A_IB @ ( self.predBody.B_J_S - skew(self.P_r_PDp) @ self.predBody.B_J_R ) -\ 50 | self.sucBody.A_IB @ ( self.sucBody.B_J_S - skew(self.S_r_SDs) @ self.sucBody.B_J_R ) 51 | 52 | sigma_lambda = self.predBody.I_a_Q( B_r_BQ=self.P_r_PDp ) - self.sucBody.I_a_Q( B_r_BQ=self.S_r_SDs ) 53 | 54 | # add Baumgarte stabilization 55 | nc = sigma_lambda.size 56 | sigma_lambda += eye(nc)*self.D @ I_cDot + eye(nc)*self.K @ I_c 57 | 58 | return [J_lambda, sigma_lambda] 59 | 60 | 61 | ''' -------------------- GRAPHICS ------------------- ''' 62 | 63 | 64 | def initGraphics(self): 65 | self.arrow = arrow(pos=vector(0,0,0), axis=vector(1,0,0)) # , shaftwidth=shaftwidth 66 | 67 | def updateGraphics(self): 68 | origin = self.predBody.A_IB @ (self.predBody.B_r_IB + self.P_r_PDp ) 69 | target = self.sucBody.A_IB @ (self.sucBody.B_r_IB + self.S_r_SDs ) 70 | self.arrow.pos = vector( *(origin) ) 71 | self.arrow.axis = vector( *(target-origin) ) 72 | 73 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/classes/RigidJoint.py: -------------------------------------------------------------------------------- 1 | from numpy import array, ones, zeros, eye, cos, sin, asscalar 2 | from .GenericJoint import GenericJoint 3 | from .robotics_helpfuns import skew 4 | 5 | class RigidJoint(GenericJoint): 6 | 7 | def __init__(self, predBody, sucBody, 8 | A_PDp = eye(3), A_SDs = eye(3), 9 | P_r_PDp = zeros([3,1]), S_r_SDs = zeros([3,1])): 10 | 11 | super().__init__(predBody, sucBody, A_PDp, A_SDs, P_r_PDp, S_r_SDs) 12 | # init generalized coordinates -> joint angle 13 | self.q = self.qDot = self.qDDot = array([0]) 14 | self.dof = 0 15 | 16 | def JointFunction(self, q): # -> [Dp_r_DpDs, A_DpDs] 17 | Dp_r_DpDs = zeros([3,1]) 18 | A_DpDs = eye(3) 19 | return [Dp_r_DpDs, A_DpDs] 20 | 21 | def JointVelocity(self, q, qDot): # -> [Dp_rDot_DpDs, Dp_omega_DpDs] 22 | # Overwrite generic JointVelocity: 23 | Dp_rDot_DpDs = zeros([3,1]) 24 | Dp_omega_DpDs = zeros([3,1]) 25 | return [Dp_rDot_DpDs, Dp_omega_DpDs] 26 | 27 | def JointAcceleration(self, q, qDot, qDDot): # -> [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] 28 | # Overwrite generic JointAcceleration: 29 | Dp_rDDot_DpDs = zeros([3,1]) 30 | Dp_omegaDot_DpDs = zeros([3,1]) 31 | return [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] 32 | 33 | def JointJacobian(self, q, qIndex, nq): # -> [S, R] 34 | # Overwrite generic JointJacobian: 35 | S = zeros([3,nq]) 36 | R = zeros([3,nq]) 37 | return [S, R] -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/classes/RollingContactJoint.py: -------------------------------------------------------------------------------- 1 | from numpy import array, ones, zeros, eye, cos, sin, asscalar 2 | from .GenericJoint import GenericJoint 3 | from .robotics_helpfuns import skew 4 | 5 | class RotationalJoint(GenericJoint): 6 | 7 | def __init__(self, predBody, sucBody, 8 | A_PDp = eye(3), A_SDs = eye(3), 9 | P_r_PDp = zeros([3,1]), S_r_SDs = zeros([3,1]), r=0): 10 | 11 | super().__init__(predBody, sucBody, A_PDp, A_SDs, P_r_PDp, S_r_SDs) 12 | # rolling contact radius 13 | self.r = r 14 | # init generalized coordinates -> joint angle 15 | self.q = self.qDot = self.qDDot = array([0]) 16 | self.dof = 1 17 | 18 | def JointFunction(self, q): # -> [Dp_r_DpDs, A_DpDs] 19 | gamma = asscalar(q) 20 | Dp_r_DpDs = array([[-self.r*gamma,0,0]]).T 21 | A_DpDs = array([[cos(gamma), -sin(gamma), 0], 22 | [sin(gamma), +cos(gamma), 0], 23 | [0 , +0 , 1]]) 24 | return [Dp_r_DpDs, A_DpDs] 25 | 26 | def JointVelocity(self, q, qDot): # -> [Dp_rDot_DpDs, Dp_omega_DpDs] 27 | # Overwrite generic JointVelocity: 28 | Dp_rDot_DpDs = array([[-self.r*qDot,0,0]]).T 29 | Dp_omega_DpDs = array([[0,0,qDot]]).T 30 | return [Dp_rDot_DpDs, Dp_omega_DpDs] 31 | 32 | def JointAcceleration(self, q, qDot, qDDot): # -> [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] 33 | # Overwrite generic JointAcceleration: 34 | Dp_rDDot_DpDs = array([[-self.r*qDDot,0,0]]).T 35 | Dp_omegaDot_DpDs = array([[0,0,qDDot]]).T 36 | return [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] 37 | 38 | def JointJacobian(self, q, qIndex, nq): # -> [S, R] 39 | # Overwrite generic JointJacobian: 40 | S = zeros([3,nq]) 41 | R = zeros([3,nq]) 42 | S[:,qIndex] = [-self.r,0,0] 43 | R[:,qIndex] = [0,0,1] 44 | return [S, R] -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/classes/RotationalJoint.py: -------------------------------------------------------------------------------- 1 | from numpy import array, ones, zeros, eye, cos, sin, asscalar 2 | from .GenericJoint import GenericJoint 3 | from .robotics_helpfuns import skew 4 | 5 | class RotationalJoint(GenericJoint): 6 | 7 | def __init__(self, predBody, sucBody, 8 | A_PDp = eye(3), A_SDs = eye(3), 9 | P_r_PDp = zeros([3,1]), S_r_SDs = zeros([3,1])): 10 | 11 | super().__init__(predBody, sucBody, A_PDp, A_SDs, P_r_PDp, S_r_SDs) 12 | # init generalized coordinates -> joint angle 13 | self.q = self.qDot = self.qDDot = array([0]) 14 | self.dof = 1 15 | 16 | def JointFunction(self, q): # -> [Dp_r_DpDs, A_DpDs] 17 | gamma = asscalar(q) 18 | Dp_r_DpDs = zeros([3,1]) 19 | A_DpDs = array([[cos(gamma), -sin(gamma), 0], 20 | [sin(gamma), +cos(gamma), 0], 21 | [0 , +0 , 1]]) 22 | return [Dp_r_DpDs, A_DpDs] 23 | 24 | def JointVelocity(self, q, qDot): # -> [Dp_rDot_DpDs, Dp_omega_DpDs] 25 | # Overwrite generic JointVelocity: 26 | Dp_rDot_DpDs = zeros([3,1]) 27 | Dp_omega_DpDs = array([[0,0,qDot]]).T 28 | return [Dp_rDot_DpDs, Dp_omega_DpDs] 29 | 30 | def JointAcceleration(self, q, qDot, qDDot): # -> [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] 31 | # Overwrite generic JointAcceleration: 32 | Dp_rDDot_DpDs = zeros([3,1]) 33 | Dp_omegaDot_DpDs = array([[0,0,qDDot]]).T 34 | return [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] 35 | 36 | def JointJacobian(self, q, qIndex, nq): # -> [S, R] 37 | # Overwrite generic JointJacobian: 38 | S = zeros([3,nq]) 39 | R = zeros([3,nq]) 40 | S[:,qIndex] = [0,0,0] 41 | R[:,qIndex] = [0,0,1] 42 | return [S, R] -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/classes/SpringDamper.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Defines Spring/Damper component that interacts with the multi-body system 3 | 4 | Created by: Lucas Rath (lucasrm25@gmail.com) 5 | ''' 6 | 7 | from numpy import append, array, size, ones, zeros, eye, ndarray, absolute 8 | from numpy.linalg import matrix_power, norm 9 | from .robotics_helpfuns import skew 10 | from .RigidBody import RigidBody 11 | 12 | from vpython import canvas, vector, color, rate, helix 13 | from classes.vpython_ext import vellipsoid 14 | 15 | 16 | class SpringDamper(): 17 | ''' 18 | Abstract Generic Joint class 19 | ''' 20 | 21 | def __init__(self, predBody:RigidBody, sucBody:RigidBody, 22 | A_PDp = eye(3), A_SDs = eye(3), 23 | P_r_PDp = zeros([3,1]), S_r_SDs = zeros([3,1]), 24 | d0 = 0, K=100, D=5, 25 | radius=0.05, coils=20): 26 | # link pred and suc bodies to current joint 27 | self.predBody = predBody 28 | self.sucBody = sucBody 29 | 30 | # store static properties 31 | self.A_PDp = A_PDp 32 | self.A_SDs = A_SDs 33 | self.P_r_PDp = P_r_PDp 34 | self.S_r_SDs = S_r_SDs 35 | self.d0 = d0 36 | self.K = K 37 | self.D = D 38 | 39 | # store graphic properties 40 | self.radius = radius 41 | self.coils = coils 42 | 43 | 44 | def computationOfTau(self) -> ndarray: 45 | ''' 46 | Compute generalized force 47 | TODO: Check from which body the update is coming and calculate: 48 | - A_IDp, Dp_J_S, Dp_J_R and A_IDs, Ds_J_S, Ds_J_R 49 | 50 | such that I_J_S = (A_IDp @ Dp_J_S - A_IDs @ Ds_J_S) 51 | ''' 52 | # calculate displacement and velocity vectors between attaching points of the spring/damper 53 | I_r_DpDs = self.predBody.I_r_IQ(B_r_BQ=self.P_r_PDp) - self.sucBody.I_r_IQ (B_r_BQ = self.S_r_SDs ) 54 | I_v_DpDs = self.predBody.I_v_Q(B_r_BQ=self.P_r_PDp) - self.sucBody.I_v_Q (B_r_BQ = self.S_r_SDs ) 55 | 56 | # calculate jacobian I_J_S, such that: dDot = I_J_S * qDot, which is the ratio of the spring/damper 57 | # displacement to the generalized coordinates 58 | I_J_S = self.predBody.A_IB @ (self.predBody.B_J_S - skew(self.P_r_PDp) @ self.predBody.B_J_R ) -\ 59 | self.sucBody.A_IB @ (self.sucBody.B_J_S - skew(self.S_r_SDs) @ self.sucBody.B_J_R ) 60 | 61 | # calculate generalized forces 62 | tau = I_J_S.T @ ( - self.K * I_r_DpDs * (1-self.d0/norm(I_r_DpDs)) - self.D * I_v_DpDs) 63 | return tau 64 | 65 | 66 | 67 | ''' -------------------- GRAPHICS ------------------- ''' 68 | 69 | 70 | def initGraphics(self): 71 | self.helix = helix(pos=vector(0,0,0), axis=vector(1,0,0), radius=self.radius, coils=self.coils) 72 | 73 | def updateGraphics(self): 74 | origin = self.predBody.A_IB @ (self.predBody.B_r_IB + self.P_r_PDp ) 75 | target = self.sucBody.A_IB @ (self.sucBody.B_r_IB + self.S_r_SDs ) 76 | self.helix.pos = vector( *(origin) ) 77 | self.helix.axis = vector( *(target-origin) ) 78 | 79 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/classes/TranslationalJoint.py: -------------------------------------------------------------------------------- 1 | from numpy import array, ones, zeros, eye, cos, sin, asscalar 2 | from .GenericJoint import GenericJoint 3 | from .robotics_helpfuns import skew 4 | 5 | class TranslationalJoint(GenericJoint): 6 | 7 | def __init__(self, predBody, sucBody, 8 | A_PDp = eye(3), A_SDs = eye(3), 9 | P_r_PDp = zeros([3,1]), S_r_SDs = zeros([3,1])): 10 | 11 | super().__init__(predBody, sucBody, A_PDp, A_SDs, P_r_PDp, S_r_SDs) 12 | # init generalized coordinates -> joint angle 13 | self.q = self.qDot = self.qDDot = array([0]) 14 | self.dof = 1 15 | 16 | def JointFunction(self, q): # -> [Dp_r_DpDs, A_DpDs] 17 | deltaX = asscalar(q) 18 | Dp_r_DpDs = array([[deltaX,0,0]]).T 19 | A_DpDs = eye(3) 20 | return [Dp_r_DpDs, A_DpDs] 21 | 22 | def JointVelocity(self, q, qDot): # -> [Dp_rDot_DpDs, Dp_omega_DpDs] 23 | # Overwrite generic JointVelocity: 24 | Dp_rDot_DpDs = array([[qDot,0,0]]).T 25 | Dp_omega_DpDs = zeros([3,1]) 26 | return [Dp_rDot_DpDs, Dp_omega_DpDs] 27 | 28 | def JointAcceleration(self, q, qDot, qDDot): # -> [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] 29 | # Overwrite generic JointAcceleration: 30 | Dp_rDDot_DpDs = array([[qDDot,0,0]]).T 31 | Dp_omegaDot_DpDs = zeros([3,1]) 32 | return [Dp_rDDot_DpDs, Dp_omegaDot_DpDs] 33 | 34 | def JointJacobian(self, q, qIndex, nq): # -> [S, R] 35 | # Overwrite generic JointJacobian: 36 | S = zeros([3,nq]) 37 | R = zeros([3,nq]) 38 | S[:,qIndex] = [1,0,0] 39 | R[:,qIndex] = [0,0,0] 40 | return [S, R] -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/classes/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Computational-Dynamics-for-Robotics/9323903eb5a09826e79bcf3280de90f054490144/Multibodydynamics_engine/python/classes/__init__.py -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/classes/robotics_helpfuns.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def skew(vec): 4 | ''' 5 | Generates a skew-symmetric matrix given a vector w 6 | ''' 7 | S = np.zeros([3,3]) 8 | 9 | S[0,1] = -vec[2] 10 | S[0,2] = vec[1] 11 | S[1,2] = -vec[0] 12 | 13 | S[1,0] = vec[2] 14 | S[2,0] = -vec[1] 15 | S[2,1] = vec[0] 16 | 17 | return S 18 | 19 | def rotZ(angle): 20 | return np.array([[np.cos(angle),-np.sin(angle),0], 21 | [np.sin(angle), np.cos(angle),0], 22 | [0, 0, 1]]) -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/classes/vpython_ext.py: -------------------------------------------------------------------------------- 1 | from vpython import vector, ellipsoid 2 | import numpy as np 3 | from scipy.spatial.transform import Rotation as R 4 | 5 | class vellipsoid: 6 | 7 | def __init__(self, pos, color, size): 8 | self.ell = ellipsoid(pos=pos, color=color, size=size) 9 | self.A_IB = np.eye(3) 10 | 11 | @property 12 | def pos(self): 13 | return self.ell.pos 14 | 15 | @pos.setter 16 | def pos(self, newpos:np.ndarray): 17 | self.ell.pos = vector( *(newpos) ) 18 | 19 | @property 20 | def orientation(self): 21 | return self.A_IB 22 | 23 | @orientation.setter 24 | def orientation(self, A_IB:np.ndarray): 25 | ''' 26 | rotate vpython object using rotation matrix instead of vector-angle 27 | ''' 28 | rot = R.from_matrix( A_IB @ self.A_IB.T ).as_rotvec() 29 | self.ell.rotate( angle=np.linalg.norm(rot), axis=vector(*rot) ) 30 | self.A_IB = A_IB -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/testfiles/TestFile_91_TriplePendulum.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('.') 3 | from numpy import eye, array, ones, zeros, pi, arange, concatenate, append, diag, linalg, linspace 4 | # from numpy.linalg import inv, norm 5 | from scipy.integrate import ode, odeint, solve_ivp 6 | from scipy.spatial.transform import Rotation as R 7 | from classes.RigidBody import RigidBody, Ground, Rod 8 | from classes.MultiRigidBody import MultiRigidBody 9 | from classes.RotationalJoint import RotationalJoint 10 | from classes.SpringDamper import SpringDamper 11 | 12 | 13 | #%% Setup MBD system 14 | 15 | I_grav = array([[0,-9.81,0]]).T 16 | ground = Ground() 17 | link1 = Rod(length=1, radius_o=0.02, radius_i=0, I_grav=I_grav) 18 | link2 = Rod(length=1, radius_o=0.02, radius_i=0, I_grav=I_grav) 19 | link3 = Rod(length=1, radius_o=0.02, radius_i=0, I_grav=I_grav) 20 | joint1 = RotationalJoint(ground,link1, A_PDp=eye(3), A_SDs=eye(3), P_r_PDp=array([[0,0,0]]).T, S_r_SDs= array([[-0.5,0,0]]).T) 21 | joint2 = RotationalJoint(link1, link2, A_PDp=eye(3), A_SDs=eye(3), P_r_PDp=array([[0.5,0,0]]).T, S_r_SDs= array([[-0.5,0,0]]).T) 22 | joint3 = RotationalJoint(link1, link3, A_PDp=eye(3), A_SDs=eye(3), P_r_PDp=array([[0.5,0,0]]).T, S_r_SDs= array([[-0.5,0,0]]).T) 23 | 24 | springDamper1 = SpringDamper(link2, link3, P_r_PDp=array([[0.5,0,0]]).T, S_r_SDs=array([[0.5,0,0]]).T, K=10, D=1, d0=1) 25 | springDamper2 = SpringDamper(ground, link1, P_r_PDp=array([[1,0,0]]).T, S_r_SDs=array([[0.5,0,0]]).T, K=100, D=15, d0=0) 26 | 27 | pendulum = MultiRigidBody(ground=ground, springDampers=[springDamper1, springDamper2]) 28 | 29 | # set generalized coordinate indices 30 | joint1.qIndex = 0 31 | joint2.qIndex = 1 32 | joint3.qIndex = 2 33 | nq=3 34 | pendulum.setup(nq=nq) 35 | 36 | # set initial conditions 37 | pendulum.recursive_setall_q( q = array([30,10,-20]) * pi/180 ) 38 | 39 | 40 | #%% Simulate 41 | 42 | def odefun(t,y): 43 | q, qDot = y[0:nq], y[nq:] 44 | qDDot = pendulum.getODE ( q=q, qDot=qDot ) 45 | return concatenate((qDot,qDDot)) 46 | 47 | # initial conditions 48 | q0, dq0, ddq0 = pendulum.recursive_getall_q() 49 | 50 | # simulate 51 | tf = 20 52 | fps = 60 53 | odesol = solve_ivp( odefun, t_span=[0,tf], t_eval=arange(0,tf,1/fps), y0=concatenate((q0,dq0)).squeeze(), method='RK45', dense_output=True, events=None ) 54 | 55 | from matplotlib import pyplot as plt 56 | plt.figure() 57 | plt.plot(odesol.t, odesol.y[0,:]*180/pi, label='joint1.q') 58 | plt.plot(odesol.t, odesol.y[1,:]*180/pi, label='joint2.q') 59 | plt.plot(odesol.t, odesol.y[2,:]*180/pi, label='joint3.q') 60 | plt.legend() 61 | plt.grid(True) 62 | plt.show() 63 | 64 | 65 | #%% Animate 66 | 67 | pendulum.initGraphics(width=1200, height=800, range=1.5, title='A double pendulum', updaterate=fps) 68 | 69 | while True: 70 | for t,y in zip(odesol.t,odesol.y.T): 71 | pendulum.recursive_setall_q( q=y[0:nq], qDot=y[nq:] ) 72 | pendulum.updateKinTree() 73 | pendulum.updateGraphics() 74 | 75 | print('Animation finished!') 76 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/testfiles/TestFile_92_BilateralConstraints.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('.') 3 | from numpy import eye, array, ones, zeros, pi, arange, concatenate, append, diag, linspace, block, sum 4 | from numpy.linalg import inv, norm, solve, pinv 5 | from scipy.integrate import ode, odeint, solve_ivp 6 | from scipy.spatial.transform import Rotation as R 7 | from classes.RigidBody import RigidBody, Ground, Rod 8 | from classes.MultiRigidBody import MultiRigidBody 9 | from classes.RotationalJoint import RotationalJoint 10 | from classes.SpringDamper import SpringDamper 11 | from classes.PositionBilateralConstraint import PositionBilateralConstraint 12 | 13 | 14 | #%% Setup MBD system 15 | 16 | I_grav = array([[0,-9.81,0]]).T 17 | ground = Ground() 18 | 19 | link1 = Rod(length=1, radius_o=0.02, radius_i=0.01, I_grav=I_grav) 20 | link2 = Rod(length=1, radius_o=0.02, radius_i=0.01, I_grav=I_grav) 21 | link3 = Rod(length=1, radius_o=0.02, radius_i=0.01, I_grav=I_grav) 22 | link4 = Rod(length=1, radius_o=0.02, radius_i=0.01, I_grav=I_grav) 23 | 24 | joint1 = RotationalJoint(ground,link1, A_PDp=eye(3), A_SDs=eye(3), P_r_PDp=array([[0,0,0]]).T, S_r_SDs= array([[-0.5,0,0]]).T) 25 | joint2 = RotationalJoint(link1, link2, A_PDp=eye(3), A_SDs=eye(3), P_r_PDp=array([[0.5,0,0]]).T, S_r_SDs= array([[-0.5,0,0]]).T) 26 | joint3 = RotationalJoint(link2, link3, A_PDp=eye(3), A_SDs=eye(3), P_r_PDp=array([[0.5,0,0]]).T, S_r_SDs= array([[-0.5,0,0]]).T) 27 | joint4 = RotationalJoint(link1, link4, A_PDp=eye(3), A_SDs=eye(3), P_r_PDp=array([[0.5,0,0]]).T, S_r_SDs= array([[-0.5,0,0]]).T) 28 | 29 | springDamper1 = SpringDamper(ground, link4, P_r_PDp=array([[1,1,0]]).T, S_r_SDs=array([[0.5,0,0]]).T, K=50, D=5, d0=0) 30 | 31 | constraint = PositionBilateralConstraint(link3,ground, P_r_PDp=array([[0.5,0,0]]).T, S_r_SDs=array([[2,0,0]]).T) 32 | 33 | # create multi-rigid-body object 34 | pendulum = MultiRigidBody(ground=ground, springDampers=[springDamper1], bilateralConstraints=[constraint]) 35 | 36 | # set generalized coordinate indices 37 | joint1.qIndex = 0 38 | joint2.qIndex = 1 39 | joint3.qIndex = 2 40 | joint4.qIndex = 3 41 | nq = 4 42 | pendulum.setup(nq=nq) 43 | 44 | # set initial conditions 45 | pendulum.recursive_setall_q( q = array([60,-60,-60,-60-90]) * pi/180 ) 46 | pendulum.updateKinTree() 47 | 48 | 49 | #%% Simulate 50 | 51 | def odefun(t,y): 52 | q, qDot = y[0:nq], y[nq:] 53 | qDDot = pendulum.getODE ( q=q, qDot=qDot ) 54 | return concatenate((qDot,qDDot)) 55 | 56 | 57 | # initial conditions 58 | q0, dq0, ddq0 = pendulum.recursive_getall_q() 59 | 60 | # simulate 61 | tf = 20 62 | fps = 60 63 | odesol = solve_ivp( odefun, t_span=[0,tf], t_eval=arange(0,tf,1/fps), y0=concatenate((q0,dq0)).squeeze(), method='RK45', dense_output=True, events=None ) 64 | print(odesol.message) 65 | 66 | from matplotlib import pyplot as plt 67 | plt.figure() 68 | plt.plot(odesol.t, odesol.y[0,:]*180/pi, label='joint1.q') 69 | plt.plot(odesol.t, odesol.y[1,:]*180/pi, label='joint2.q') 70 | plt.plot(odesol.t, odesol.y[2,:]*180/pi, label='joint3.q') 71 | plt.legend() 72 | plt.grid(True) 73 | plt.show() 74 | 75 | 76 | #%% Animate 77 | 78 | pendulum.initGraphics(width=1200, height=800, range=1.5, title='A double pendulum', updaterate=fps) 79 | 80 | while True: 81 | for t,y in zip(odesol.t,odesol.y.T): 82 | pendulum.recursive_setall_q( q=y[0:nq], qDot=y[nq:] ) 83 | pendulum.updateKinTree() 84 | pendulum.updateGraphics() 85 | 86 | print('Animation finished!') 87 | -------------------------------------------------------------------------------- /Multibodydynamics_engine/python/testfiles/TestFile_93_Suspension.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('.') 3 | from numpy import eye, array, ones, zeros, pi, arange, concatenate, append, diag, linspace, block, sum 4 | from numpy.linalg import inv, norm, solve, pinv 5 | from scipy.integrate import ode, odeint, solve_ivp 6 | from scipy.spatial.transform import Rotation as R 7 | from classes.RigidBody import RigidBody, Ground, Rod, Ellipsoid 8 | from classes.MultiRigidBody import MultiRigidBody 9 | from classes.RotationalJoint import RotationalJoint 10 | from classes.SpringDamper import SpringDamper 11 | from classes.PositionBilateralConstraint import PositionBilateralConstraint 12 | from classes.robotics_helpfuns import rotZ 13 | 14 | 15 | #%% Setup MBD system 16 | 17 | I_grav = array([[0,-9.81,0]]).T 18 | chassis = Ground() 19 | lowerA = Rod(length=0.3, radius_o=0.01, radius_i=0, I_grav=I_grav) 20 | upperA = Rod(length=0.3, radius_o=0.01, radius_i=0, I_grav=I_grav) 21 | pushbar = Rod(length=0.45, radius_o=0.01, radius_i=0, I_grav=I_grav) 22 | upright = Ellipsoid(rx=0.2, ry=0.01, rz=0.2, density=2000, I_grav=I_grav) 23 | rocker = Ellipsoid(rx=0.05, ry=0.05, rz=0.02, density=8000, I_grav=I_grav) 24 | 25 | j_chassis_lowerA = RotationalJoint(chassis,lowerA, A_PDp=eye(3), A_SDs=eye(3), P_r_PDp=array([[0,0,0]]).T, S_r_SDs= array([[-lowerA.length/2,0,0]]).T) 26 | j_lowerA_upright = RotationalJoint(lowerA,upright, A_PDp=rotZ(pi/2), A_SDs=eye(3), P_r_PDp=array([[lowerA.length/2,0,0]]).T, S_r_SDs= array([[-0.1,0,0]]).T) 27 | j_upright_upperA = RotationalJoint(upright, upperA, A_PDp=rotZ(pi/2), A_SDs=eye(3), P_r_PDp=array([[0.1,0,0]]).T, S_r_SDs= array([[-upperA.length/2,0,0]]).T) 28 | c_upperA_chassis = PositionBilateralConstraint(upperA, chassis, P_r_PDp=array([[upperA.length/2,0,0]]).T, S_r_SDs=array([[0,0.2,0]]).T) 29 | 30 | j_chassis_rocker = RotationalJoint(chassis, rocker, A_PDp=eye(3), A_SDs=eye(3), P_r_PDp=array([[-0.1,0.3,0]]).T, S_r_SDs= array([[0,-0.05,0]]).T) 31 | j_rocker_pushbar = RotationalJoint(rocker, pushbar, A_PDp=eye(3), A_SDs=eye(3), P_r_PDp=array([[0.05,0,0]]).T, S_r_SDs= array([[-pushbar.length/2,0,0]]).T) 32 | sd_chassis_rocker = SpringDamper(chassis, rocker, P_r_PDp=array([[-0.25,0.35,0]]).T, S_r_SDs=array([[-0.05,0,0]]).T, K=500, D=200, d0=0, radius=0.02, coils=30) 33 | 34 | c_pushbar_lowerA = PositionBilateralConstraint(pushbar,lowerA, P_r_PDp=array([[pushbar.length/2,0,0]]).T, S_r_SDs=array([[1/2.5*lowerA.length,0,0]]).T) 35 | 36 | 37 | 38 | # create multi-rigid-body object 39 | # suspension = MultiRigidBody(ground=chassis, springDampers=[sd_chassis_rocker], bilateralConstraints=[c_pushbar_lowerA]) 40 | suspension = MultiRigidBody(ground=chassis, springDampers=[sd_chassis_rocker], bilateralConstraints=[c_upperA_chassis,c_pushbar_lowerA]) 41 | 42 | 43 | # set generalized coordinate indices 44 | j_chassis_lowerA.qIndex = 0 45 | j_lowerA_upright.qIndex = 1 46 | j_upright_upperA.qIndex = 2 47 | j_chassis_rocker.qIndex = 3 48 | j_rocker_pushbar.qIndex = 4 49 | nq = 5 50 | suspension.setup(nq=nq) 51 | 52 | # set initial conditions 53 | suspension.recursive_setall_q( q = array([0, 0, 0, 0, -50]) * pi/180 ) 54 | suspension.updateKinTree() 55 | 56 | 57 | #%% Simulate 58 | 59 | def odefun(t,y): 60 | q, qDot = y[0:nq], y[nq:] 61 | qDDot = suspension.getODE ( q=q, qDot=qDot ) 62 | return concatenate((qDot,qDDot)) 63 | 64 | 65 | # initial conditions 66 | q0, dq0, ddq0 = suspension.recursive_getall_q() 67 | 68 | # simulate 69 | tf = 1 70 | fps = 60 71 | odesol = solve_ivp( odefun, t_span=[0,tf], t_eval=arange(0,tf,1/fps), y0=concatenate((q0,dq0)).squeeze(), method='RK45', dense_output=True, events=None ) 72 | print(odesol.message) 73 | 74 | from matplotlib import pyplot as plt 75 | plt.figure() 76 | plt.plot(odesol.t, odesol.y[0,:]*180/pi, label='joint1.q') 77 | plt.plot(odesol.t, odesol.y[1,:]*180/pi, label='joint2.q') 78 | plt.plot(odesol.t, odesol.y[2,:]*180/pi, label='joint3.q') 79 | plt.legend() 80 | plt.grid(True) 81 | plt.show() 82 | 83 | 84 | #%% Animate 85 | 86 | suspension.initGraphics(width=1200, height=800, range=1.5, title='Suspension', updaterate=fps/6) 87 | 88 | while True: 89 | for t,y in zip(odesol.t,odesol.y.T): 90 | suspension.recursive_setall_q( q=y[0:nq], qDot=y[nq:] ) 91 | suspension.updateKinTree() 92 | suspension.updateGraphics() 93 | 94 | print('Animation finished!') 95 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi-Body-Dynamics-Engine 2 | Repository for the course "Computational Dynamics for Robotics" at University of Stuttgart 3 | 4 | Two versions of the multi-body dynamics simulator are available: 5 | 6 | 1. [Matlab Version](./Multibodydynamics_engine/matlab) 7 | 2. [Python version](./Multibodydynamics_engine/python) (Graphics generated with OpenGL engine VPython) 8 | 9 | Use the TestFiles to see how to setup a system 10 | --------------------------------------------------------------------------------