[Done] Partition Stewart struct - Variant Simscape
This commit is contained in:
parent
2231aa53ce
commit
e1cd9b0aa8
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 5.4 KiB After Width: | Height: | Size: 5.1 KiB |
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 5.8 KiB After Width: | Height: | Size: 5.0 KiB |
@ -4,7 +4,7 @@
|
||||
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
||||
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
||||
<head>
|
||||
<!-- 2020-01-29 mer. 20:22 -->
|
||||
<!-- 2020-02-11 mar. 15:10 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||||
<title>Kinematic Study of the Stewart Platform</title>
|
||||
@ -247,15 +247,15 @@ for the JavaScript code in this tag.
|
||||
/*]]>*///-->
|
||||
</script>
|
||||
<script>
|
||||
MathJax = {
|
||||
tex: { macros: {
|
||||
bm: ["\\boldsymbol{#1}",1],
|
||||
MathJax = {
|
||||
tex: { macros: {
|
||||
bm: ["\\boldsymbol{#1}",1],
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
</script>
|
||||
<script type="text/javascript"
|
||||
src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js"></script>
|
||||
};
|
||||
</script>
|
||||
<script type="text/javascript"
|
||||
src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js"></script>
|
||||
</head>
|
||||
<body>
|
||||
<div id="org-div-home-and-up">
|
||||
@ -268,69 +268,73 @@ for the JavaScript code in this tag.
|
||||
<h2>Table of Contents</h2>
|
||||
<div id="text-table-of-contents">
|
||||
<ul>
|
||||
<li><a href="#orgc15a3c4">1. Jacobian Analysis</a>
|
||||
<li><a href="#org12dba9f">1. Jacobian Analysis</a>
|
||||
<ul>
|
||||
<li><a href="#orgfe46499">1.1. Jacobian Computation</a></li>
|
||||
<li><a href="#org47a46f9">1.2. Jacobian - Velocity loop closure</a></li>
|
||||
<li><a href="#org39fe037">1.3. Jacobian - Static Force Transformation</a></li>
|
||||
<li><a href="#orgf11b2b6">1.1. Jacobian Computation</a></li>
|
||||
<li><a href="#org77e916f">1.2. Jacobian - Velocity loop closure</a></li>
|
||||
<li><a href="#orgbd5d473">1.3. Jacobian - Static Force Transformation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org022818a">2. Stiffness Analysis</a>
|
||||
<li><a href="#orgecbb7b3">2. Stiffness Analysis</a>
|
||||
<ul>
|
||||
<li><a href="#orgd894508">2.1. Computation of the Stiffness and Compliance Matrix</a></li>
|
||||
<li><a href="#orgbea68d5">2.1. Computation of the Stiffness and Compliance Matrix</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org8111618">3. Forward and Inverse Kinematics</a>
|
||||
<li><a href="#org7c304b7">3. Forward and Inverse Kinematics</a>
|
||||
<ul>
|
||||
<li><a href="#org7985a6a">3.1. Inverse Kinematics</a></li>
|
||||
<li><a href="#orgf5c4aa7">3.2. Forward Kinematics</a></li>
|
||||
<li><a href="#orgbb81fc8">3.3. Approximate solution of the Forward and Inverse Kinematic problem for small displacement using the Jacobian matrix</a></li>
|
||||
<li><a href="#org62d03d6">3.4. Estimation of the range validity of the approximate inverse kinematics</a>
|
||||
<li><a href="#org9b180e9">3.1. Inverse Kinematics</a></li>
|
||||
<li><a href="#orga3a2319">3.2. Forward Kinematics</a></li>
|
||||
<li><a href="#org12d14b0">3.3. Approximate solution of the Forward and Inverse Kinematic problem for small displacement using the Jacobian matrix</a></li>
|
||||
<li><a href="#orgbad3ae9">3.4. Estimation of the range validity of the approximate inverse kinematics</a>
|
||||
<ul>
|
||||
<li><a href="#orga5bc217">3.4.1. Stewart architecture definition</a></li>
|
||||
<li><a href="#org871d94a">3.4.2. Comparison for “pure” translations</a></li>
|
||||
<li><a href="#orgc70d027">3.4.3. Conclusion</a></li>
|
||||
<li><a href="#orge3ab830">3.4.1. Stewart architecture definition</a></li>
|
||||
<li><a href="#org294ab65">3.4.2. Comparison for “pure” translations</a></li>
|
||||
<li><a href="#orgea391a3">3.4.3. Conclusion</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#orgac7950e">4. Estimated required actuator stroke from specified platform mobility</a>
|
||||
<li><a href="#orgddb8d5d">4. Estimated required actuator stroke from specified platform mobility</a>
|
||||
<ul>
|
||||
<li><a href="#org4c2a482">4.1. Stewart architecture definition</a></li>
|
||||
<li><a href="#org0e2cd41">4.2. Wanted translations and rotations</a></li>
|
||||
<li><a href="#org546da0e">4.3. Needed stroke for “pure” rotations or translations</a></li>
|
||||
<li><a href="#org782ea0a">4.4. Needed stroke for “combined” rotations or translations</a></li>
|
||||
<li><a href="#orgfb6bdb7">4.1. Stewart architecture definition</a></li>
|
||||
<li><a href="#orgba0801f">4.2. Wanted translations and rotations</a></li>
|
||||
<li><a href="#orgdeca909">4.3. Needed stroke for “pure” rotations or translations</a></li>
|
||||
<li><a href="#orgb713047">4.4. Needed stroke for “combined” rotations or translations</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org531e516">5. Estimated platform mobility from specified actuator stroke</a>
|
||||
<li><a href="#orge5f80c6">5. Estimated platform mobility from specified actuator stroke</a>
|
||||
<ul>
|
||||
<li><a href="#org741de4c">5.1. Stewart architecture definition</a></li>
|
||||
<li><a href="#orgbc52ab0">5.2. Pure translations</a></li>
|
||||
<li><a href="#org1865dbc">5.1. Stewart architecture definition</a></li>
|
||||
<li><a href="#orga4b5e5b">5.2. Pure translations</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org09341b5">6. Functions</a>
|
||||
<li><a href="#org748d42a">6. Functions</a>
|
||||
<ul>
|
||||
<li><a href="#orga28bc27">6.1. <code>computeJacobian</code>: Compute the Jacobian Matrix</a>
|
||||
<li><a href="#org00ba36f">6.1. <code>computeJacobian</code>: Compute the Jacobian Matrix</a>
|
||||
<ul>
|
||||
<li><a href="#org129055c">Function description</a></li>
|
||||
<li><a href="#orgffa7442">Compute Jacobian Matrix</a></li>
|
||||
<li><a href="#org318cdc7">Compute Stiffness Matrix</a></li>
|
||||
<li><a href="#orgeb50736">Compute Compliance Matrix</a></li>
|
||||
<li><a href="#orgbdf3a2a">Function description</a></li>
|
||||
<li><a href="#orgd1d8163">Check the <code>stewart</code> structure elements</a></li>
|
||||
<li><a href="#orge54466d">Compute Jacobian Matrix</a></li>
|
||||
<li><a href="#org560e1a1">Compute Stiffness Matrix</a></li>
|
||||
<li><a href="#orga853f82">Compute Compliance Matrix</a></li>
|
||||
<li><a href="#org4e11a57">Populate the <code>stewart</code> structure</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#orgccfd192">6.2. <code>inverseKinematics</code>: Compute Inverse Kinematics</a>
|
||||
<li><a href="#org6c7006f">6.2. <code>inverseKinematics</code>: Compute Inverse Kinematics</a>
|
||||
<ul>
|
||||
<li><a href="#org14c61e7">Function description</a></li>
|
||||
<li><a href="#orgb04bb62">Optional Parameters</a></li>
|
||||
<li><a href="#orge635491">Theory</a></li>
|
||||
<li><a href="#orgbc14960">Compute</a></li>
|
||||
<li><a href="#org7136bfb">Theory</a></li>
|
||||
<li><a href="#org75ec482">Function description</a></li>
|
||||
<li><a href="#orgc83e0f8">Optional Parameters</a></li>
|
||||
<li><a href="#org3ea2f51">Check the <code>stewart</code> structure elements</a></li>
|
||||
<li><a href="#org210ed60">Compute</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org1f75518">6.3. <code>forwardKinematicsApprox</code>: Compute the Approximate Forward Kinematics</a>
|
||||
<li><a href="#org8f0dc6c">6.3. <code>forwardKinematicsApprox</code>: Compute the Approximate Forward Kinematics</a>
|
||||
<ul>
|
||||
<li><a href="#org665e0fd">Function description</a></li>
|
||||
<li><a href="#org89c6f2b">Optional Parameters</a></li>
|
||||
<li><a href="#org3ae22c5">Computation</a></li>
|
||||
<li><a href="#org7c35749">Function description</a></li>
|
||||
<li><a href="#org5eee73c">Optional Parameters</a></li>
|
||||
<li><a href="#orgee36d86">Check the <code>stewart</code> structure elements</a></li>
|
||||
<li><a href="#orgc3e4684">Computation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
@ -353,17 +357,17 @@ In this analysis, the relation between the geometrical parameters of the manipul
|
||||
The current document is divided in the following sections:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>Section <a href="#org9f3c0b7">1</a>: The Jacobian matrix is derived from the geometry of the Stewart platform. Then it is shown that the Jacobian can link velocities and forces present in the system, and thus this matrix can be very useful for both analysis and control of the Stewart platform.</li>
|
||||
<li>Section <a href="#orgbd08620">2</a>: The stiffness and compliance matrices are derived from the Jacobian matrix and the stiffness of each strut.</li>
|
||||
<li>Section <a href="#orgde74d25">3</a>: The Forward and Inverse kinematic problems are presented.</li>
|
||||
<li>Section <a href="#orge3719af">4</a>: The Inverse kinematic solution is used to estimate required actuator stroke from the wanted mobility of the Stewart platform.</li>
|
||||
<li>Section <a href="#orge973aef">1</a>: The Jacobian matrix is derived from the geometry of the Stewart platform. Then it is shown that the Jacobian can link velocities and forces present in the system, and thus this matrix can be very useful for both analysis and control of the Stewart platform.</li>
|
||||
<li>Section <a href="#org2a4a521">2</a>: The stiffness and compliance matrices are derived from the Jacobian matrix and the stiffness of each strut.</li>
|
||||
<li>Section <a href="#org8833597">3</a>: The Forward and Inverse kinematic problems are presented.</li>
|
||||
<li>Section <a href="#org6ddfbc5">4</a>: The Inverse kinematic solution is used to estimate required actuator stroke from the wanted mobility of the Stewart platform.</li>
|
||||
</ul>
|
||||
|
||||
<div id="outline-container-orgc15a3c4" class="outline-2">
|
||||
<h2 id="orgc15a3c4"><span class="section-number-2">1</span> Jacobian Analysis</h2>
|
||||
<div id="outline-container-org12dba9f" class="outline-2">
|
||||
<h2 id="org12dba9f"><span class="section-number-2">1</span> Jacobian Analysis</h2>
|
||||
<div class="outline-text-2" id="text-1">
|
||||
<p>
|
||||
<a id="org9f3c0b7"></a>
|
||||
<a id="orge973aef"></a>
|
||||
</p>
|
||||
<p>
|
||||
From <a class='org-ref-reference' href="#taghirad13_paral">taghirad13_paral</a>:
|
||||
@ -374,8 +378,8 @@ The Jacobian matrix not only reveals the <b>relation between the joint variable
|
||||
</p>
|
||||
</blockquote>
|
||||
</div>
|
||||
<div id="outline-container-orgfe46499" class="outline-3">
|
||||
<h3 id="orgfe46499"><span class="section-number-3">1.1</span> Jacobian Computation</h3>
|
||||
<div id="outline-container-orgf11b2b6" class="outline-3">
|
||||
<h3 id="orgf11b2b6"><span class="section-number-3">1.1</span> Jacobian Computation</h3>
|
||||
<div class="outline-text-3" id="text-1-1">
|
||||
<p>
|
||||
If we note:
|
||||
@ -400,7 +404,7 @@ Then, we can compute the Jacobian with the following equation (the superscript \
|
||||
\end{equation*}
|
||||
|
||||
<p>
|
||||
The Jacobian matrix \(\bm{J}\) can be computed using the <code>computeJacobian</code> function (accessible <a href="#orga4eba5e">here</a>).
|
||||
The Jacobian matrix \(\bm{J}\) can be computed using the <code>computeJacobian</code> function (accessible <a href="#org519ef53">here</a>).
|
||||
For instance:
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
@ -418,8 +422,8 @@ This will add three new matrix to the <code>stewart</code> structure:
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org47a46f9" class="outline-3">
|
||||
<h3 id="org47a46f9"><span class="section-number-3">1.2</span> Jacobian - Velocity loop closure</h3>
|
||||
<div id="outline-container-org77e916f" class="outline-3">
|
||||
<h3 id="org77e916f"><span class="section-number-3">1.2</span> Jacobian - Velocity loop closure</h3>
|
||||
<div class="outline-text-3" id="text-1-2">
|
||||
<p>
|
||||
The Jacobian matrix links the input joint rate \(\dot{\bm{\mathcal{L}}} = [ \dot{l}_1, \dot{l}_2, \dot{l}_3, \dot{l}_4, \dot{l}_5, \dot{l}_6 ]^T\) of each strut to the output twist vector of the mobile platform is denoted by \(\dot{\bm{X}} = [^A\bm{v}_p, {}^A\bm{\omega}]^T\):
|
||||
@ -442,13 +446,13 @@ If the Jacobian matrix is inversible, we can also compute \(\dot{\bm{\mathcal{X}
|
||||
|
||||
<p>
|
||||
The Jacobian matrix can also be used to approximate forward and inverse kinematics for small displacements.
|
||||
This is explained in section <a href="#org907e556">3.3</a>.
|
||||
This is explained in section <a href="#org2e542bf">3.3</a>.
|
||||
</p>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org39fe037" class="outline-3">
|
||||
<h3 id="org39fe037"><span class="section-number-3">1.3</span> Jacobian - Static Force Transformation</h3>
|
||||
<div id="outline-container-orgbd5d473" class="outline-3">
|
||||
<h3 id="orgbd5d473"><span class="section-number-3">1.3</span> Jacobian - Static Force Transformation</h3>
|
||||
<div class="outline-text-3" id="text-1-3">
|
||||
<p>
|
||||
If we note:
|
||||
@ -475,19 +479,19 @@ If the Jacobian matrix is inversible, we also have the following relation:
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org022818a" class="outline-2">
|
||||
<h2 id="org022818a"><span class="section-number-2">2</span> Stiffness Analysis</h2>
|
||||
<div id="outline-container-orgecbb7b3" class="outline-2">
|
||||
<h2 id="orgecbb7b3"><span class="section-number-2">2</span> Stiffness Analysis</h2>
|
||||
<div class="outline-text-2" id="text-2">
|
||||
<p>
|
||||
<a id="orgbd08620"></a>
|
||||
<a id="org2a4a521"></a>
|
||||
</p>
|
||||
<p>
|
||||
Here, we focus on the deflections of the manipulator moving platform that are the result of the external applied wrench to the mobile platform.
|
||||
The amount of these deflections are a function of the applied wrench as well as the manipulator <b>structural stiffness</b>.
|
||||
</p>
|
||||
</div>
|
||||
<div id="outline-container-orgd894508" class="outline-3">
|
||||
<h3 id="orgd894508"><span class="section-number-3">2.1</span> Computation of the Stiffness and Compliance Matrix</h3>
|
||||
<div id="outline-container-orgbea68d5" class="outline-3">
|
||||
<h3 id="orgbea68d5"><span class="section-number-3">2.1</span> Computation of the Stiffness and Compliance Matrix</h3>
|
||||
<div class="outline-text-3" id="text-2-1">
|
||||
<p>
|
||||
As explain in <a href="stewart-architecture.html">this</a> document, each Actuator is modeled by 3 elements in parallel:
|
||||
@ -538,24 +542,24 @@ The compliance matrix of a manipulator shows the mapping of the moving platform
|
||||
\end{equation*}
|
||||
|
||||
<p>
|
||||
The stiffness and compliance matrices are computed using the <code>computeJacobian</code> function (accessible <a href="#orga4eba5e">here</a>).
|
||||
The stiffness and compliance matrices are computed using the <code>computeJacobian</code> function (accessible <a href="#org519ef53">here</a>).
|
||||
</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org8111618" class="outline-2">
|
||||
<h2 id="org8111618"><span class="section-number-2">3</span> Forward and Inverse Kinematics</h2>
|
||||
<div id="outline-container-org7c304b7" class="outline-2">
|
||||
<h2 id="org7c304b7"><span class="section-number-2">3</span> Forward and Inverse Kinematics</h2>
|
||||
<div class="outline-text-2" id="text-3">
|
||||
<p>
|
||||
<a id="orgde74d25"></a>
|
||||
<a id="org8833597"></a>
|
||||
</p>
|
||||
</div>
|
||||
<div id="outline-container-org7985a6a" class="outline-3">
|
||||
<h3 id="org7985a6a"><span class="section-number-3">3.1</span> Inverse Kinematics</h3>
|
||||
<div id="outline-container-org9b180e9" class="outline-3">
|
||||
<h3 id="org9b180e9"><span class="section-number-3">3.1</span> Inverse Kinematics</h3>
|
||||
<div class="outline-text-3" id="text-3-1">
|
||||
<p>
|
||||
<a id="orgc35b397"></a>
|
||||
<a id="org3d57c25"></a>
|
||||
</p>
|
||||
|
||||
<blockquote>
|
||||
@ -584,16 +588,16 @@ Otherwise, the solution gives complex numbers.
|
||||
</p>
|
||||
|
||||
<p>
|
||||
This inverse kinematic solution can be obtained using the function <code>inverseKinematics</code> (described <a href="#org38cce35">here</a>).
|
||||
This inverse kinematic solution can be obtained using the function <code>inverseKinematics</code> (described <a href="#orgaf5a9a0">here</a>).
|
||||
</p>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgf5c4aa7" class="outline-3">
|
||||
<h3 id="orgf5c4aa7"><span class="section-number-3">3.2</span> Forward Kinematics</h3>
|
||||
<div id="outline-container-orga3a2319" class="outline-3">
|
||||
<h3 id="orga3a2319"><span class="section-number-3">3.2</span> Forward Kinematics</h3>
|
||||
<div class="outline-text-3" id="text-3-2">
|
||||
<p>
|
||||
<a id="orga5abf69"></a>
|
||||
<a id="orgf7da4c0"></a>
|
||||
</p>
|
||||
|
||||
<blockquote>
|
||||
@ -612,11 +616,11 @@ In a next section, an approximate solution of the forward kinematics problem is
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgbb81fc8" class="outline-3">
|
||||
<h3 id="orgbb81fc8"><span class="section-number-3">3.3</span> Approximate solution of the Forward and Inverse Kinematic problem for small displacement using the Jacobian matrix</h3>
|
||||
<div id="outline-container-org12d14b0" class="outline-3">
|
||||
<h3 id="org12d14b0"><span class="section-number-3">3.3</span> Approximate solution of the Forward and Inverse Kinematic problem for small displacement using the Jacobian matrix</h3>
|
||||
<div class="outline-text-3" id="text-3-3">
|
||||
<p>
|
||||
<a id="org907e556"></a>
|
||||
<a id="org2e542bf"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -639,15 +643,18 @@ As the inverse kinematic can be easily solved exactly this is not much useful, h
|
||||
</p>
|
||||
|
||||
<p>
|
||||
The function <code>forwardKinematicsApprox</code> (described <a href="#org0199633">here</a>) can be used to solve the forward kinematic problem using the Jacobian matrix.
|
||||
The function <code>forwardKinematicsApprox</code> (described <a href="#org7ba3dc5">here</a>) can be used to solve the forward kinematic problem using the Jacobian matrix.
|
||||
</p>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org62d03d6" class="outline-3">
|
||||
<h3 id="org62d03d6"><span class="section-number-3">3.4</span> Estimation of the range validity of the approximate inverse kinematics</h3>
|
||||
<div id="outline-container-orgbad3ae9" class="outline-3">
|
||||
<h3 id="orgbad3ae9"><span class="section-number-3">3.4</span> Estimation of the range validity of the approximate inverse kinematics</h3>
|
||||
<div class="outline-text-3" id="text-3-4">
|
||||
<p>
|
||||
<a id="org6f7aec6"></a>
|
||||
</p>
|
||||
<p>
|
||||
As we know how to exactly solve the Inverse kinematic problem, we can compare the exact solution with the approximate solution using the Jacobian matrix.
|
||||
For small displacements, the approximate solution is expected to work well.
|
||||
We would like here to determine up to what displacement this approximation can be considered as correct.
|
||||
@ -659,8 +666,8 @@ This will also gives us the range for which the approximate forward kinematic is
|
||||
</p>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orga5bc217" class="outline-4">
|
||||
<h4 id="orga5bc217"><span class="section-number-4">3.4.1</span> Stewart architecture definition</h4>
|
||||
<div id="outline-container-orge3ab830" class="outline-4">
|
||||
<h4 id="orge3ab830"><span class="section-number-4">3.4.1</span> Stewart architecture definition</h4>
|
||||
<div class="outline-text-4" id="text-3-4-1">
|
||||
<p>
|
||||
We first define some general Stewart architecture.
|
||||
@ -680,8 +687,8 @@ stewart = computeJacobian(stewart);
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org871d94a" class="outline-4">
|
||||
<h4 id="org871d94a"><span class="section-number-4">3.4.2</span> Comparison for “pure” translations</h4>
|
||||
<div id="outline-container-org294ab65" class="outline-4">
|
||||
<h4 id="org294ab65"><span class="section-number-4">3.4.2</span> Comparison for “pure” translations</h4>
|
||||
<div class="outline-text-4" id="text-3-4-2">
|
||||
<p>
|
||||
Let’s first compare the perfect and approximate solution of the inverse for pure \(x\) translations.
|
||||
@ -689,8 +696,8 @@ Let’s first compare the perfect and approximate solution of the inverse fo
|
||||
|
||||
<p>
|
||||
We compute the approximate and exact required strut stroke to have the wanted mobile platform \(x\) displacement.
|
||||
The estimate required strut stroke for both the approximate and exact solutions are shown in Figure <a href="#org9fbaffc">1</a>.
|
||||
The relative strut length displacement is shown in Figure <a href="#org76bebd2">2</a>.
|
||||
The estimate required strut stroke for both the approximate and exact solutions are shown in Figure <a href="#orgb688993">1</a>.
|
||||
The relative strut length displacement is shown in Figure <a href="#org4c3ea1b">2</a>.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">Xrs = logspace(<span class="org-type">-</span>6, <span class="org-type">-</span>1, 100); <span class="org-comment">% Wanted X translation of the mobile platform [m]</span>
|
||||
@ -707,14 +714,14 @@ Ls_exact = zeros(6, length(Xrs));
|
||||
</div>
|
||||
|
||||
|
||||
<div id="org9fbaffc" class="figure">
|
||||
<div id="orgb688993" class="figure">
|
||||
<p><img src="figs/inverse_kinematics_approx_validity_x_translation.png" alt="inverse_kinematics_approx_validity_x_translation.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 1: </span>Comparison of the Approximate solution and True solution for the Inverse kinematic problem (<a href="./figs/inverse_kinematics_approx_validity_x_translation.png">png</a>, <a href="./figs/inverse_kinematics_approx_validity_x_translation.pdf">pdf</a>)</p>
|
||||
</div>
|
||||
|
||||
|
||||
<div id="org76bebd2" class="figure">
|
||||
<div id="org4c3ea1b" class="figure">
|
||||
<p><img src="figs/inverse_kinematics_approx_validity_x_translation_relative.png" alt="inverse_kinematics_approx_validity_x_translation_relative.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 2: </span>Relative length error by using the Approximate solution of the Inverse kinematic problem (<a href="./figs/inverse_kinematics_approx_validity_x_translation_relative.png">png</a>, <a href="./figs/inverse_kinematics_approx_validity_x_translation_relative.pdf">pdf</a>)</p>
|
||||
@ -722,8 +729,8 @@ Ls_exact = zeros(6, length(Xrs));
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgc70d027" class="outline-4">
|
||||
<h4 id="orgc70d027"><span class="section-number-4">3.4.3</span> Conclusion</h4>
|
||||
<div id="outline-container-orgea391a3" class="outline-4">
|
||||
<h4 id="orgea391a3"><span class="section-number-4">3.4.3</span> Conclusion</h4>
|
||||
<div class="outline-text-4" id="text-3-4-3">
|
||||
<p>
|
||||
For small wanted displacements (up to \(\approx 1\%\) of the size of the Hexapod), the approximate inverse kinematic solution using the Jacobian matrix is quite correct.
|
||||
@ -733,11 +740,11 @@ For small wanted displacements (up to \(\approx 1\%\) of the size of the Hexapod
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgac7950e" class="outline-2">
|
||||
<h2 id="orgac7950e"><span class="section-number-2">4</span> Estimated required actuator stroke from specified platform mobility</h2>
|
||||
<div id="outline-container-orgddb8d5d" class="outline-2">
|
||||
<h2 id="orgddb8d5d"><span class="section-number-2">4</span> Estimated required actuator stroke from specified platform mobility</h2>
|
||||
<div class="outline-text-2" id="text-4">
|
||||
<p>
|
||||
<a id="orge3719af"></a>
|
||||
<a id="org6ddfbc5"></a>
|
||||
</p>
|
||||
<p>
|
||||
Let’s say one want to design a Stewart platform with some specified mobility (position and orientation).
|
||||
@ -745,8 +752,8 @@ One may want to determine the required actuator stroke required to obtain the sp
|
||||
This is what is analyzed in this section.
|
||||
</p>
|
||||
</div>
|
||||
<div id="outline-container-org4c2a482" class="outline-3">
|
||||
<h3 id="org4c2a482"><span class="section-number-3">4.1</span> Stewart architecture definition</h3>
|
||||
<div id="outline-container-orgfb6bdb7" class="outline-3">
|
||||
<h3 id="orgfb6bdb7"><span class="section-number-3">4.1</span> Stewart architecture definition</h3>
|
||||
<div class="outline-text-3" id="text-4-1">
|
||||
<p>
|
||||
Let’s first define the Stewart platform architecture that we want to study.
|
||||
@ -766,8 +773,8 @@ stewart = computeJacobian(stewart);
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org0e2cd41" class="outline-3">
|
||||
<h3 id="org0e2cd41"><span class="section-number-3">4.2</span> Wanted translations and rotations</h3>
|
||||
<div id="outline-container-orgba0801f" class="outline-3">
|
||||
<h3 id="orgba0801f"><span class="section-number-3">4.2</span> Wanted translations and rotations</h3>
|
||||
<div class="outline-text-3" id="text-4-2">
|
||||
<p>
|
||||
Let’s now define the wanted extreme translations and rotations.
|
||||
@ -784,8 +791,8 @@ Rz_max = 0; <span class="org-comment">% Rotation [rad]</span>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org546da0e" class="outline-3">
|
||||
<h3 id="org546da0e"><span class="section-number-3">4.3</span> Needed stroke for “pure” rotations or translations</h3>
|
||||
<div id="outline-container-orgdeca909" class="outline-3">
|
||||
<h3 id="orgdeca909"><span class="section-number-3">4.3</span> Needed stroke for “pure” rotations or translations</h3>
|
||||
<div class="outline-text-3" id="text-4-3">
|
||||
<p>
|
||||
As a first estimation, we estimate the needed actuator stroke for “pure” rotations and translation.
|
||||
@ -816,8 +823,8 @@ This is surely a low estimation of the required stroke.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org782ea0a" class="outline-3">
|
||||
<h3 id="org782ea0a"><span class="section-number-3">4.4</span> Needed stroke for “combined” rotations or translations</h3>
|
||||
<div id="outline-container-orgb713047" class="outline-3">
|
||||
<h3 id="orgb713047"><span class="section-number-3">4.4</span> Needed stroke for “combined” rotations or translations</h3>
|
||||
<div class="outline-text-3" id="text-4-4">
|
||||
<p>
|
||||
We know would like to have a more precise estimation.
|
||||
@ -1137,23 +1144,23 @@ This is probably a much realistic estimation of the required actuator stroke.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org531e516" class="outline-2">
|
||||
<h2 id="org531e516"><span class="section-number-2">5</span> Estimated platform mobility from specified actuator stroke</h2>
|
||||
<div id="outline-container-orge5f80c6" class="outline-2">
|
||||
<h2 id="orge5f80c6"><span class="section-number-2">5</span> Estimated platform mobility from specified actuator stroke</h2>
|
||||
<div class="outline-text-2" id="text-5">
|
||||
<p>
|
||||
<a id="orgf61fb6c"></a>
|
||||
<a id="org10d9ea8"></a>
|
||||
</p>
|
||||
<p>
|
||||
Here, from some value of the actuator stroke, we would like to estimate the mobility of the Stewart platform.
|
||||
</p>
|
||||
|
||||
<p>
|
||||
As explained in section <a href="#orgde74d25">3</a>, the forward kinematic problem of the Stewart platform is quite difficult to solve.
|
||||
As explained in section <a href="#org8833597">3</a>, the forward kinematic problem of the Stewart platform is quite difficult to solve.
|
||||
However, for small displacements, we can use the Jacobian as an approximate solution.
|
||||
</p>
|
||||
</div>
|
||||
<div id="outline-container-org741de4c" class="outline-3">
|
||||
<h3 id="org741de4c"><span class="section-number-3">5.1</span> Stewart architecture definition</h3>
|
||||
<div id="outline-container-org1865dbc" class="outline-3">
|
||||
<h3 id="org1865dbc"><span class="section-number-3">5.1</span> Stewart architecture definition</h3>
|
||||
<div class="outline-text-3" id="text-5-1">
|
||||
<p>
|
||||
Let’s first define the Stewart platform architecture that we want to study.
|
||||
@ -1182,8 +1189,8 @@ L_max = 50e<span class="org-type">-</span>6; <span class="org-comment">% [m]</s
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgbc52ab0" class="outline-3">
|
||||
<h3 id="orgbc52ab0"><span class="section-number-3">5.2</span> Pure translations</h3>
|
||||
<div id="outline-container-orga4b5e5b" class="outline-3">
|
||||
<h3 id="orga4b5e5b"><span class="section-number-3">5.2</span> Pure translations</h3>
|
||||
<div class="outline-text-3" id="text-5-2">
|
||||
<p>
|
||||
Let’s first estimate the mobility in translation when the orientation of the Stewart platform stays the same.
|
||||
@ -1255,7 +1262,7 @@ We can also approximate the mobility by a sphere with a radius equal to the mini
|
||||
</table>
|
||||
|
||||
|
||||
<div id="org16756e7" class="figure">
|
||||
<div id="orga71e017" class="figure">
|
||||
<p><img src="figs/mobility_translations_null_rotation.png" alt="mobility_translations_null_rotation.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 3: </span>Obtain mobility of the Stewart platform for zero rotations (<a href="./figs/mobility_translations_null_rotation.png">png</a>, <a href="./figs/mobility_translations_null_rotation.pdf">pdf</a>)</p>
|
||||
@ -1264,18 +1271,18 @@ We can also approximate the mobility by a sphere with a radius equal to the mini
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org09341b5" class="outline-2">
|
||||
<h2 id="org09341b5"><span class="section-number-2">6</span> Functions</h2>
|
||||
<div id="outline-container-org748d42a" class="outline-2">
|
||||
<h2 id="org748d42a"><span class="section-number-2">6</span> Functions</h2>
|
||||
<div class="outline-text-2" id="text-6">
|
||||
<p>
|
||||
<a id="org2d73657"></a>
|
||||
<a id="org1e1d299"></a>
|
||||
</p>
|
||||
</div>
|
||||
<div id="outline-container-orga28bc27" class="outline-3">
|
||||
<h3 id="orga28bc27"><span class="section-number-3">6.1</span> <code>computeJacobian</code>: Compute the Jacobian Matrix</h3>
|
||||
<div id="outline-container-org00ba36f" class="outline-3">
|
||||
<h3 id="org00ba36f"><span class="section-number-3">6.1</span> <code>computeJacobian</code>: Compute the Jacobian Matrix</h3>
|
||||
<div class="outline-text-3" id="text-6-1">
|
||||
<p>
|
||||
<a id="orga4eba5e"></a>
|
||||
<a id="org519ef53"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1283,9 +1290,9 @@ This Matlab function is accessible <a href="src/computeJacobian.m">here</a>.
|
||||
</p>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org129055c" class="outline-4">
|
||||
<h4 id="org129055c">Function description</h4>
|
||||
<div class="outline-text-4" id="text-org129055c">
|
||||
<div id="outline-container-orgbdf3a2a" class="outline-4">
|
||||
<h4 id="orgbdf3a2a">Function description</h4>
|
||||
<div class="outline-text-4" id="text-orgbdf3a2a">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[stewart]</span> = <span class="org-function-name">computeJacobian</span>(<span class="org-variable-name">stewart</span>)
|
||||
<span class="org-comment">% computeJacobian -</span>
|
||||
@ -1294,55 +1301,87 @@ This Matlab function is accessible <a href="src/computeJacobian.m">here</a>.
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Inputs:</span>
|
||||
<span class="org-comment">% - stewart - With at least the following fields:</span>
|
||||
<span class="org-comment">% - As [3x6] - The 6 unit vectors for each strut expressed in {A}</span>
|
||||
<span class="org-comment">% - Ab [3x6] - The 6 position of the joints bi expressed in {A}</span>
|
||||
<span class="org-comment">% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}</span>
|
||||
<span class="org-comment">% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}</span>
|
||||
<span class="org-comment">% - actuators.K [6x1] - Total stiffness of the actuators</span>
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Outputs:</span>
|
||||
<span class="org-comment">% - stewart - With the 3 added field:</span>
|
||||
<span class="org-comment">% - J [6x6] - The Jacobian Matrix</span>
|
||||
<span class="org-comment">% - K [6x6] - The Stiffness Matrix</span>
|
||||
<span class="org-comment">% - C [6x6] - The Compliance Matrix</span>
|
||||
<span class="org-comment">% - kinematics.J [6x6] - The Jacobian Matrix</span>
|
||||
<span class="org-comment">% - kinematics.K [6x6] - The Stiffness Matrix</span>
|
||||
<span class="org-comment">% - kinematics.C [6x6] - The Compliance Matrix</span>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgffa7442" class="outline-4">
|
||||
<h4 id="orgffa7442">Compute Jacobian Matrix</h4>
|
||||
<div class="outline-text-4" id="text-orgffa7442">
|
||||
<div id="outline-container-orgd1d8163" class="outline-4">
|
||||
<h4 id="orgd1d8163">Check the <code>stewart</code> structure elements</h4>
|
||||
<div class="outline-text-4" id="text-orgd1d8163">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">stewart.J = [stewart.As<span class="org-type">'</span> , cross(stewart.Ab, stewart.As)<span class="org-type">'</span>];
|
||||
<pre class="src src-matlab">assert(isfield(stewart.geometry, <span class="org-string">'As'</span>), <span class="org-string">'stewart.geometry should have attribute As'</span>)
|
||||
As = stewart.geometry.As;
|
||||
|
||||
assert(isfield(stewart.geometry, <span class="org-string">'Ab'</span>), <span class="org-string">'stewart.geometry should have attribute Ab'</span>)
|
||||
Ab = stewart.geometry.Ab;
|
||||
|
||||
assert(isfield(stewart.actuators, <span class="org-string">'K'</span>), <span class="org-string">'stewart.actuators should have attribute K'</span>)
|
||||
Ki = stewart.actuators.K;
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org318cdc7" class="outline-4">
|
||||
<h4 id="org318cdc7">Compute Stiffness Matrix</h4>
|
||||
<div class="outline-text-4" id="text-org318cdc7">
|
||||
|
||||
<div id="outline-container-orge54466d" class="outline-4">
|
||||
<h4 id="orge54466d">Compute Jacobian Matrix</h4>
|
||||
<div class="outline-text-4" id="text-orge54466d">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">stewart.K = stewart.J<span class="org-type">'*</span>diag(stewart.Ki)<span class="org-type">*</span>stewart.J;
|
||||
<pre class="src src-matlab">J = [As<span class="org-type">'</span> , cross(Ab, As)<span class="org-type">'</span>];
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgeb50736" class="outline-4">
|
||||
<h4 id="orgeb50736">Compute Compliance Matrix</h4>
|
||||
<div class="outline-text-4" id="text-orgeb50736">
|
||||
<div id="outline-container-org560e1a1" class="outline-4">
|
||||
<h4 id="org560e1a1">Compute Stiffness Matrix</h4>
|
||||
<div class="outline-text-4" id="text-org560e1a1">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">stewart.C = inv(stewart.K);
|
||||
<pre class="src src-matlab">K = J<span class="org-type">'*</span>diag(Ki)<span class="org-type">*</span>J;
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orga853f82" class="outline-4">
|
||||
<h4 id="orga853f82">Compute Compliance Matrix</h4>
|
||||
<div class="outline-text-4" id="text-orga853f82">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">C = inv(K);
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org4e11a57" class="outline-4">
|
||||
<h4 id="org4e11a57">Populate the <code>stewart</code> structure</h4>
|
||||
<div class="outline-text-4" id="text-org4e11a57">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">stewart.kinematics.J = J;
|
||||
stewart.kinematics.K = K;
|
||||
stewart.kinematics.C = C;
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgccfd192" class="outline-3">
|
||||
<h3 id="orgccfd192"><span class="section-number-3">6.2</span> <code>inverseKinematics</code>: Compute Inverse Kinematics</h3>
|
||||
|
||||
<div id="outline-container-org6c7006f" class="outline-3">
|
||||
<h3 id="org6c7006f"><span class="section-number-3">6.2</span> <code>inverseKinematics</code>: Compute Inverse Kinematics</h3>
|
||||
<div class="outline-text-3" id="text-6-2">
|
||||
<p>
|
||||
<a id="org38cce35"></a>
|
||||
<a id="orgaf5a9a0"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1350,48 +1389,9 @@ This Matlab function is accessible <a href="src/inverseKinematics.m">here</a>.
|
||||
</p>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org14c61e7" class="outline-4">
|
||||
<h4 id="org14c61e7">Function description</h4>
|
||||
<div class="outline-text-4" id="text-org14c61e7">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[Li, dLi]</span> = <span class="org-function-name">inverseKinematics</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
||||
<span class="org-comment">% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}</span>
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Syntax: [stewart] = inverseKinematics(stewart)</span>
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Inputs:</span>
|
||||
<span class="org-comment">% - stewart - A structure with the following fields</span>
|
||||
<span class="org-comment">% - Aa [3x6] - The positions ai expressed in {A}</span>
|
||||
<span class="org-comment">% - Bb [3x6] - The positions bi expressed in {B}</span>
|
||||
<span class="org-comment">% - args - Can have the following fields:</span>
|
||||
<span class="org-comment">% - AP [3x1] - The wanted position of {B} with respect to {A}</span>
|
||||
<span class="org-comment">% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}</span>
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Outputs:</span>
|
||||
<span class="org-comment">% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}</span>
|
||||
<span class="org-comment">% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}</span>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgb04bb62" class="outline-4">
|
||||
<h4 id="orgb04bb62">Optional Parameters</h4>
|
||||
<div class="outline-text-4" id="text-orgb04bb62">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
<span class="org-keyword">end</span>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orge635491" class="outline-4">
|
||||
<h4 id="orge635491">Theory</h4>
|
||||
<div class="outline-text-4" id="text-orge635491">
|
||||
<div id="outline-container-org7136bfb" class="outline-4">
|
||||
<h4 id="org7136bfb">Theory</h4>
|
||||
<div class="outline-text-4" id="text-org7136bfb">
|
||||
<p>
|
||||
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\).
|
||||
</p>
|
||||
@ -1425,27 +1425,85 @@ Otherwise, when the limbs’ lengths derived yield complex numbers, then the
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgbc14960" class="outline-4">
|
||||
<h4 id="orgbc14960">Compute</h4>
|
||||
<div class="outline-text-4" id="text-orgbc14960">
|
||||
<div id="outline-container-org75ec482" class="outline-4">
|
||||
<h4 id="org75ec482">Function description</h4>
|
||||
<div class="outline-text-4" id="text-org75ec482">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">Li = sqrt(args.AP<span class="org-type">'*</span>args.AP <span class="org-type">+</span> diag(stewart.Bb<span class="org-type">'*</span>stewart.Bb) <span class="org-type">+</span> diag(stewart.Aa<span class="org-type">'*</span>stewart.Aa) <span class="org-type">-</span> (2<span class="org-type">*</span>args.AP<span class="org-type">'*</span>stewart.Aa)<span class="org-type">'</span> <span class="org-type">+</span> (2<span class="org-type">*</span>args.AP<span class="org-type">'*</span>(args.ARB<span class="org-type">*</span>stewart.Bb))<span class="org-type">'</span> <span class="org-type">-</span> diag(2<span class="org-type">*</span>(args.ARB<span class="org-type">*</span>stewart.Bb)<span class="org-type">'*</span>stewart.Aa));
|
||||
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[Li, dLi]</span> = <span class="org-function-name">inverseKinematics</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
||||
<span class="org-comment">% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}</span>
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Syntax: [stewart] = inverseKinematics(stewart)</span>
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Inputs:</span>
|
||||
<span class="org-comment">% - stewart - A structure with the following fields</span>
|
||||
<span class="org-comment">% - geometry.Aa [3x6] - The positions ai expressed in {A}</span>
|
||||
<span class="org-comment">% - geometry.Bb [3x6] - The positions bi expressed in {B}</span>
|
||||
<span class="org-comment">% - geometry.l [6x1] - Length of each strut</span>
|
||||
<span class="org-comment">% - args - Can have the following fields:</span>
|
||||
<span class="org-comment">% - AP [3x1] - The wanted position of {B} with respect to {A}</span>
|
||||
<span class="org-comment">% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}</span>
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Outputs:</span>
|
||||
<span class="org-comment">% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}</span>
|
||||
<span class="org-comment">% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}</span>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgc83e0f8" class="outline-4">
|
||||
<h4 id="orgc83e0f8">Optional Parameters</h4>
|
||||
<div class="outline-text-4" id="text-orgc83e0f8">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
<span class="org-keyword">end</span>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org3ea2f51" class="outline-4">
|
||||
<h4 id="org3ea2f51">Check the <code>stewart</code> structure elements</h4>
|
||||
<div class="outline-text-4" id="text-org3ea2f51">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">assert(isfield(stewart.geometry, <span class="org-string">'Aa'</span>), <span class="org-string">'stewart.geometry should have attribute Aa'</span>)
|
||||
Aa = stewart.geometry.Aa;
|
||||
|
||||
assert(isfield(stewart.geometry, <span class="org-string">'Bb'</span>), <span class="org-string">'stewart.geometry should have attribute Bb'</span>)
|
||||
Bb = stewart.geometry.Bb;
|
||||
|
||||
assert(isfield(stewart.geometry, <span class="org-string">'l'</span>), <span class="org-string">'stewart.geometry should have attribute l'</span>)
|
||||
l = stewart.geometry.l;
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
|
||||
<div id="outline-container-org210ed60" class="outline-4">
|
||||
<h4 id="org210ed60">Compute</h4>
|
||||
<div class="outline-text-4" id="text-org210ed60">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">Li = sqrt(args.AP<span class="org-type">'*</span>args.AP <span class="org-type">+</span> diag(Bb<span class="org-type">'*</span>Bb) <span class="org-type">+</span> diag(Aa<span class="org-type">'*</span>Aa) <span class="org-type">-</span> (2<span class="org-type">*</span>args.AP<span class="org-type">'*</span>Aa)<span class="org-type">'</span> <span class="org-type">+</span> (2<span class="org-type">*</span>args.AP<span class="org-type">'*</span>(args.ARB<span class="org-type">*</span>Bb))<span class="org-type">'</span> <span class="org-type">-</span> diag(2<span class="org-type">*</span>(args.ARB<span class="org-type">*</span>Bb)<span class="org-type">'*</span>Aa));
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">dLi = Li<span class="org-type">-</span>stewart.l;
|
||||
<pre class="src src-matlab">dLi = Li<span class="org-type">-</span>l;
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org1f75518" class="outline-3">
|
||||
<h3 id="org1f75518"><span class="section-number-3">6.3</span> <code>forwardKinematicsApprox</code>: Compute the Approximate Forward Kinematics</h3>
|
||||
<div id="outline-container-org8f0dc6c" class="outline-3">
|
||||
<h3 id="org8f0dc6c"><span class="section-number-3">6.3</span> <code>forwardKinematicsApprox</code>: Compute the Approximate Forward Kinematics</h3>
|
||||
<div class="outline-text-3" id="text-6-3">
|
||||
<p>
|
||||
<a id="org0199633"></a>
|
||||
<a id="org7ba3dc5"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1453,9 +1511,9 @@ This Matlab function is accessible <a href="src/forwardKinematicsApprox.m">here<
|
||||
</p>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org665e0fd" class="outline-4">
|
||||
<h4 id="org665e0fd">Function description</h4>
|
||||
<div class="outline-text-4" id="text-org665e0fd">
|
||||
<div id="outline-container-org7c35749" class="outline-4">
|
||||
<h4 id="org7c35749">Function description</h4>
|
||||
<div class="outline-text-4" id="text-org7c35749">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[P, R]</span> = <span class="org-function-name">forwardKinematicsApprox</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
||||
<span class="org-comment">% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using</span>
|
||||
@ -1465,7 +1523,7 @@ This Matlab function is accessible <a href="src/forwardKinematicsApprox.m">here<
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Inputs:</span>
|
||||
<span class="org-comment">% - stewart - A structure with the following fields</span>
|
||||
<span class="org-comment">% - J [6x6] - The Jacobian Matrix</span>
|
||||
<span class="org-comment">% - kinematics.J [6x6] - The Jacobian Matrix</span>
|
||||
<span class="org-comment">% - args - Can have the following fields:</span>
|
||||
<span class="org-comment">% - dL [6x1] - Displacement of each strut [m]</span>
|
||||
<span class="org-comment">%</span>
|
||||
@ -1477,9 +1535,9 @@ This Matlab function is accessible <a href="src/forwardKinematicsApprox.m">here<
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org89c6f2b" class="outline-4">
|
||||
<h4 id="org89c6f2b">Optional Parameters</h4>
|
||||
<div class="outline-text-4" id="text-org89c6f2b">
|
||||
<div id="outline-container-org5eee73c" class="outline-4">
|
||||
<h4 id="org5eee73c">Optional Parameters</h4>
|
||||
<div class="outline-text-4" id="text-org5eee73c">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">arguments
|
||||
stewart
|
||||
@ -1490,16 +1548,27 @@ This Matlab function is accessible <a href="src/forwardKinematicsApprox.m">here<
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org3ae22c5" class="outline-4">
|
||||
<h4 id="org3ae22c5">Computation</h4>
|
||||
<div class="outline-text-4" id="text-org3ae22c5">
|
||||
<div id="outline-container-orgee36d86" class="outline-4">
|
||||
<h4 id="orgee36d86">Check the <code>stewart</code> structure elements</h4>
|
||||
<div class="outline-text-4" id="text-orgee36d86">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">assert(isfield(stewart.kinematics, <span class="org-string">'J'</span>), <span class="org-string">'stewart.kinematics should have attribute J'</span>)
|
||||
J = stewart.kinematics.J;
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgc3e4684" class="outline-4">
|
||||
<h4 id="orgc3e4684">Computation</h4>
|
||||
<div class="outline-text-4" id="text-orgc3e4684">
|
||||
<p>
|
||||
From a small displacement of each strut \(d\bm{\mathcal{L}}\), we can compute the
|
||||
position and orientation of {B} with respect to {A} using the following formula:
|
||||
\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">X = stewart.J<span class="org-type">\</span>args.dL;
|
||||
<pre class="src src-matlab">X = J<span class="org-type">\</span>args.dL;
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
@ -1534,6 +1603,7 @@ We then compute the corresponding rotation matrix.
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
|
||||
<h1 class='org-ref-bib-h1'>Bibliography</h1>
|
||||
@ -1543,7 +1613,7 @@ We then compute the corresponding rotation matrix.
|
||||
</div>
|
||||
<div id="postamble" class="status">
|
||||
<p class="author">Author: Dehaeze Thomas</p>
|
||||
<p class="date">Created: 2020-01-29 mer. 20:22</p>
|
||||
<p class="date">Created: 2020-02-11 mar. 15:10</p>
|
||||
</div>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
@ -591,22 +591,39 @@ This Matlab function is accessible [[file:src/computeJacobian.m][here]].
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - With at least the following fields:
|
||||
% - As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
% - actuators.K [6x1] - Total stiffness of the actuators
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - With the 3 added field:
|
||||
% - J [6x6] - The Jacobian Matrix
|
||||
% - K [6x6] - The Stiffness Matrix
|
||||
% - C [6x6] - The Compliance Matrix
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - kinematics.K [6x6] - The Stiffness Matrix
|
||||
% - kinematics.C [6x6] - The Compliance Matrix
|
||||
#+end_src
|
||||
|
||||
*** Check the =stewart= structure elements
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
||||
As = stewart.geometry.As;
|
||||
|
||||
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
||||
Ab = stewart.geometry.Ab;
|
||||
|
||||
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
|
||||
Ki = stewart.actuators.K;
|
||||
#+end_src
|
||||
|
||||
|
||||
*** Compute Jacobian Matrix
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)'];
|
||||
J = [As' , cross(Ab, As)'];
|
||||
#+end_src
|
||||
|
||||
*** Compute Stiffness Matrix
|
||||
@ -614,7 +631,7 @@ This Matlab function is accessible [[file:src/computeJacobian.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J;
|
||||
K = J'*diag(Ki)*J;
|
||||
#+end_src
|
||||
|
||||
*** Compute Compliance Matrix
|
||||
@ -622,9 +639,20 @@ This Matlab function is accessible [[file:src/computeJacobian.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
stewart.C = inv(stewart.K);
|
||||
C = inv(K);
|
||||
#+end_src
|
||||
|
||||
*** Populate the =stewart= structure
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
stewart.kinematics.J = J;
|
||||
stewart.kinematics.K = K;
|
||||
stewart.kinematics.C = C;
|
||||
#+end_src
|
||||
|
||||
|
||||
** =inverseKinematics=: Compute Inverse Kinematics
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle src/inverseKinematics.m
|
||||
@ -634,41 +662,6 @@ This Matlab function is accessible [[file:src/computeJacobian.m][here]].
|
||||
|
||||
This Matlab function is accessible [[file:src/inverseKinematics.m][here]].
|
||||
|
||||
*** Function description
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [Li, dLi] = inverseKinematics(stewart, args)
|
||||
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
||||
%
|
||||
% Syntax: [stewart] = inverseKinematics(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - Aa [3x6] - The positions ai expressed in {A}
|
||||
% - Bb [3x6] - The positions bi expressed in {B}
|
||||
% - args - Can have the following fields:
|
||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Theory
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
@ -694,16 +687,68 @@ Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
|
||||
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.
|
||||
|
||||
*** Function description
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [Li, dLi] = inverseKinematics(stewart, args)
|
||||
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
||||
%
|
||||
% Syntax: [stewart] = inverseKinematics(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - geometry.Aa [3x6] - The positions ai expressed in {A}
|
||||
% - geometry.Bb [3x6] - The positions bi expressed in {B}
|
||||
% - geometry.l [6x1] - Length of each strut
|
||||
% - args - Can have the following fields:
|
||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Check the =stewart= structure elements
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
|
||||
Aa = stewart.geometry.Aa;
|
||||
|
||||
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
|
||||
Bb = stewart.geometry.Bb;
|
||||
|
||||
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
|
||||
l = stewart.geometry.l;
|
||||
#+end_src
|
||||
|
||||
|
||||
*** Compute
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
Li = sqrt(args.AP'*args.AP + diag(stewart.Bb'*stewart.Bb) + diag(stewart.Aa'*stewart.Aa) - (2*args.AP'*stewart.Aa)' + (2*args.AP'*(args.ARB*stewart.Bb))' - diag(2*(args.ARB*stewart.Bb)'*stewart.Aa));
|
||||
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
dLi = Li-stewart.l;
|
||||
dLi = Li-l;
|
||||
#+end_src
|
||||
|
||||
** =forwardKinematicsApprox=: Compute the Approximate Forward Kinematics
|
||||
@ -728,7 +773,7 @@ This Matlab function is accessible [[file:src/forwardKinematicsApprox.m][here]].
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - J [6x6] - The Jacobian Matrix
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - args - Can have the following fields:
|
||||
% - dL [6x1] - Displacement of each strut [m]
|
||||
%
|
||||
@ -748,6 +793,15 @@ This Matlab function is accessible [[file:src/forwardKinematicsApprox.m][here]].
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Check the =stewart= structure elements
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J')
|
||||
J = stewart.kinematics.J;
|
||||
#+end_src
|
||||
|
||||
*** Computation
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
@ -756,7 +810,7 @@ From a small displacement of each strut $d\bm{\mathcal{L}}$, we can compute the
|
||||
position and orientation of {B} with respect to {A} using the following formula:
|
||||
\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
|
||||
#+begin_src matlab
|
||||
X = stewart.J\args.dL;
|
||||
X = J\args.dL;
|
||||
#+end_src
|
||||
|
||||
The position vector corresponds to the first 3 elements.
|
||||
@ -777,6 +831,7 @@ We then compute the corresponding rotation matrix.
|
||||
s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
|
||||
s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
|
||||
#+end_src
|
||||
|
||||
* Bibliography :ignore:
|
||||
bibliographystyle:unsrt
|
||||
bibliography:ref.bib
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -5,17 +5,31 @@ function [stewart] = computeJacobian(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - With at least the following fields:
|
||||
% - As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
% - actuators.K [6x1] - Total stiffness of the actuators
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - With the 3 added field:
|
||||
% - J [6x6] - The Jacobian Matrix
|
||||
% - K [6x6] - The Stiffness Matrix
|
||||
% - C [6x6] - The Compliance Matrix
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - kinematics.K [6x6] - The Stiffness Matrix
|
||||
% - kinematics.C [6x6] - The Compliance Matrix
|
||||
|
||||
stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)'];
|
||||
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
||||
As = stewart.geometry.As;
|
||||
|
||||
stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J;
|
||||
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
||||
Ab = stewart.geometry.Ab;
|
||||
|
||||
stewart.C = inv(stewart.K);
|
||||
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
|
||||
Ki = stewart.actuators.K;
|
||||
|
||||
J = [As' , cross(Ab, As)'];
|
||||
|
||||
K = J'*diag(Ki)*J;
|
||||
|
||||
C = inv(K);
|
||||
|
||||
stewart.kinematics.J = J;
|
||||
stewart.kinematics.K = K;
|
||||
stewart.kinematics.C = C;
|
||||
|
||||
@ -5,43 +5,74 @@ function [stewart] = computeJointsPose(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
% - FO_A [3x1] - Position of {A} with respect to {F}
|
||||
% - MO_B [3x1] - Position of {B} with respect to {M}
|
||||
% - FO_M [3x1] - Position of {M} with respect to {F}
|
||||
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
|
||||
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
|
||||
% - geometry.FO_M [3x1] - Position of {M} with respect to {F}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following added fields
|
||||
% - Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
||||
% - Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
||||
% - Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
||||
% - Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
||||
% - l [6x1] - The i'th element is the initial length of strut i
|
||||
% - As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
||||
% - Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
||||
% - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
||||
% - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
||||
% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
||||
% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
||||
% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
||||
% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
||||
% - geometry.l [6x1] - The i'th element is the initial length of strut i
|
||||
% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
||||
% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
||||
% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut
|
||||
% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut
|
||||
% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
||||
% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
||||
|
||||
stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]);
|
||||
stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]);
|
||||
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
|
||||
Fa = stewart.platform_F.Fa;
|
||||
|
||||
stewart.Ab = stewart.Bb - repmat(-stewart.MO_B-stewart.FO_M+stewart.FO_A, [1, 6]);
|
||||
stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]);
|
||||
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
|
||||
Mb = stewart.platform_M.Mb;
|
||||
|
||||
stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As
|
||||
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
|
||||
FO_A = stewart.platform_F.FO_A;
|
||||
|
||||
stewart.l = vecnorm(stewart.Ab - stewart.Aa)';
|
||||
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
|
||||
MO_B = stewart.platform_M.MO_B;
|
||||
|
||||
stewart.Bs = (stewart.Bb - stewart.Ba)./vecnorm(stewart.Bb - stewart.Ba);
|
||||
assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M')
|
||||
FO_M = stewart.geometry.FO_M;
|
||||
|
||||
stewart.FRa = zeros(3,3,6);
|
||||
stewart.MRb = zeros(3,3,6);
|
||||
Aa = Fa - repmat(FO_A, [1, 6]);
|
||||
Bb = Mb - repmat(MO_B, [1, 6]);
|
||||
|
||||
Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
|
||||
Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
|
||||
|
||||
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
|
||||
|
||||
l = vecnorm(Ab - Aa)';
|
||||
|
||||
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
|
||||
|
||||
FRa = zeros(3,3,6);
|
||||
MRb = zeros(3,3,6);
|
||||
|
||||
for i = 1:6
|
||||
stewart.FRa(:,:,i) = [cross([0;1;0], stewart.As(:,i)) , cross(stewart.As(:,i), cross([0;1;0], stewart.As(:,i))) , stewart.As(:,i)];
|
||||
stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,i));
|
||||
FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
|
||||
FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
|
||||
|
||||
stewart.MRb(:,:,i) = [cross([0;1;0], stewart.Bs(:,i)) , cross(stewart.Bs(:,i), cross([0;1;0], stewart.Bs(:,i))) , stewart.Bs(:,i)];
|
||||
stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,i));
|
||||
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
|
||||
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
|
||||
end
|
||||
|
||||
stewart.geometry.Aa = Aa;
|
||||
stewart.geometry.Ab = Ab;
|
||||
stewart.geometry.Ba = Ba;
|
||||
stewart.geometry.Bb = Bb;
|
||||
stewart.geometry.As = As;
|
||||
stewart.geometry.Bs = Bs;
|
||||
stewart.geometry.l = l;
|
||||
|
||||
stewart.struts_F.l = l/2;
|
||||
stewart.struts_M.l = l/2;
|
||||
|
||||
stewart.platform_F.FRa = FRa;
|
||||
stewart.platform_M.MRb = MRb;
|
||||
|
||||
@ -27,28 +27,43 @@ arguments
|
||||
args.F_color = [0 0.4470 0.7410]
|
||||
args.M_color = [0.8500 0.3250 0.0980]
|
||||
args.L_color = [0 0 0]
|
||||
args.frames logical {mustBeNumericOrLogical} = true
|
||||
args.legs logical {mustBeNumericOrLogical} = true
|
||||
args.joints logical {mustBeNumericOrLogical} = true
|
||||
args.labels logical {mustBeNumericOrLogical} = true
|
||||
args.frames logical {mustBeNumericOrLogical} = true
|
||||
args.legs logical {mustBeNumericOrLogical} = true
|
||||
args.joints logical {mustBeNumericOrLogical} = true
|
||||
args.labels logical {mustBeNumericOrLogical} = true
|
||||
args.platforms logical {mustBeNumericOrLogical} = true
|
||||
end
|
||||
|
||||
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
|
||||
FO_A = stewart.platform_F.FO_A;
|
||||
|
||||
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
|
||||
MO_B = stewart.platform_M.MO_B;
|
||||
|
||||
assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H')
|
||||
H = stewart.geometry.H;
|
||||
|
||||
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
|
||||
Fa = stewart.platform_F.Fa;
|
||||
|
||||
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
|
||||
Mb = stewart.platform_M.Mb;
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
|
||||
FTa = [eye(3), stewart.FO_A; ...
|
||||
FTa = [eye(3), FO_A; ...
|
||||
zeros(1,3), 1];
|
||||
ATb = [args.ARB, args.AP; ...
|
||||
zeros(1,3), 1];
|
||||
BTm = [eye(3), -stewart.MO_B; ...
|
||||
BTm = [eye(3), -MO_B; ...
|
||||
zeros(1,3), 1];
|
||||
|
||||
FTm = FTa*ATb*BTm;
|
||||
|
||||
d_unit_vector = stewart.H/4;
|
||||
d_unit_vector = H/4;
|
||||
|
||||
d_label = stewart.H/20;
|
||||
d_label = H/20;
|
||||
|
||||
Ff = [0, 0, 0];
|
||||
if args.frames
|
||||
@ -62,24 +77,22 @@ if args.frames
|
||||
end
|
||||
end
|
||||
|
||||
Fa = stewart.FO_A;
|
||||
|
||||
if args.frames
|
||||
quiver3(Fa(1)*ones(1,3), Fa(2)*ones(1,3), Fa(3)*ones(1,3), ...
|
||||
quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ...
|
||||
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
|
||||
|
||||
if args.labels
|
||||
text(Fa(1) + d_label, ...
|
||||
Fa(2) + d_label, ...
|
||||
Fa(3) + d_label, '$\{A\}$', 'Color', args.F_color);
|
||||
text(FO_A(1) + d_label, ...
|
||||
FO_A(2) + d_label, ...
|
||||
FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color);
|
||||
end
|
||||
end
|
||||
|
||||
if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Fpr')
|
||||
if args.platforms && stewart.platform_F.type == 1
|
||||
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
|
||||
v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
|
||||
center = [0; 0; 0]; % Center of the circle
|
||||
radius = stewart.platforms.Fpr; % Radius of the circle [m]
|
||||
radius = stewart.platform_F.R; % Radius of the circle [m]
|
||||
|
||||
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
|
||||
|
||||
@ -89,14 +102,14 @@ if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms,
|
||||
end
|
||||
|
||||
if args.joints
|
||||
scatter3(stewart.Fa(1,:), ...
|
||||
stewart.Fa(2,:), ...
|
||||
stewart.Fa(3,:), 'MarkerEdgeColor', args.F_color);
|
||||
scatter3(Fa(1,:), ...
|
||||
Fa(2,:), ...
|
||||
Fa(3,:), 'MarkerEdgeColor', args.F_color);
|
||||
if args.labels
|
||||
for i = 1:size(stewart.Fa,2)
|
||||
text(stewart.Fa(1,i) + d_label, ...
|
||||
stewart.Fa(2,i), ...
|
||||
stewart.Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
|
||||
for i = 1:size(Fa,2)
|
||||
text(Fa(1,i) + d_label, ...
|
||||
Fa(2,i), ...
|
||||
Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
|
||||
end
|
||||
end
|
||||
end
|
||||
@ -115,7 +128,7 @@ if args.frames
|
||||
end
|
||||
end
|
||||
|
||||
FB = stewart.FO_A + args.AP;
|
||||
FB = FO_A + args.AP;
|
||||
|
||||
if args.frames
|
||||
FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
|
||||
@ -129,11 +142,11 @@ if args.frames
|
||||
end
|
||||
end
|
||||
|
||||
if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Mpr')
|
||||
if args.platforms && stewart.platform_M.type == 1
|
||||
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
|
||||
v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
|
||||
center = Fm(1:3); % Center of the circle
|
||||
radius = stewart.platforms.Mpr; % Radius of the circle [m]
|
||||
radius = stewart.platform_M.R; % Radius of the circle [m]
|
||||
|
||||
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
|
||||
|
||||
@ -143,7 +156,7 @@ if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms,
|
||||
end
|
||||
|
||||
if args.joints
|
||||
Fb = FTm*[stewart.Mb;ones(1,6)];
|
||||
Fb = FTm*[Mb;ones(1,6)];
|
||||
|
||||
scatter3(Fb(1,:), ...
|
||||
Fb(2,:), ...
|
||||
@ -160,14 +173,14 @@ end
|
||||
|
||||
if args.legs
|
||||
for i = 1:6
|
||||
plot3([stewart.Fa(1,i), Fb(1,i)], ...
|
||||
[stewart.Fa(2,i), Fb(2,i)], ...
|
||||
[stewart.Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
|
||||
plot3([Fa(1,i), Fb(1,i)], ...
|
||||
[Fa(2,i), Fb(2,i)], ...
|
||||
[Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
|
||||
|
||||
if args.labels
|
||||
text((stewart.Fa(1,i)+Fb(1,i))/2 + d_label, ...
|
||||
(stewart.Fa(2,i)+Fb(2,i))/2, ...
|
||||
(stewart.Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
|
||||
text((Fa(1,i)+Fb(1,i))/2 + d_label, ...
|
||||
(Fa(2,i)+Fb(2,i))/2, ...
|
||||
(Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
@ -6,7 +6,7 @@ function [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - J [6x6] - The Jacobian Matrix
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - args - Can have the following fields:
|
||||
% - dL [6x1] - Displacement of each strut [m]
|
||||
%
|
||||
@ -19,7 +19,10 @@ arguments
|
||||
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
||||
end
|
||||
|
||||
X = stewart.J\args.dL;
|
||||
assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J')
|
||||
J = stewart.kinematics.J;
|
||||
|
||||
X = J\args.dL;
|
||||
|
||||
P = X(1:3);
|
||||
|
||||
|
||||
@ -5,7 +5,7 @@ function [stewart] = generateCubicConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - H [1x1] - Total height of the platform [m]
|
||||
% - geometry.H [1x1] - Total height of the platform [m]
|
||||
% - args - Can have the following fields:
|
||||
% - Hc [1x1] - Height of the "useful" part of the cube [m]
|
||||
% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
|
||||
@ -14,8 +14,8 @@ function [stewart] = generateCubicConfiguration(stewart, args)
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
@ -25,6 +25,9 @@ arguments
|
||||
args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
|
||||
end
|
||||
|
||||
assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H')
|
||||
H = stewart.geometry.H;
|
||||
|
||||
sx = [ 2; -1; -1];
|
||||
sy = [ 0; 1; -1];
|
||||
sz = [ 1; 1; 1];
|
||||
@ -40,5 +43,8 @@ CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresp
|
||||
|
||||
CSi = (CCm - CCf)./vecnorm(CCm - CCf);
|
||||
|
||||
stewart.Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||||
stewart.Mb = CCf + [0; 0; args.FOc-stewart.H] + ((stewart.H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||||
Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||||
Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||||
|
||||
stewart.platform_F.Fa = Fa;
|
||||
stewart.platform_M.Mb = Mb;
|
||||
|
||||
@ -14,8 +14,8 @@ function [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
@ -27,10 +27,13 @@ arguments
|
||||
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
||||
end
|
||||
|
||||
stewart.Fa = zeros(3,6);
|
||||
stewart.Mb = zeros(3,6);
|
||||
Fa = zeros(3,6);
|
||||
Mb = zeros(3,6);
|
||||
|
||||
for i = 1:6
|
||||
stewart.Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
||||
stewart.Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
||||
Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
||||
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
||||
end
|
||||
|
||||
stewart.platform_F.Fa = Fa;
|
||||
stewart.platform_M.Mb = Mb;
|
||||
|
||||
@ -5,35 +5,39 @@ function [stewart] = initializeAmplifiedStrutDynamics(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Ksi [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m]
|
||||
% - Csi [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)]
|
||||
% - Kdi [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m]
|
||||
% - Cdi [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)]
|
||||
% - Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m]
|
||||
% - Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)]
|
||||
% - Kr [6x1] - Vertical (residual) stiffness when the piezoelectric stack is removed [N/m]
|
||||
% - Cr [6x1] - Vertical (residual) damping when the piezoelectric stack is removed [N/(m/s)]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Ki [6x1] - Total Stiffness of each strut [N/m]
|
||||
% - Ci [6x1] - Total Damping of each strut [N/(m/s)]
|
||||
% - Ksi [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m]
|
||||
% - Csi [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)]
|
||||
% - Kdi [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m]
|
||||
% - Cdi [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)]
|
||||
% - actuators.type = 2
|
||||
% - actuators.K [6x1] - Total Stiffness of each strut [N/m]
|
||||
% - actuators.C [6x1] - Total Damping of each strut [N/(m/s)]
|
||||
% - actuators.Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m]
|
||||
% - actuators.Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)]
|
||||
% - actuators.Kr [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m]
|
||||
% - actuators.Cr [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.Kdi (6,1) double {mustBeNumeric, mustBeNonnegative} = 5e6*ones(6,1)
|
||||
args.Cdi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.Ksi (6,1) double {mustBeNumeric, mustBeNonnegative} = 15e6*ones(6,1)
|
||||
args.Csi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.Kr (6,1) double {mustBeNumeric, mustBeNonnegative} = 5e6*ones(6,1)
|
||||
args.Cr (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.Ka (6,1) double {mustBeNumeric, mustBeNonnegative} = 15e6*ones(6,1)
|
||||
args.Ca (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
end
|
||||
|
||||
stewart.Ksi = args.Ksi;
|
||||
stewart.Csi = args.Csi;
|
||||
K = args.Ka + args.Kr;
|
||||
C = args.Ca + args.Cr;
|
||||
|
||||
stewart.Kdi = args.Kdi;
|
||||
stewart.Cdi = args.Cdi;
|
||||
stewart.actuators.type = 2;
|
||||
|
||||
stewart.Ki = args.Ksi + args.Kdi;
|
||||
stewart.Ci = args.Csi + args.Cdi;
|
||||
stewart.actuators.Ka = args.Ka;
|
||||
stewart.actuators.Ca = args.Ca;
|
||||
|
||||
stewart.actuator_type = 2;
|
||||
stewart.actuators.Kr = args.Kr;
|
||||
stewart.actuators.Cr = args.Cr;
|
||||
|
||||
stewart.actuators.K = K;
|
||||
stewart.actuators.C = K;
|
||||
|
||||
@ -14,15 +14,17 @@ function [stewart] = initializeCylindricalPlatforms(stewart, args)
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - platforms [struct] - structure with the following fields:
|
||||
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
||||
% - Msi [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
||||
% - Fph [1x1] - Fixed Platform Height [m]
|
||||
% - Fpr [1x1] - Fixed Platform Radius [m]
|
||||
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
||||
% - Fsi [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
||||
% - Mph [1x1] - Mobile Platform Height [m]
|
||||
% - Mpr [1x1] - Mobile Platform Radius [m]
|
||||
% - platform_F [struct] - structure with the following fields:
|
||||
% - type = 1
|
||||
% - M [1x1] - Fixed Platform Mass [kg]
|
||||
% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
||||
% - H [1x1] - Fixed Platform Height [m]
|
||||
% - R [1x1] - Fixed Platform Radius [m]
|
||||
% - platform_M [struct] - structure with the following fields:
|
||||
% - M [1x1] - Mobile Platform Mass [kg]
|
||||
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
||||
% - H [1x1] - Mobile Platform Height [m]
|
||||
% - R [1x1] - Mobile Platform Radius [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
@ -34,20 +36,24 @@ arguments
|
||||
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||||
end
|
||||
|
||||
platforms = struct();
|
||||
I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
||||
1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
||||
1/2 *args.Fpm * args.Fpr^2]);
|
||||
|
||||
platforms.Fpm = args.Fpm;
|
||||
platforms.Fph = args.Fph;
|
||||
platforms.Fpr = args.Fpr;
|
||||
platforms.Fpi = diag([1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ...
|
||||
1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ...
|
||||
1/2 * platforms.Fpm * platforms.Fpr^2]);
|
||||
I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
||||
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
||||
1/2 *args.Mpm * args.Mpr^2]);
|
||||
|
||||
platforms.Mpm = args.Mpm;
|
||||
platforms.Mph = args.Mph;
|
||||
platforms.Mpr = args.Mpr;
|
||||
platforms.Mpi = diag([1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ...
|
||||
1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ...
|
||||
1/2 * platforms.Mpm * platforms.Mpr^2]);
|
||||
stewart.platform_F.type = 1;
|
||||
|
||||
stewart.platforms = platforms;
|
||||
stewart.platform_F.I = I_F;
|
||||
stewart.platform_F.M = args.Fpm;
|
||||
stewart.platform_F.R = args.Fpr;
|
||||
stewart.platform_F.H = args.Fph;
|
||||
|
||||
stewart.platform_M.type = 1;
|
||||
|
||||
stewart.platform_M.I = I_M;
|
||||
stewart.platform_M.M = args.Mpm;
|
||||
stewart.platform_M.R = args.Mpr;
|
||||
stewart.platform_M.H = args.Mph;
|
||||
|
||||
@ -14,15 +14,16 @@ function [stewart] = initializeCylindricalStruts(stewart, args)
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - struts [struct] - structure with the following fields:
|
||||
% - Fsm [6x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - Fsi [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
|
||||
% - Msm [6x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - Msi [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
||||
% - Fsh [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - Fsr [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - Msh [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - Msr [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
% - struts_F [struct] - structure with the following fields:
|
||||
% - M [6x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
|
||||
% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - struts_M [struct] - structure with the following fields:
|
||||
% - M [6x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
||||
% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
@ -34,25 +35,37 @@ arguments
|
||||
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
end
|
||||
|
||||
struts = struct();
|
||||
Fsm = ones(6,1).*args.Fsm;
|
||||
Fsh = ones(6,1).*args.Fsh;
|
||||
Fsr = ones(6,1).*args.Fsr;
|
||||
|
||||
struts.Fsm = ones(6,1).*args.Fsm;
|
||||
struts.Msm = ones(6,1).*args.Msm;
|
||||
Msm = ones(6,1).*args.Msm;
|
||||
Msh = ones(6,1).*args.Msh;
|
||||
Msr = ones(6,1).*args.Msr;
|
||||
|
||||
struts.Fsh = ones(6,1).*args.Fsh;
|
||||
struts.Fsr = ones(6,1).*args.Fsr;
|
||||
struts.Msh = ones(6,1).*args.Msh;
|
||||
struts.Msr = ones(6,1).*args.Msr;
|
||||
I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
|
||||
I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
|
||||
|
||||
struts.Fsi = zeros(3, 3, 6);
|
||||
struts.Msi = zeros(3, 3, 6);
|
||||
for i = 1:6
|
||||
struts.Fsi(:,:,i) = diag([1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
|
||||
1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
|
||||
1/2 * struts.Fsm(i) * struts.Fsr(i)^2]);
|
||||
struts.Msi(:,:,i) = diag([1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ...
|
||||
1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ...
|
||||
1/2 * struts.Msm(i) * struts.Msr(i)^2]);
|
||||
I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
||||
1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
||||
1/2 * Fsm(i) * Fsr(i)^2]);
|
||||
|
||||
I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
||||
1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
||||
1/2 * Msm(i) * Msr(i)^2]);
|
||||
end
|
||||
|
||||
stewart.struts = struts;
|
||||
stewart.struts_M.type = 1;
|
||||
|
||||
stewart.struts_M.I = I_M;
|
||||
stewart.struts_M.M = Msm;
|
||||
stewart.struts_M.R = Msr;
|
||||
stewart.struts_M.H = Msh;
|
||||
|
||||
stewart.struts_F.type = 1;
|
||||
|
||||
stewart.struts_F.I = I_F;
|
||||
stewart.struts_F.M = Fsm;
|
||||
stewart.struts_F.R = Fsr;
|
||||
stewart.struts_F.H = Fsh;
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
function [stewart] = initializeFramesPositions(args)
|
||||
function [stewart] = initializeFramesPositions(stewart, args)
|
||||
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
||||
%
|
||||
% Syntax: [stewart] = initializeFramesPositions(args)
|
||||
@ -10,22 +10,26 @@ function [stewart] = initializeFramesPositions(args)
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following fields:
|
||||
% - H [1x1] - Total Height of the Stewart Platform [m]
|
||||
% - FO_M [3x1] - Position of {M} with respect to {F} [m]
|
||||
% - MO_B [3x1] - Position of {B} with respect to {M} [m]
|
||||
% - FO_A [3x1] - Position of {A} with respect to {F} [m]
|
||||
% - geometry.H [1x1] - Total Height of the Stewart Platform [m]
|
||||
% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]
|
||||
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
|
||||
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
||||
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
||||
end
|
||||
|
||||
stewart = struct();
|
||||
H = args.H; % Total Height of the Stewart Platform [m]
|
||||
|
||||
stewart.H = args.H; % Total Height of the Stewart Platform [m]
|
||||
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
|
||||
|
||||
stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m]
|
||||
MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
||||
|
||||
stewart.MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
||||
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
|
||||
|
||||
stewart.FO_A = stewart.MO_B + stewart.FO_M; % Position of {A} with respect to {F} [m]
|
||||
stewart.geometry.H = H;
|
||||
stewart.geometry.FO_M = FO_M;
|
||||
stewart.platform_M.MO_B = MO_B;
|
||||
stewart.platform_F.FO_A = FO_A;
|
||||
|
||||
48
src/initializeInertialSensor.m
Normal file
48
src/initializeInertialSensor.m
Normal file
@ -0,0 +1,48 @@
|
||||
function [stewart] = initializeInertialSensor(stewart, args)
|
||||
% initializeInertialSensor - Initialize the inertial sensor in each strut
|
||||
%
|
||||
% Syntax: [stewart] = initializeInertialSensor(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - type - 'geophone', 'accelerometer', 'none'
|
||||
% - mass [1x1] - Weight of the inertial mass [kg]
|
||||
% - freq [1x1] - Cutoff frequency [Hz]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - stewart.sensors.inertial
|
||||
% - type - 1 (geophone), 2 (accelerometer), 3 (none)
|
||||
% - K [1x1] - Stiffness [N/m]
|
||||
% - C [1x1] - Damping [N/(m/s)]
|
||||
% - M [1x1] - Inertial Mass [kg]
|
||||
% - G [1x1] - Gain
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.type char {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
|
||||
args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
|
||||
args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
|
||||
end
|
||||
|
||||
sensor = struct();
|
||||
|
||||
switch args.type
|
||||
case 'geophone'
|
||||
sensor.type = 1;
|
||||
|
||||
sensor.M = args.mass;
|
||||
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
||||
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
||||
case 'accelerometer'
|
||||
sensor.type = 2;
|
||||
|
||||
sensor.M = args.mass;
|
||||
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
||||
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
||||
sensor.G = -sensor.K/sensor.M;
|
||||
case 'none'
|
||||
sensor.type = 3;
|
||||
end
|
||||
|
||||
stewart.sensors.inertial = sensor;
|
||||
@ -5,46 +5,88 @@ function [stewart] = initializeJointDynamics(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Ksbi [6x1] - Bending (Rx, Ry) Stiffness for each top Spherical joints [(N.m)/rad]
|
||||
% - Csbi [6x1] - Bending (Rx, Ry) Damping of each top Spherical joint [(N.m)/(rad/s)]
|
||||
% - Ksti [6x1] - Torsion (Rz) Stiffness for each top Spherical joints [(N.m)/rad]
|
||||
% - Csti [6x1] - Torsion (Rz) Damping of each top Spherical joint [(N.m)/(rad/s)]
|
||||
% - Kubi [6x1] - Bending (Rx, Ry) Stiffness for each bottom Universal joints [(N.m)/rad]
|
||||
% - Cubi [6x1] - Bending (Rx, Ry) Damping of each bottom Universal joint [(N.m)/(rad/s)]
|
||||
% - disable [boolean] - Sets all the stiffness/damping to zero
|
||||
% - type_F - 'universal', 'spherical', 'univesal_p', 'spherical_p'
|
||||
% - type_M - 'universal', 'spherical', 'univesal_p', 'spherical_p'
|
||||
% - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
|
||||
% - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
|
||||
% - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
|
||||
% - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
|
||||
% - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
|
||||
% - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
|
||||
% - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
|
||||
% - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Ksbi [6x1] - Bending (Rx, Ry) Stiffness for each top Spherical joints [(N.m)/rad]
|
||||
% - Csbi [6x1] - Bending (Rx, Ry) Damping of each top Spherical joint [(N.m)/(rad/s)]
|
||||
% - Ksti [6x1] - Torsion (Rz) Stiffness for each top Spherical joints [(N.m)/rad]
|
||||
% - Csti [6x1] - Torsion (Rz) Damping of each top Spherical joint [(N.m)/(rad/s)]
|
||||
% - Kubi [6x1] - Bending (Rx, Ry) Stiffness for each bottom Universal joints [(N.m)/rad]
|
||||
% - Cubi [6x1] - Bending (Rx, Ry) Damping of each bottom Universal joint [(N.m)/(rad/s)]
|
||||
% - stewart.joints_F and stewart.joints_M:
|
||||
% - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
|
||||
% - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
|
||||
% - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
|
||||
% - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
|
||||
% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
|
||||
% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
|
||||
% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.Ksbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1)
|
||||
args.Csbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
||||
args.Ksti (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1)
|
||||
args.Csti (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
||||
args.Kubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1)
|
||||
args.Cubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
||||
args.disable logical {mustBeNumericOrLogical} = false
|
||||
args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'univesal_p', 'spherical_p'})} = 'universal'
|
||||
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'univesal_p', 'spherical_p'})} = 'spherical'
|
||||
args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1)
|
||||
args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
||||
args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1)
|
||||
args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
||||
args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1)
|
||||
args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
||||
args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1)
|
||||
args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
||||
end
|
||||
|
||||
if args.disable
|
||||
stewart.Ksbi = zeros(6,1);
|
||||
stewart.Csbi = zeros(6,1);
|
||||
stewart.Ksti = zeros(6,1);
|
||||
stewart.Csti = zeros(6,1);
|
||||
stewart.Kubi = zeros(6,1);
|
||||
stewart.Cubi = zeros(6,1);
|
||||
else
|
||||
stewart.Ksbi = args.Ksbi;
|
||||
stewart.Csbi = args.Csbi;
|
||||
stewart.Ksti = args.Ksti;
|
||||
stewart.Csti = args.Csti;
|
||||
stewart.Kubi = args.Kubi;
|
||||
stewart.Cubi = args.Cubi;
|
||||
switch args.type_F
|
||||
case 'universal'
|
||||
stewart.joints_F.type = 1;
|
||||
case 'spherical'
|
||||
stewart.joints_F.type = 2;
|
||||
case 'universal_p'
|
||||
stewart.joints_F.type = 3;
|
||||
case 'spherical_p'
|
||||
stewart.joints_F.type = 4;
|
||||
end
|
||||
|
||||
switch args.type_M
|
||||
case 'universal'
|
||||
stewart.joints_M.type = 1;
|
||||
case 'spherical'
|
||||
stewart.joints_M.type = 2;
|
||||
case 'universal_p'
|
||||
stewart.joints_M.type = 3;
|
||||
case 'spherical_p'
|
||||
stewart.joints_M.type = 4;
|
||||
end
|
||||
|
||||
stewart.joints_M.Kx = zeros(6,1);
|
||||
stewart.joints_M.Ky = zeros(6,1);
|
||||
stewart.joints_M.Kz = zeros(6,1);
|
||||
|
||||
stewart.joints_F.Kx = zeros(6,1);
|
||||
stewart.joints_F.Ky = zeros(6,1);
|
||||
stewart.joints_F.Kz = zeros(6,1);
|
||||
|
||||
stewart.joints_M.Cx = zeros(6,1);
|
||||
stewart.joints_M.Cy = zeros(6,1);
|
||||
stewart.joints_M.Cz = zeros(6,1);
|
||||
|
||||
stewart.joints_F.Cx = zeros(6,1);
|
||||
stewart.joints_F.Cy = zeros(6,1);
|
||||
stewart.joints_F.Cz = zeros(6,1);
|
||||
|
||||
stewart.joints_M.Kf = args.Kf_M;
|
||||
stewart.joints_M.Kt = args.Kf_M;
|
||||
|
||||
stewart.joints_F.Kf = args.Kf_F;
|
||||
stewart.joints_F.Kt = args.Kf_F;
|
||||
|
||||
stewart.joints_M.Cf = args.Cf_M;
|
||||
stewart.joints_M.Ct = args.Cf_M;
|
||||
|
||||
stewart.joints_F.Cf = args.Cf_F;
|
||||
stewart.joints_F.Ct = args.Cf_F;
|
||||
|
||||
31
src/initializeStewartPlatform.m
Normal file
31
src/initializeStewartPlatform.m
Normal file
@ -0,0 +1,31 @@
|
||||
function [stewart] = initializeStewartPlatform()
|
||||
% initializeStewartPlatform - Initialize the stewart structure
|
||||
%
|
||||
% Syntax: [stewart] = initializeStewartPlatform(args)
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following sub-structures:
|
||||
% - platform_F -
|
||||
% - platform_M -
|
||||
% - joints_F -
|
||||
% - joints_M -
|
||||
% - struts_F -
|
||||
% - struts_M -
|
||||
% - actuators -
|
||||
% - geometry -
|
||||
% - properties -
|
||||
|
||||
stewart = struct();
|
||||
stewart.platform_F = struct();
|
||||
stewart.platform_M = struct();
|
||||
stewart.joints_F = struct();
|
||||
stewart.joints_M = struct();
|
||||
stewart.struts_F = struct();
|
||||
stewart.struts_M = struct();
|
||||
stewart.actuators = struct();
|
||||
stewart.sensors = struct();
|
||||
stewart.sensors.inertial = struct();
|
||||
stewart.sensors.force = struct();
|
||||
stewart.sensors.relative = struct();
|
||||
stewart.geometry = struct();
|
||||
stewart.kinematics = struct();
|
||||
@ -14,7 +14,7 @@ function [stewart] = initializeStewartPose(stewart, args)
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - dLi[6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
% - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
@ -24,4 +24,4 @@ end
|
||||
|
||||
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
||||
|
||||
stewart.dLi = dLi;
|
||||
stewart.actuators.Leq = dLi;
|
||||
|
||||
@ -5,21 +5,22 @@ function [stewart] = initializeStrutDynamics(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Ki [6x1] - Stiffness of each strut [N/m]
|
||||
% - Ci [6x1] - Damping of each strut [N/(m/s)]
|
||||
% - K [6x1] - Stiffness of each strut [N/m]
|
||||
% - C [6x1] - Damping of each strut [N/(m/s)]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Ki [6x1] - Stiffness of each strut [N/m]
|
||||
% - Ci [6x1] - Damping of each strut [N/(m/s)]
|
||||
% - actuators.type = 1
|
||||
% - actuators.K [6x1] - Stiffness of each strut [N/m]
|
||||
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e6*ones(6,1)
|
||||
args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e6*ones(6,1)
|
||||
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
end
|
||||
|
||||
stewart.Ki = args.Ki;
|
||||
stewart.Ci = args.Ci;
|
||||
stewart.actuators.type = 1;
|
||||
|
||||
stewart.actuator_type = 1;
|
||||
stewart.actuators.K = args.K;
|
||||
stewart.actuators.C = args.C;
|
||||
|
||||
@ -5,8 +5,9 @@ function [Li, dLi] = inverseKinematics(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - Aa [3x6] - The positions ai expressed in {A}
|
||||
% - Bb [3x6] - The positions bi expressed in {B}
|
||||
% - geometry.Aa [3x6] - The positions ai expressed in {A}
|
||||
% - geometry.Bb [3x6] - The positions bi expressed in {B}
|
||||
% - geometry.l [6x1] - Length of each strut
|
||||
% - args - Can have the following fields:
|
||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
@ -21,6 +22,15 @@ arguments
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
|
||||
Li = sqrt(args.AP'*args.AP + diag(stewart.Bb'*stewart.Bb) + diag(stewart.Aa'*stewart.Aa) - (2*args.AP'*stewart.Aa)' + (2*args.AP'*(args.ARB*stewart.Bb))' - diag(2*(args.ARB*stewart.Bb)'*stewart.Aa));
|
||||
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
|
||||
Aa = stewart.geometry.Aa;
|
||||
|
||||
dLi = Li-stewart.l;
|
||||
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
|
||||
Bb = stewart.geometry.Bb;
|
||||
|
||||
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
|
||||
l = stewart.geometry.l;
|
||||
|
||||
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
|
||||
|
||||
dLi = Li-l;
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue
Block a user