\section{Introduction to SimMechanics\texttrademark }

SimMechanics provides a multibody simulation environment for 3D mechanical systems, such as robots, vehicle suspensions, construction equipment, and aircraft landing gear. The multibody system is modelled using blocks representing bodies, joints, constraints, and force elements, and then SimMechanics formulates and solves the equations of motion for the complete mechanical system. Models from CAD systems, including mass, inertia, joint, constraint, and 3D geometry, can be imported into SimMechanics. An automatically generated 3D animation allows the visualization of the system dynamics.

The models can be parametrized using Matlab variables and expressions, and design control systems for a multibody system in Simulink\textregistered. Electrical, hydraulic, pneumatic, and other components can be added to a mechanical model using Simscape\texttrademark{} and test them all in a single simulation environment.

A new multibody model can be started by typing \texttt{smnew} in the Matlab command line. A new model window opens with commonly used blocks. The SimMechanics block library also opens. Figure \ref{figure:new_model} shows the new model window. To begin modelling a new multibody system, blocks from the library are dragged into the model window. These blocks belong to the SimMechanics Second Generation library which has more tools and functionalities compared to First Generation.

	\begin{figure}[!hb]
	\centering
	\includegraphics[scale=0.35]{Imagens/simmechanics/new_model_window.png}
	\caption{New model window and SimMechanics Second Generation library.}
	\label{figure:new_model}
	\end{figure}

Mechanical links are common building blocks in linkages, mechanisms, and machines. A link needs end frames to connect to joints. Rigid Transform Blocks provide the end frames, while a Solid block provides geometry, inertia and colour. Joint blocks, as in real life, connect two solids. 
The Solver Configuration block specifies the solver parameters that the model needs before the simulation can begin. 
The World Frame block represents the global reference frame in a model. This frame is inertial and at absolute rest. Rigidly connecting a frame to the World frame makes that frame inertial. Frame axes are orthogonal and arranged according to the right-hand rule. In a frame network, the World frame is the ultimate reference frame. Directly or indirectly, all other frames are defined with respect to the World frame.
The Mechanism Configuration block provides mechanical and simulation parameters to a mechanism, i.e., a self-contained group of interconnected SimMechanics blocks. Parameters include gravity and a linearisation delta for computing numerical partial derivatives during linearisation. These parameters apply only to the target mechanism, i.e., the mechanism that the block connects to. These are the main blocks to construct a multibody model and they can be seen in figure \ref{figure:main_blocks}.

	\begin{figure}[!hb]
	\centering
	\includegraphics[scale=0.35]{Imagens/simmechanics/main_blocks.png}
	\caption{Main blocks necessary to build a multibody system.}
	\label{figure:main_blocks}
	\end{figure}

When the model is finished the joint blocks can be actuated with a Signal block to perceive the systems behaviour or with a PID block to find an optimal controller.
Also to be noted that the Matlab version should be higher than R2014 to access the new SimMechanics blocks that have more sensing options.
To find out more detailed information and help for SimMechanics please refer to the the official MathWorks\textregistered{} website.

Building a multibody model using the SimMechanics and Simscape libraries can be time consuming and difficult since objects with complex geometries are impossible to accurately model using only the Solid block, so fortunately there is a faster way to build a model which is using the tool SimMechanics Link.

\section{Introduction to SimMechanics Link}

SimMechanics Link is a Matlab function that can be downloaded in the official MathWorks website. This function creates a plug-in for a Computer Aided Design (CAD) software such as Solidworks\texttrademark{}, Creo-Pro/E\texttrademark{} and Inventor\texttrademark. The selected CAD software was Solidworks because it was the only one dealt with before.

When the function is installed and the plug-in is activated on the CAD software a 3D assembly of the system can be built with all needed constraints to make the model behave just as desired. These constraints are important because when the assembly is exported to SimMechanics they will be transformed into joints. There are two options to export the assembly which are SimMechanics First and Second Generation. Second Generation is preferred because it provides more features.

%% http://www.mathworks.com/help/physmod/smlink/ug/installing-and-linking-simmechanics-link-software.html

\section{Pendulum Model}
\subsection{Construction of the Pendulum Assembly on Solidworks and SimMechanics Import}

Modelling the pendulum brought up a problem which is to represent the winding of rope, this physical process can be visually produced but that is not enough because rope can be tensioned but not compressed and the final assembly wouldn't be able to reproduce that property. So, knowing that constraints on an assembly produce joints when exported to a SimMechanics model a simpler solution was approached. 

The part seen in figure \ref{figure:articulacao} will duplicate the winding. Its hole will allow rotation, mimicking the rope's rotation around the pulley's radial plane and two edges are coincident with the opposing part (highlighted in figure \ref{figure:articulacao}) will mimic the length variation of unwinded rope. On other words one servo with a pulley and rope is simplified with two solids and three joints, more specifically two rotational joints and one prismatic. The hole constraint will result in a revolute joint and the two coincident edges will result in a prismatic joint.

	\begin{figure}[!hb]
	\centering
	\includegraphics[scale=0.5]{Imagens/simmechanics/articulacao_2.png}
	\caption{Part designed to simplify the winding process.}
	\label{figure:articulacao}
	\end{figure}


The final assembly is composed of six parts seen in figure \ref{figure:assembly}. To simplify even more the virtual pendulum it is symmetric, contrary to reality, were the pulleys aren't on the same plane producing a slight rotation on the pole. This was done to increase Matalab's computation time.

	\begin{figure}[!hb]
	\centering
	\includegraphics[scale=0.4]{Imagens/simmechanics/assembly.png}
	\caption{Pendulum assembly.}
	\label{figure:assembly}
	\end{figure}

After the 3D model was completed it was exported to Matlab. On the Matlab side the assembly was imported to a SimMechanics multibody, the blocks can be seen in figure \ref{figure:imported_model}. To note that this process produced cylindrical joints instead of prismatic, this is not a problem because it is a joint with one prismatic and one revolute primitives possessing parallel motion axes, meaning that the importing process considered that the constraint between the two parts (figure \ref{figure:articulacao}) has some torsion.

	\begin{figure}[!hb]
	\centering
	\includegraphics[scale=0.4]{Imagens/simmechanics/imported_model.png}
	\caption{Pendulum assembly after it was imported to SimMechanics.}
	\label{figure:imported_model}
	\end{figure}

\subsection{SimMechanics Model Changes}

Joint spring stiffness and friction parameters can't be specified on Solidworks, so it was added friction and a spring coefficient to the pole's rotational joint. These values were adjusted until the simulation would behave similarly to the real system. Note that this model is self supported by the spring in the pole's joint, this mimics the real pendulum when it is supported with the elastics.

It was not an objective to construct the virtual model as identical as possible to the real system because the main goal of this approach is to assess if a controller can be designed and see the effects of stimuli on the system.

Figure \ref{figure:hard_stop} shows the implementation of a hard stop on the pole's bottom joint to prevent it from going through the base.

	\begin{figure}[!hb]
	\centering
	\includegraphics[scale=0.3]{Imagens/simmechanics/hard_stop.png}
	\caption{Rotational Hard Stop to limit the pendulum's revolute joint movement range.}
	\label{figure:hard_stop}
	\end{figure}

The solver chosen to run the system simulation was ode15s, recommended by MathWorks.

After all these changes the model is ready to start being measured and controlled. On a first approach the reaction forces from the prismatic joint were analysed while a force was being applied to the pole. This did not produce the expected output which was a force along the Z axis of the linear joint with values close to the force applied. Many settings were tweaked but none produced values close to a real situation. 

From the experience of other users a solution to measure tensile force between two parts connected by a linear joint could be adding a spring and damper force in series. To use this block to aid this situation it is advised to set its spring stiffness to a high level, so it becomes almost rigid, and then measure the output force. Unfortunately this solution did not solve the problem, the values of the outputted force continued to be distinct from the expected. The experience with the real pendulum supports the failure to read the tensile force on the virtual model. 

None of these force readings were similar to what was expected because their value was always between 1 and -1. When a force is applied on the pendulum the rope is pulled and its tensile force will have a magnitude close to the applied force, this is what happens with the real pendulum.

Even more, after some online research and conversations with other SimMechanis users it was realised that this tool doesn't allow the extration of the model's mathematical equations, it only solves them to produce the simulation.

%Most likely this issue is due to a lack of experience with the software. So to make the most out of this tool, for now, it was decided to study some stimuli types and then apply them to the real pendulum.
% 
% \subsection{Stimuli on the SimMechanics Model}
% 
% 	\begin{figure}[!hb]
% 	\centering
% 	\includegraphics[scale=0.35]{Imagens/simmechanics/final_multibody.png}
% 	\caption{Testing different stimuli.}
% 	\label{figure:final_multibody}
% 	\end{figure}
% 
% The objective of this part of the work is to get some experience with different types of stimuli and their effects. The stimuli tested here are aimed for the robot which its safety isn't as concerning as for a person. After watching some videos of DARPA's robot it was noticed that they disrupted their robot's balance using random and short termed stimuli, always bellow one second. But for now the type of stimuli tested will have the standard Simulink duration, which are the default values on the signal blocks. It was done like this to collect more information and to prevent any unexpected result that could be generated due to the quickness of the stimulus because these stimuli will be replicated on the real pendulum. With the final objective of assessing the reliability of the application of stimuli and data collection on the real system.
% 
% The signals are steps, ramps, triangles and sawtooth. The simulation setup to perform these tests can be seen in figure \ref{figure:final_multibody}. Different signals were applied to the multibody, more specifically a pull force on one of the prismatic primitive of the cylindrical joint, this can be seen in the bottom of figure \ref{figure:final_multibody}. To analyse the results the actuation force (or the applied signal) and the pendulum's joint angle were sent to Matlab's workspace and later were written in a Comma-separated values file (csv). The angle was measured because it was not possible to measure the tensile force, just like in the real pendulum and also because the angle gives information about the equilibrium state and this measure can be done in the real system as it can be seen further. The stimuli and there effects can be seen in annex \ref{annex:simu}.
% 
% Not much can be concluded from the simulation results. From the ramps it is clear that this is a nonlinear system.
% 
% This procedure will be replicated on the real pendulum, using these graphics as reference for the stimuli but not their effects which are expected to be different because the servos can't replicate that actuation and the real springs on the pendulum aren't radial.