diff --git a/kinematics/index.html b/kinematics/index.html index 0aab43e..752dc31 100644 --- a/kinematics/index.html +++ b/kinematics/index.html @@ -3,7 +3,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - + Kinematics of the station @@ -246,6 +246,31 @@ for the JavaScript code in this tag. } /*]]>*///--> + +
@@ -254,17 +279,299 @@ for the JavaScript code in this tag. HOME

Kinematics of the station

+
+

Table of Contents

+
+ +
+
-

-All the files (data and Matlab scripts) are accessible here. +In this document, we discuss the way the motion of each stage is defined.

+
+

1 Micro Hexapod

+
+
+
+

1.1 How the Symetrie Hexapod is controlled on the micro station

+
+

+For the Micro-Hexapod, the convention for the angles are defined in MAN_A_Software API_4.0.150918_EN.pdf on page 13 (section 2.4 - Rotation Vectors): +

+ +
+

+The Euler type II convention is used to express the rotation vector. +This convention is mainly used in the aeronautics field (standard ISO 1151 concerning flight mechanics). +

+ +

+This convention uses the concepts of rotation of vehicles (ship, car and plane). +Generally, we consider that the main movement of the vehicle is following the X-axis and the Z-axis is parallel to the axis of gravity (at the initial position). +The roll rotation is around the X-axis, the pitch is around the Y-axis and yaw is the rotation around the Z-axis. +The order of rotation is: Rx, Ry and then Rz. +

+ +

+In most case, rotations are related to a reference with fixed axis; thus we say the rotations are around fixed axes. +The combination of these three rotations enables to write a rotation matrix. +This writing is unique and equal to: +\[ \bm{R} = \bm{R}_z(\gamma) \cdot \bm{R}_y(\beta) \cdot \bm{R}_x(\alpha) \] +

+ +

+The Euler type II convention corresponding to the succession of rotations with respect to fixed axes: first around X0, then Y0 and Z0. +This is equivalent to the succession of rotations with respect to mobile axes: first around Z0, then Y1' and X2'. +

+
+ +

+More generally on the Control of the Micro-Hexapod: +

+
+

+Note that for all control modes, the rotation center coincides with Object coordinate system origin. +Moreover, the movements are controlled with translation components at first (Tx, Ty, Tz) then rotation components (Rx, Ry, Rz). +

+
+ +

+Thus, it does the translations and then the rotation around the new translated frame. +

+
+
+ +
+

1.2 Control of the Micro-Hexapod using Simscape

+
+

+We can think of two main ways to position the Micro-Hexapod using Simscape. +

+ +

+The first one is to use only one Bushing Joint between the base and the mobile platform. +The advantage is that it is very easy to impose the wanted displacement, however, we loose the dynamical properties of the Hexapod. +

+ +

+The second way is to specify the wanted length of the legs of the Hexapod in order to have the wanted position of the mobile platform. +This require a little bit more of mathematical derivations but this is the chosen solution. +

+
+ +
+

1.2.1 Using Bushing Joint

+
+

+In the documentation of the Bushing Joint (doc "Bushing Joint") that is used to position the Hexapods, it is mention that the following frame is positioned with respect to the base frame in a way shown in figure 1. +

+ + +
+

bushing_joint_transform.png +

+

Figure 1: Joint Transformation Sequence for the Bushing Joint

+
+ +

+Basically, it performs the translations, and then the rotation along the X, Y and Z axis of the moving frame. +The three rotations that we define thus corresponds to the Euler U-V-W angles. +

+ +

+We should have the same behavior for the Micro-Hexapod on Simscape (same inputs at least). +However, the Bushing Joint makes rotations around mobiles axes (X, Y' and then Z'') and not fixed axes (X, Y and Z). +

+
+
+ +
+

1.2.2 Using Inverse Kinematics and Leg Actuators

+
+

+Here, we can use the Inverse Kinematic of the Hexapod to determine the length of each leg in order to obtain some defined translation and rotation of the mobile platform. +

+ +

+The advantages are: +

+
    +
  • we can position the Hexapod as we want by specifying a rotation matrix
    • +
  • the hexapod keeps its full flexibility as we don't specify any wanted displacements, only leg's rest position
    • +
+
+ +
+
1.2.2.1 Theory
+
+

+For inverse kinematic analysis, it is assumed that the position \({}^A\bm{P}\) and orientation of the moving platform \({}^A\bm{R}_B\) are given and the problem is to obtain the joint variables, namely, \(\bm{L} = [l_1, l_2, \dots, l_6]^T\). +

+ +

+From the geometry of the manipulator, the loop closure for each limb, \(i = 1, 2, \dots, 6\) can be written as +

+\begin{align*} + l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\ + &= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i +\end{align*} + +

+To obtain the length of each actuator and eliminate \(\hat{\bm{s}}_i\), it is sufficient to dot multiply each side by itself: +

+\begin{equation} + l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right] +\end{equation} + +

+Hence, for \(i = 1, 2, \dots, 6\), each limb length can be uniquely determined by: +

+\begin{equation} + l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i} +\end{equation} + +

+If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation. +Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable. +

+
+
+ +
+
1.2.2.2 Matlab Implementation
+
+
+
open 'simscape/hexapod_tests.slx'
+
+
+ +
+
load('simscape/conf_simscape.mat');
+set_param(conf_simscape, 'StopTime', '0.5');
+
+
+ +
+
tx = 0.1; % [rad]
+ty = 0.2; % [rad]
+tz = 0.05; % [rad]
+
+Rx = [1 0        0;
+      0 cos(tx) -sin(tx);
+      0 sin(tx)  cos(tx)];
+
+Ry = [ cos(ty) 0 sin(ty);
+      0        1 0;
+      -sin(ty) 0 cos(ty)];
+
+Rz = [cos(tz) -sin(tz) 0;
+      sin(tz)  cos(tz) 0;
+      0        0       1];
+
+ARB = Rz*Ry*Rx;
+AP = [0.01; 0.02; 0.03]; % [m]
+
+hexapod = initializeMicroHexapod(struct('AP', AP, 'ARB', ARB));
+
+
+ +
+
sim('simscape/hexapod_tests.slx')
+
+
+ +

+And we verify that we indeed succeed to go to the wanted position. +

+
+
[simout.x.Data(end) ; simout.y.Data(end) ; simout.z.Data(end)] - AP
+
+
+ + + + +++ + + + + + + + + + + + + + +
-2.12e-06
2.9787e-06
-4.4341e-06
+ +
+
simout.R.Data(:, :, end)-ARB
+
+
+ + + + +++ ++ ++ + + + + + + + + + + + + + + + + + + + +
-1.5714e-061.4513e-067.8133e-06
8.4113e-07-7.1485e-07-7.4572e-06
-7.5348e-067.7112e-06-2.3088e-06
+
+
+
+

Author: Dehaeze Thomas

-

Created: 2019-10-08 mar. 11:13

+

Created: 2019-12-10 mar. 18:03

Validate

diff --git a/kinematics/index.org b/kinematics/index.org index 88602ef..0b2f8a8 100644 --- a/kinematics/index.org +++ b/kinematics/index.org @@ -42,23 +42,91 @@ :END: * Introduction :ignore: +In this document, we discuss the way the motion of each stage is defined. -* ZIP file containing the data and matlab files :ignore: -#+begin_src bash :exports none :results none - if [ matlab/kinematics.m -nt data/kinematics.zip ]; then - cp matlab/kinematics.m kinematics.m; - zip data/kinematics \ - mat/data.mat \ - kinematics.m - rm kinematics.m; - fi -#+end_src +* Micro Hexapod +** How the Symetrie Hexapod is controlled on the micro station +For the Micro-Hexapod, the convention for the angles are defined in =MAN_A_Software API_4.0.150918_EN.pdf= on page 13 (section 2.4 - Rotation Vectors): -#+begin_note - All the files (data and Matlab scripts) are accessible [[file:data/kinematics.zip][here]]. -#+end_note +#+begin_quote +The *Euler type II convention* is used to express the rotation vector. +This convention is mainly used in the aeronautics field (standard ISO 1151 concerning flight mechanics). -* Matlab Init :noexport:ignore: +This convention uses the concepts of rotation of vehicles (ship, car and plane). +Generally, we consider that the main movement of the vehicle is following the X-axis and the Z-axis is parallel to the axis of gravity (at the initial position). +The roll rotation is around the X-axis, the pitch is around the Y-axis and yaw is the rotation around the Z-axis. +*The order of rotation is: Rx, Ry and then Rz.* + +In most case, rotations are related to a reference with fixed axis; thus we say the rotations are around fixed axes. +The combination of these three rotations enables to write a rotation matrix. +This writing is unique and equal to: +\[ \bm{R} = \bm{R}_z(\gamma) \cdot \bm{R}_y(\beta) \cdot \bm{R}_x(\alpha) \] + +The Euler type II convention corresponding to the *succession of rotations with respect to fixed axes*: first around X0, then Y0 and Z0. +This is equivalent to the succession of rotations with respect to mobile axes: first around Z0, then Y1' and X2'. +#+end_quote + +More generally on the Control of the Micro-Hexapod: +#+begin_quote +Note that for all control modes, *the rotation center coincides with Object coordinate system origin*. +Moreover, the movements are controlled with *translation components at first* (Tx, Ty, Tz) *then rotation components* (Rx, Ry, Rz). +#+end_quote + +Thus, it does the translations and then the rotation around the new translated frame. + +** Control of the Micro-Hexapod using Simscape +*** Introduction :ignore: +We can think of two main ways to position the Micro-Hexapod using Simscape. + +The first one is to use only one Bushing Joint between the base and the mobile platform. +The advantage is that it is very easy to impose the wanted displacement, however, we loose the dynamical properties of the Hexapod. + +The second way is to specify the wanted length of the legs of the Hexapod in order to have the wanted position of the mobile platform. +This require a little bit more of mathematical derivations but this is the chosen solution. + +*** Using Bushing Joint +In the documentation of the Bushing Joint (=doc "Bushing Joint"=) that is used to position the Hexapods, it is mention that the following frame is positioned with respect to the base frame in a way shown in figure [[fig:bushing_joint_transform]]. + +#+name: fig:bushing_joint_transform +#+caption: Joint Transformation Sequence for the Bushing Joint +[[file:figs/bushing_joint_transform.png]] + +Basically, it performs the translations, and then the rotation along the X, Y and Z axis of the moving frame. +The three rotations that we define thus corresponds to the Euler U-V-W angles. + +We should have the *same behavior* for the Micro-Hexapod on Simscape (same inputs at least). +However, the Bushing Joint makes rotations around mobiles axes (X, Y' and then Z'') and not fixed axes (X, Y and Z). + +*** Using Inverse Kinematics and Leg Actuators +Here, we can use the Inverse Kinematic of the Hexapod to determine the length of each leg in order to obtain some defined translation and rotation of the mobile platform. + +The advantages are: +- we can position the Hexapod as we want by specifying a rotation matrix +- the hexapod keeps its full flexibility as we don't specify any wanted displacements, only leg's rest position + +**** Theory +For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$. + +From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as +\begin{align*} + l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\ + &= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i +\end{align*} + +To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself: +\begin{equation} + l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right] +\end{equation} + +Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by: +\begin{equation} + l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i} +\end{equation} + +If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation. +Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable. + +**** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src @@ -66,3 +134,63 @@ #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src + +#+begin_src matlab :tangle no + simulinkproject('../'); +#+end_src + +**** Matlab Implementation +#+begin_src matlab + open 'simscape/hexapod_tests.slx' +#+end_src + +#+begin_src matlab + load('simscape/conf_simscape.mat'); + set_param(conf_simscape, 'StopTime', '0.5'); +#+end_src + +#+begin_src matlab + tx = 0.1; % [rad] + ty = 0.2; % [rad] + tz = 0.05; % [rad] + + Rx = [1 0 0; + 0 cos(tx) -sin(tx); + 0 sin(tx) cos(tx)]; + + Ry = [ cos(ty) 0 sin(ty); + 0 1 0; + -sin(ty) 0 cos(ty)]; + + Rz = [cos(tz) -sin(tz) 0; + sin(tz) cos(tz) 0; + 0 0 1]; + + ARB = Rz*Ry*Rx; + AP = [0.01; 0.02; 0.03]; % [m] + + hexapod = initializeMicroHexapod(struct('AP', AP, 'ARB', ARB)); +#+end_src + +#+begin_src matlab + sim('simscape/hexapod_tests.slx') +#+end_src + +And we verify that we indeed succeed to go to the wanted position. +#+begin_src matlab :results table replace + [simout.x.Data(end) ; simout.y.Data(end) ; simout.z.Data(end)] - AP +#+end_src + +#+RESULTS: +| -2.12e-06 | +| 2.9787e-06 | +| -4.4341e-06 | + +#+begin_src matlab :results table replace + simout.R.Data(:, :, end)-ARB +#+end_src + +#+RESULTS: +| -1.5714e-06 | 1.4513e-06 | 7.8133e-06 | +| 8.4113e-07 | -7.1485e-07 | -7.4572e-06 | +| -7.5348e-06 | 7.7112e-06 | -2.3088e-06 | diff --git a/simscape/hexapod_tests.slx b/simscape/hexapod_tests.slx new file mode 100644 index 0000000..4e0d886 Binary files /dev/null and b/simscape/hexapod_tests.slx differ diff --git a/simscape/index.html b/simscape/index.html index 7fe3a76..295be48 100644 --- a/simscape/index.html +++ b/simscape/index.html @@ -3,7 +3,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - + Simscape Model @@ -258,64 +258,65 @@ for the JavaScript code in this tag.

Table of Contents

@@ -326,19 +327,19 @@ for the JavaScript code in this tag. This file is used to explain how this Simscape Model works.

    -
  • In section 1, the simulink project with the associated scripts are presented
    • -
  • In section 2, an introduction to Simscape Multibody is done
    • -
  • In section 3, each simscape files are presented with the associated signal names and joint architectures
    • -
  • In section 4, the list of the Simulink library elements are described
    • -
  • In section 5, a list of Matlab function that will be used are defined here
    • -
  • In section 6, all the functions that are used to initialize the Simscape Multibody elements are defined here. This includes the mass of all solids for instance.
    • +
  • In section 1, the simulink project with the associated scripts are presented
    • +
  • In section 2, an introduction to Simscape Multibody is done
    • +
  • In section 3, each simscape files are presented with the associated signal names and joint architectures
    • +
  • In section 4, the list of the Simulink library elements are described
    • +
  • In section 5, a list of Matlab function that will be used are defined here
    • +
  • In section 6, all the functions that are used to initialize the Simscape Multibody elements are defined here. This includes the mass of all solids for instance.
-
-

1 Simulink Project - Startup and Shutdown scripts

+
+

1 Simulink Project - Startup and Shutdown scripts

- +

@@ -400,11 +401,11 @@ The project also permits to automatically add defined folder to the path when th

-
-

2 Simscape Multibody - Presentation

+
+

2 Simscape Multibody - Presentation

- +

@@ -416,8 +417,8 @@ A simscape model per

-
-

2.1 Solid bodies

+
+

2.1 Solid bodies

Each solid body is represented by a solid block. @@ -426,8 +427,8 @@ The geometry of the solid body can be imported using a step file. T

-
-

2.2 Frames

+
+

2.2 Frames

Frames are very important in simscape multibody, they defined where the forces are applied, where the joints are located and where the measurements are made. @@ -439,8 +440,8 @@ They can be defined from the solid body geometry, or using the -

2.3 Joints

+
+

2.3 Joints

Solid Bodies are connected with joints (between frames of the two solid bodies). @@ -450,7 +451,7 @@ Solid Bodies are connected with joints (between frames of the two solid bodies). There are various types of joints that are all described here.

- +
@@ -583,7 +584,7 @@ Joint blocks are assortments of joint primitives:
  • Constant Velocity: Allows rotation at constant velocity between intersection through arbitrarily aligned shafts: CV
    • -
    Table 1: Degrees of freedom associated with each joint
    +
    @@ -875,8 +876,8 @@ Composite Force/Torque sensing: -
    -

    2.4 Measurements

    +
    +

    2.4 Measurements

    A transform sensor block measures the spatial relationship between two frames: the base B and the follower F. @@ -900,8 +901,8 @@ If we want to simulate an inertial sensor, we just have to choose B

    -
    -

    2.5 Excitation

    +
    +

    2.5 Excitation

    We can apply external forces to the model by using an external force and torque block. @@ -914,11 +915,11 @@ Internal force, acting reciprocally between base and following origins is implem

    -
    -

    3 Simulink files and signal names

    +
    +

    3 Simulink files and signal names

    - +

    @@ -926,8 +927,8 @@ In order to "normalize" things, the names of all the signal are listed here.

    -
    -

    3.1 List of Simscape files

    +
    +

    3.1 List of Simscape files

    Few different Simulink files are used: @@ -939,7 +940,7 @@ Few different Simulink files are used:

  • control
    • -
    Table 2: Joint primitives for each joint type
    +
    @@ -1006,14 +1007,14 @@ Few different Simulink files are used: -
    -

    3.2 List of Inputs

    +
    +

    3.2 List of Inputs

    -
    -

    3.2.1 Perturbations

    +
    +

    3.2.1 Perturbations

    -
    Table 3: List of simscape files
    +
    @@ -1059,10 +1060,10 @@ Few different Simulink files are used: -
    -

    3.2.2 Measurement Noise

    +
    +

    3.2.2 Measurement Noise

    -
    Table 4: List of Disturbances
    +
    @@ -1094,10 +1095,10 @@ Few different Simulink files are used: -
    -

    3.2.3 Control Inputs

    +
    +

    3.2.3 Control Inputs

    -
    Table 5: List of Measurement Noise
    +
    @@ -1219,10 +1220,10 @@ Few different Simulink files are used: -
    -

    3.3 List of Outputs

    +
    +

    3.3 List of Outputs

    -
    Table 6: List of Control Inputs
    +
    @@ -1338,11 +1339,11 @@ Few different Simulink files are used: -
    -

    4 Simulink Library

    +
    +

    4 Simulink Library

    - +

    @@ -1350,8 +1351,8 @@ A simulink library is developed in order to share elements between the different

    -
    -

    4.1 inputs

    +
    +

    4.1 inputs

    inputs.slx @@ -1359,8 +1360,8 @@ A simulink library is developed in order to share elements between the different

    -
    -

    4.2 nass_library

    +
    +

    4.2 nass_library

    nasslibrary.slx @@ -1368,8 +1369,8 @@ A simulink library is developed in order to share elements between the different

    -
    -

    4.3 pos_error_wrt_nass_base

    +
    +

    4.3 pos_error_wrt_nass_base

    poserrorwrtnassbase.slx @@ -1377,8 +1378,8 @@ A simulink library is developed in order to share elements between the different

    -
    -

    4.4 QuaternionToAngles

    +
    +

    4.4 QuaternionToAngles

    QuaternionToAngles.slx @@ -1386,8 +1387,8 @@ A simulink library is developed in order to share elements between the different

    -
    -

    4.5 RotationMatrixToAngle

    +
    +

    4.5 RotationMatrixToAngle

    RotationMatrixToAngle.slx @@ -1396,18 +1397,18 @@ A simulink library is developed in order to share elements between the different

    -
    -

    5 Functions

    +
    +

    5 Functions

    - +

    -
    -

    5.1 computePsdDispl

    +
    +

    5.1 computePsdDispl

    - +

    @@ -1443,11 +1444,11 @@ This Matlab function is accessible here.

    -
    -

    5.2 computeSetpoint

    +
    +

    5.2 computeSetpoint

    - +

    @@ -1519,11 +1520,11 @@ setpoint( -

    5.3 converErrorBasis

    +
    +

    5.3 converErrorBasis

    - +

    @@ -1661,11 +1662,11 @@ error_nass = [dx; dy; dz; th

    -
    -

    5.4 generateDiagPidControl

    +
    +

    5.4 generateDiagPidControl

    - +

    @@ -1695,11 +1696,11 @@ This Matlab function is accessible her

    -
    -

    5.5 identifyPlant

    +
    +

    5.5 identifyPlant

    - +

    @@ -1791,11 +1792,11 @@ This Matlab function is accessible here.

    -
    -

    5.6 runSimulation

    +
    +

    5.6 runSimulation

    - +

    @@ -1864,18 +1865,18 @@ This Matlab function is accessible here.

    -
    -

    6 Initialize Elements

    +
    +

    6 Initialize Elements

    - +

    -
    -

    6.1 Experiment

    +
    +

    6.1 Experiment

    - +

    @@ -1909,11 +1910,11 @@ This Matlab function is accessible here<

    -
    -

    6.2 Generate Reference Signals

    +
    +

    6.2 Generate Reference Signals

    - +

    @@ -1929,10 +1930,10 @@ This Matlab function is accessible here. 'Dy_amplitude', 0, ... % Amplitude of the displacement [m] 'Dy_period', 1, ... % Period of the displacement [s] 'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal" - 'Ry_amplitude', 0, ... % Amplitude [deg] + 'Ry_amplitude', 0, ... % Amplitude [rad] 'Ry_period', 10, ... % Period of the displacement [s] 'Rz_type', 'constant', ... % Either "constant" / "rotating" - 'Rz_amplitude', 0, ... % Initial angle [deg] + 'Rz_amplitude', 0, ... % Initial angle [rad] 'Rz_period', 1, ... % Period of the rotating [s] 'Dh_type', 'constant', ... % For now, only constant is implemented 'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform @@ -1976,13 +1977,13 @@ This Matlab function is accessible here. switch opts.Ry_type case 'constant' - Ry(:) = pi/180*opts.Ry_amplitude; + Ry(:) = opts.Ry_amplitude; case 'triangular' - Ry(:) = -4*(pi/180*opts.Ry_amplitude) + 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t; - Ry(t<0.75*opts.Ry_period) = 2*(pi/180*opts.Ry_amplitude) - 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.75*opts.Ry_period); - Ry(t<0.25*opts.Ry_period) = 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.25*opts.Ry_period); + Ry(:) = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t; + Ry(t<0.75*opts.Ry_period) = 2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period); + Ry(t<0.25*opts.Ry_period) = 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period); case 'sinusoidal' - Ry(:) = opts.Ry_amplitude*sin(2*pi/opts.Ry_period*t); + otherwise warning('Ry_type is not set correctly'); end @@ -1994,9 +1995,9 @@ This Matlab function is accessible here. switch opts.Rz_type case 'constant' - Rz(:) = pi/180*opts.Rz_amplitude; + Rz(:) = opts.Rz_amplitude; case 'rotating' - Rz(:) = pi/180*opts.Rz_amplitude+2*pi/opts.Rz_period*t; + Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t; otherwise warning('Rz_type is not set correctly'); end @@ -2006,8 +2007,6 @@ This Matlab function is accessible here. t = [0, Ts]; Dh = zeros(length(t), 6); - opts.Dh_pos(4:6) = pi/180*opts.Dh_pos(4:6); % convert from [deg] to [rad] - switch opts.Dh_type case 'constant' Dh = [opts.Dh_pos, opts.Dh_pos]; @@ -2025,8 +2024,6 @@ This Matlab function is accessible here. t = [0, Ts]; Dn = zeros(length(t), 6); - opts.Dn_pos(4:6) = pi/180*opts.Dn_pos(4:6); % convert from [deg] to [rad] - switch opts.Dn_type case 'constant' Dn = [opts.Dn_pos, opts.Dn_pos]; @@ -2043,11 +2040,11 @@ This Matlab function is accessible here.

    -
    -

    6.3 TODO Inputs

    +
    +

    6.3 TODO Inputs

    - +

      @@ -2239,11 +2236,11 @@ This Matlab function is accessible here.
    -
    -

    6.4 Ground

    +
    +

    6.4 Ground

    - +

    @@ -2267,11 +2264,11 @@ This Matlab function is accessible here.

    -
    -

    6.5 Granite

    +
    +

    6.5 Granite

    - +

    @@ -2337,11 +2334,11 @@ This Matlab function is accessible here

    -
    -

    6.6 Translation Stage

    +
    +

    6.6 Translation Stage

    - +

    @@ -2423,11 +2420,11 @@ This Matlab function is accessible here.

    -
    -

    6.7 Tilt Stage

    +
    +

    6.7 Tilt Stage

    - +

    @@ -2498,11 +2495,11 @@ This Matlab function is accessible here.

    -
    -

    6.8 Spindle

    +
    +

    6.8 Spindle

    - +

    @@ -2569,11 +2566,49 @@ This Matlab function is accessible here.

    -
    -

    6.9 Micro Hexapod

    +
    +

    6.9 Initialize Hexapod legs' length

    - + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    function [hexapod] = initializeHexapodPosition(hexapod, AP, ARB)
+% initializeHexapodPosition -
+%
+% Syntax: initializeHexapodPosition(hexapod, AP, ARB)
+%
+% Inputs:
+%    - hexapod - Hexapod object containing the geometry of the hexapod
+%    - AP - Position vector of point OB expressed in frame {A}
+%    - ARB - Rotation Matrix expressed in frame {A}
+
+  L0 = zeros(6, 1);
+
+  for i = 1:length(L)
+    Bbi = hexapod.pos_top_tranform(i, :)';
+    Aai = hexapod.pos_base(i, :)';
+
+    L0(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
+  end
+
+  hexapod.L0 = L0;
+end
+
    +
    +
    +
    + +
    +

    6.10 Micro Hexapod

    +
    +

    +

    @@ -2583,7 +2618,11 @@ This Matlab function is accessible her 

    function [micro_hexapod] = initializeMicroHexapod(opts_param)
     %% Default values for opts
-    opts = struct('rigid', false);
+    opts = struct(...
+      'rigid', false, ...
+      'AP',    zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A}
+      'ARB',   eye(3) ...       % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}
+    );
 
     %% Populate opts with input parameters
     if exist('opts_param','var')
@@ -2595,7 +2634,7 @@ This Matlab function is accessible her
     %% Stewart Object
     micro_hexapod = struct();
     micro_hexapod.h        = 350; % Total height of the platform [mm]
-    micro_hexapod.jacobian = 270; % Distance from the top platform to the Jacobian point [mm]
+    micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm]
 
     %% Bottom Plate - Mechanical Design
     BP = struct();
@@ -2630,7 +2669,7 @@ This Matlab function is accessible her
     else
       Leg.k.ax     = 2e7; % Stiffness of each leg [N/m]
     end
-    Leg.ksi.ax     = 0.1;     % Modal damping ksi = 1/2*c/sqrt(km) []
+    Leg.ksi.ax     = 0.1;   % Modal damping ksi = 1/2*c/sqrt(km) []
     Leg.rad.bottom = 25;    % Radius of the cylinder of the bottom part [mm]
     Leg.rad.top    = 17;    % Radius of the cylinder of the top part [mm]
     Leg.density    = 8000;  % Density of the material [kg/m^3]
@@ -2676,6 +2715,9 @@ This Matlab function is accessible her
     %%
     micro_hexapod = initializeParameters(micro_hexapod);
 
+    %% Setup equilibrium position of each leg
+    micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB);
+
     %% Save
     save('./mat/stages.mat', 'micro_hexapod', '-append');
 
@@ -2769,24 +2811,47 @@ This Matlab function is accessible her
         stewart.J  = getJacobianMatrix(leg_vectors', aa');
     end
 
-    function J  = getJacobianMatrix(RM,M_pos_base)
+    %%
+    function J  = getJacobianMatrix(RM, M_pos_base)
         % RM: [3x6] unit vector of each leg in the fixed frame
         % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
         J = zeros(6);
         J(:, 1:3) = RM';
         J(:, 4:6) = cross(M_pos_base, RM)';
     end
+
+    %%
+    function [L] = initializeHexapodPosition(hexapod, AP, ARB)
+    % initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position
+    %
+    % Syntax: initializeHexapodPosition(hexapod, AP, ARB)
+    %
+    % Inputs:
+    %    - hexapod - Hexapod object containing the geometry of the hexapod
+    %    - AP - Position vector of point OB expressed in frame {A} in [m]
+    %    - ARB - Rotation Matrix expressed in frame {A}
+
+      % Wanted Length of the hexapod's legs [m]
+      L = zeros(6, 1);
+
+      for i = 1:length(L)
+        Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
+        Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-micro_hexapod.h-hexapod.jacobian]; % [m]
+
+        L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
+      end
+    end
 end
    -
    -

    6.10 Center of gravity compensation

    -
    +
    +

    6.11 Center of gravity compensation

    +

    - +

    @@ -2830,11 +2895,11 @@ This Matlab function is accessible here.

    -
    -

    6.11 Mirror

    -
    +
    +

    6.12 Mirror

    +

    - +

    @@ -2900,11 +2965,11 @@ This Matlab function is accessible here.

    -
    -

    6.12 Nano Hexapod

    -
    +
    +

    6.13 Nano Hexapod

    +

    - +

    @@ -3121,11 +3186,11 @@ This Matlab function is accessible here

    -
    -

    6.13 Cedrat Actuator

    -
    +
    +

    6.14 Cedrat Actuator

    +

    - +

    @@ -3165,11 +3230,11 @@ This Matlab function is accessible here

    -
    -

    6.14 Sample

    -
    +
    +

    6.15 Sample

    +

    - +

    @@ -3214,7 +3279,7 @@ This Matlab function is accessible here.

    Author: Dehaeze Thomas

    -

    Created: 2019-12-06 ven. 12:04

    +

    Created: 2019-12-10 mar. 18:05

    Validate

    diff --git a/simscape/index.org b/simscape/index.org index 6f74b68..7909dc4 100644 --- a/simscape/index.org +++ b/simscape/index.org @@ -797,6 +797,7 @@ This Matlab function is accessible [[file:../src/initializeExperiment.m][here]]. :header-args:matlab+: :comments org :mkdirp yes :header-args:matlab+: :eval no :results none :END: + <> This Matlab function is accessible [[file:../src/initializeInputs.m][here]]. @@ -810,10 +811,10 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]]. 'Dy_amplitude', 0, ... % Amplitude of the displacement [m] 'Dy_period', 1, ... % Period of the displacement [s] 'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal" - 'Ry_amplitude', 0, ... % Amplitude [deg] + 'Ry_amplitude', 0, ... % Amplitude [rad] 'Ry_period', 10, ... % Period of the displacement [s] 'Rz_type', 'constant', ... % Either "constant" / "rotating" - 'Rz_amplitude', 0, ... % Initial angle [deg] + 'Rz_amplitude', 0, ... % Initial angle [rad] 'Rz_period', 1, ... % Period of the rotating [s] 'Dh_type', 'constant', ... % For now, only constant is implemented 'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform @@ -857,13 +858,13 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]]. switch opts.Ry_type case 'constant' - Ry(:) = pi/180*opts.Ry_amplitude; + Ry(:) = opts.Ry_amplitude; case 'triangular' - Ry(:) = -4*(pi/180*opts.Ry_amplitude) + 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t; - Ry(t<0.75*opts.Ry_period) = 2*(pi/180*opts.Ry_amplitude) - 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.75*opts.Ry_period); - Ry(t<0.25*opts.Ry_period) = 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.25*opts.Ry_period); + Ry(:) = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t; + Ry(t<0.75*opts.Ry_period) = 2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period); + Ry(t<0.25*opts.Ry_period) = 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period); case 'sinusoidal' - Ry(:) = opts.Ry_amplitude*sin(2*pi/opts.Ry_period*t); + otherwise warning('Ry_type is not set correctly'); end @@ -875,9 +876,9 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]]. switch opts.Rz_type case 'constant' - Rz(:) = pi/180*opts.Rz_amplitude; + Rz(:) = opts.Rz_amplitude; case 'rotating' - Rz(:) = pi/180*opts.Rz_amplitude+2*pi/opts.Rz_period*t; + Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t; otherwise warning('Rz_type is not set correctly'); end @@ -887,8 +888,6 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]]. t = [0, Ts]; Dh = zeros(length(t), 6); - opts.Dh_pos(4:6) = pi/180*opts.Dh_pos(4:6); % convert from [deg] to [rad] - switch opts.Dh_type case 'constant' Dh = [opts.Dh_pos, opts.Dh_pos]; @@ -906,8 +905,6 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]]. t = [0, Ts]; Dn = zeros(length(t), 6); - opts.Dn_pos(4:6) = pi/180*opts.Dn_pos(4:6); % convert from [deg] to [rad] - switch opts.Dn_type case 'constant' Dn = [opts.Dn_pos, opts.Dn_pos]; @@ -1421,6 +1418,40 @@ This Matlab function is accessible [[file:../src/initializeRz.m][here]]. end #+end_src +** Initialize Hexapod legs' length +:PROPERTIES: +:header-args:matlab+: :tangle ../src/initializeHexapodPosition.m +:header-args:matlab+: :comments org :mkdirp yes +:header-args:matlab+: :eval no :results none +:END: +<> + +This Matlab function is accessible [[file:../src/initializeHexapodPosition.m][here]]. + +#+begin_src matlab + function [hexapod] = initializeHexapodPosition(hexapod, AP, ARB) + % initializeHexapodPosition - + % + % Syntax: initializeHexapodPosition(hexapod, AP, ARB) + % + % Inputs: + % - hexapod - Hexapod object containing the geometry of the hexapod + % - AP - Position vector of point OB expressed in frame {A} + % - ARB - Rotation Matrix expressed in frame {A} + + L0 = zeros(6, 1); + + for i = 1:length(L) + Bbi = hexapod.pos_top_tranform(i, :)'; + Aai = hexapod.pos_base(i, :)'; + + L0(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai); + end + + hexapod.L0 = L0; + end +#+end_src + ** Micro Hexapod :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeMicroHexapod.m @@ -1434,7 +1465,11 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here] #+begin_src matlab function [micro_hexapod] = initializeMicroHexapod(opts_param) %% Default values for opts - opts = struct('rigid', false); + opts = struct(... + 'rigid', false, ... + 'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A} + 'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A} + ); %% Populate opts with input parameters if exist('opts_param','var') @@ -1446,7 +1481,7 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here] %% Stewart Object micro_hexapod = struct(); micro_hexapod.h = 350; % Total height of the platform [mm] - micro_hexapod.jacobian = 270; % Distance from the top platform to the Jacobian point [mm] + micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm] %% Bottom Plate - Mechanical Design BP = struct(); @@ -1481,7 +1516,7 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here] else Leg.k.ax = 2e7; % Stiffness of each leg [N/m] end - Leg.ksi.ax = 0.1; % Modal damping ksi = 1/2*c/sqrt(km) [] + Leg.ksi.ax = 0.1; % Modal damping ksi = 1/2*c/sqrt(km) [] Leg.rad.bottom = 25; % Radius of the cylinder of the bottom part [mm] Leg.rad.top = 17; % Radius of the cylinder of the top part [mm] Leg.density = 8000; % Density of the material [kg/m^3] @@ -1527,6 +1562,9 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here] %% micro_hexapod = initializeParameters(micro_hexapod); + %% Setup equilibrium position of each leg + micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB); + %% Save save('./mat/stages.mat', 'micro_hexapod', '-append'); @@ -1620,13 +1658,36 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here] stewart.J = getJacobianMatrix(leg_vectors', aa'); end - function J = getJacobianMatrix(RM,M_pos_base) + %% + function J = getJacobianMatrix(RM, M_pos_base) % RM: [3x6] unit vector of each leg in the fixed frame % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame J = zeros(6); J(:, 1:3) = RM'; J(:, 4:6) = cross(M_pos_base, RM)'; end + + %% + function [L] = initializeHexapodPosition(hexapod, AP, ARB) + % initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position + % + % Syntax: initializeHexapodPosition(hexapod, AP, ARB) + % + % Inputs: + % - hexapod - Hexapod object containing the geometry of the hexapod + % - AP - Position vector of point OB expressed in frame {A} in [m] + % - ARB - Rotation Matrix expressed in frame {A} + + % Wanted Length of the hexapod's legs [m] + L = zeros(6, 1); + + for i = 1:length(L) + Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m] + Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-micro_hexapod.h-hexapod.jacobian]; % [m] + + L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai); + end + end end #+end_src diff --git a/simscape_subsystems/hexapod_leg.slx b/simscape_subsystems/hexapod_leg.slx new file mode 100644 index 0000000..497b5de Binary files /dev/null and b/simscape_subsystems/hexapod_leg.slx differ diff --git a/src/initializeMicroHexapod.m b/src/initializeMicroHexapod.m index 47c3c9d..eeeb604 100644 --- a/src/initializeMicroHexapod.m +++ b/src/initializeMicroHexapod.m @@ -11,7 +11,11 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param) %% Default values for opts - opts = struct('rigid', false); + opts = struct(... + 'rigid', false, ... + 'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A} + 'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A} + ); %% Populate opts with input parameters if exist('opts_param','var') @@ -23,7 +27,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param) %% Stewart Object micro_hexapod = struct(); micro_hexapod.h = 350; % Total height of the platform [mm] - micro_hexapod.jacobian = 270; % Distance from the top platform to the Jacobian point [mm] + micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm] %% Bottom Plate - Mechanical Design BP = struct(); @@ -58,7 +62,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param) else Leg.k.ax = 2e7; % Stiffness of each leg [N/m] end - Leg.ksi.ax = 0.1; % Modal damping ksi = 1/2*c/sqrt(km) [] + Leg.ksi.ax = 0.1; % Modal damping ksi = 1/2*c/sqrt(km) [] Leg.rad.bottom = 25; % Radius of the cylinder of the bottom part [mm] Leg.rad.top = 17; % Radius of the cylinder of the top part [mm] Leg.density = 8000; % Density of the material [kg/m^3] @@ -104,6 +108,9 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param) %% micro_hexapod = initializeParameters(micro_hexapod); + %% Setup equilibrium position of each leg + micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB); + %% Save save('./mat/stages.mat', 'micro_hexapod', '-append'); @@ -197,11 +204,34 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param) stewart.J = getJacobianMatrix(leg_vectors', aa'); end - function J = getJacobianMatrix(RM,M_pos_base) + %% + function J = getJacobianMatrix(RM, M_pos_base) % RM: [3x6] unit vector of each leg in the fixed frame % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame J = zeros(6); J(:, 1:3) = RM'; J(:, 4:6) = cross(M_pos_base, RM)'; end + + %% + function [L] = initializeHexapodPosition(hexapod, AP, ARB) + % initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position + % + % Syntax: initializeHexapodPosition(hexapod, AP, ARB) + % + % Inputs: + % - hexapod - Hexapod object containing the geometry of the hexapod + % - AP - Position vector of point OB expressed in frame {A} in [m] + % - ARB - Rotation Matrix expressed in frame {A} + + % Wanted Length of the hexapod's legs [m] + L = zeros(6, 1); + + for i = 1:length(L) + Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m] + Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-micro_hexapod.h-hexapod.jacobian]; % [m] + + L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai); + end + end end
    Table 7: List of Outputs