2518 lines
124 KiB
HTML
2518 lines
124 KiB
HTML
<?xml version="1.0" encoding="utf-8"?>
|
|
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
|
|
"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-11-03 mar. 09:45 -->
|
|
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
|
<title>Stewart Platform - Simscape Model</title>
|
|
<meta name="generator" content="Org mode" />
|
|
<meta name="author" content="Dehaeze Thomas" />
|
|
<link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
|
<link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
|
<script src="./js/jquery.min.js"></script>
|
|
<script src="./js/bootstrap.min.js"></script>
|
|
<script src="./js/jquery.stickytableheaders.min.js"></script>
|
|
<script src="./js/readtheorg.js"></script>
|
|
<script>MathJax = {
|
|
tex: {
|
|
tags: 'ams',
|
|
macros: {bm: ["\\boldsymbol{#1}",1],}
|
|
}
|
|
};
|
|
</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">
|
|
<a accesskey="h" href="./index.html"> UP </a>
|
|
|
|
|
<a accesskey="H" href="./index.html"> HOME </a>
|
|
</div><div id="content">
|
|
<h1 class="title">Stewart Platform - Simscape Model</h1>
|
|
<div id="table-of-contents">
|
|
<h2>Table of Contents</h2>
|
|
<div id="text-table-of-contents">
|
|
<ul>
|
|
<li><a href="#orgcaca5e0">1. <code>initializeStewartPlatform</code>: Initialize the Stewart Platform structure</a>
|
|
<ul>
|
|
<li><a href="#org48f8ee2">Documentation</a></li>
|
|
<li><a href="#org96f92cb">Function description</a></li>
|
|
<li><a href="#org3622825">Initialize the Stewart structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgac25f89">2. <code>initializeFramesPositions</code>: Initialize the positions of frames {A}, {B}, {F} and {M}</a>
|
|
<ul>
|
|
<li><a href="#org8ff1cb0">Documentation</a></li>
|
|
<li><a href="#orgc23d352">Function description</a></li>
|
|
<li><a href="#org8ccdfcd">Optional Parameters</a></li>
|
|
<li><a href="#org7d50d54">Compute the position of each frame</a></li>
|
|
<li><a href="#orgc902066">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgccb31c6">3. <code>generateGeneralConfiguration</code>: Generate a Very General Configuration</a>
|
|
<ul>
|
|
<li><a href="#org50b57cc">Documentation</a></li>
|
|
<li><a href="#org5edcb6f">Function description</a></li>
|
|
<li><a href="#orgc271059">Optional Parameters</a></li>
|
|
<li><a href="#orgefcf050">Compute the pose</a></li>
|
|
<li><a href="#org0b8bbf0">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org9944c04">4. <code>computeJointsPose</code>: Compute the Pose of the Joints</a>
|
|
<ul>
|
|
<li><a href="#org88c006f">Documentation</a></li>
|
|
<li><a href="#org3a97461">Function description</a></li>
|
|
<li><a href="#org8e43add">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#orge87b302">Compute the position of the Joints</a></li>
|
|
<li><a href="#org3a7e3c5">Compute the strut length and orientation</a></li>
|
|
<li><a href="#org9e1258f">Compute the orientation of the Joints</a></li>
|
|
<li><a href="#orgdee1da8">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org1315282">5. <code>initializeStewartPose</code>: Determine the initial stroke in each leg to have the wanted pose</a>
|
|
<ul>
|
|
<li><a href="#org7f4912f">Function description</a></li>
|
|
<li><a href="#orga884eb1">Optional Parameters</a></li>
|
|
<li><a href="#orgbb9abb5">Use the Inverse Kinematic function</a></li>
|
|
<li><a href="#org9f3b0a3">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org4674203">6. <code>initializeCylindricalPlatforms</code>: Initialize the geometry of the Fixed and Mobile Platforms</a>
|
|
<ul>
|
|
<li><a href="#orgf7385de">Function description</a></li>
|
|
<li><a href="#org737b9ce">Optional Parameters</a></li>
|
|
<li><a href="#orgf654de0">Compute the Inertia matrices of platforms</a></li>
|
|
<li><a href="#orgd7d42c3">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgb0a1d7b">7. <code>initializeCylindricalStruts</code>: Define the inertia of cylindrical struts</a>
|
|
<ul>
|
|
<li><a href="#org9e6d34f">Function description</a></li>
|
|
<li><a href="#orgd0165df">Optional Parameters</a></li>
|
|
<li><a href="#orgd943059">Compute the properties of the cylindrical struts</a></li>
|
|
<li><a href="#org3ff10a3">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgae8d0dc">8. <code>initializeStrutDynamics</code>: Add Stiffness and Damping properties of each strut</a>
|
|
<ul>
|
|
<li><a href="#org0eac2ce">Documentation</a></li>
|
|
<li><a href="#org8c4942a">Function description</a></li>
|
|
<li><a href="#org0436866">Optional Parameters</a></li>
|
|
<li><a href="#org3c2e550">Add Stiffness and Damping properties of each strut</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgbc5232e">9. <code>initializeJointDynamics</code>: Add Stiffness and Damping properties for spherical joints</a>
|
|
<ul>
|
|
<li><a href="#org2568d4c">Function description</a></li>
|
|
<li><a href="#orgbf466fe">Optional Parameters</a></li>
|
|
<li><a href="#orgd5b8278">Add Actuator Type</a></li>
|
|
<li><a href="#org51cf135">Add Stiffness and Damping in Translation of each strut</a></li>
|
|
<li><a href="#org1e8eceb">Add Stiffness and Damping in Rotation of each strut</a></li>
|
|
<li><a href="#org74a3fc5">Stiffness and Mass matrices for flexible joint</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org3a7f26e">10. <code>initializeInertialSensor</code>: Initialize the inertial sensor in each strut</a>
|
|
<ul>
|
|
<li><a href="#orgcfc37af">Geophone - Working Principle</a></li>
|
|
<li><a href="#org986e38f">Accelerometer - Working Principle</a></li>
|
|
<li><a href="#org8eae4fc">Function description</a></li>
|
|
<li><a href="#org14e8700">Optional Parameters</a></li>
|
|
<li><a href="#org1c3d7c8">Compute the properties of the sensor</a></li>
|
|
<li><a href="#org50cda50">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgd6baa46">11. <code>displayArchitecture</code>: 3D plot of the Stewart platform architecture</a>
|
|
<ul>
|
|
<li><a href="#orgf427022">Function description</a></li>
|
|
<li><a href="#orgaa63f3d">Optional Parameters</a></li>
|
|
<li><a href="#orgb289e7f">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#orgb11fd92">Figure Creation, Frames and Homogeneous transformations</a></li>
|
|
<li><a href="#org7cd8fee">Fixed Base elements</a></li>
|
|
<li><a href="#orgacb8eb7">Mobile Platform elements</a></li>
|
|
<li><a href="#org7f9320b">Legs</a></li>
|
|
<li><a href="#org925a393">11.1. Figure parameters</a></li>
|
|
<li><a href="#org44e536d">11.2. Subplots</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgecfd55f">12. <code>describeStewartPlatform</code>: Display some text describing the current defined Stewart Platform</a>
|
|
<ul>
|
|
<li><a href="#orgf8354f3">Function description</a></li>
|
|
<li><a href="#org80e76f7">Optional Parameters</a></li>
|
|
<li><a href="#org1d49caa">12.1. Geometry</a></li>
|
|
<li><a href="#orgcb66771">12.2. Actuators</a></li>
|
|
<li><a href="#org4630b77">12.3. Joints</a></li>
|
|
<li><a href="#org47a9cf0">12.4. Kinematics</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org65fc289">13. <code>generateCubicConfiguration</code>: Generate a Cubic Configuration</a>
|
|
<ul>
|
|
<li><a href="#org3b2822c">Function description</a></li>
|
|
<li><a href="#orga88ecd8">Documentation</a></li>
|
|
<li><a href="#org0bd99c9">Optional Parameters</a></li>
|
|
<li><a href="#org4cb31f0">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#orge94a885">Position of the Cube</a></li>
|
|
<li><a href="#orgffbfeac">Compute the pose</a></li>
|
|
<li><a href="#org99658a2">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org9e8cbfa">14. <code>computeJacobian</code>: Compute the Jacobian Matrix</a>
|
|
<ul>
|
|
<li><a href="#org9bd1578">Function description</a></li>
|
|
<li><a href="#org1e70cf8">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#org9bcd9b9">Compute Jacobian Matrix</a></li>
|
|
<li><a href="#orgf08eda6">Compute Stiffness Matrix</a></li>
|
|
<li><a href="#orgd164132">Compute Compliance Matrix</a></li>
|
|
<li><a href="#orgaf3b338">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org03168fc">15. <code>inverseKinematics</code>: Compute Inverse Kinematics</a>
|
|
<ul>
|
|
<li><a href="#orgbdc5fb1">Theory</a></li>
|
|
<li><a href="#org17070b1">Function description</a></li>
|
|
<li><a href="#orgaf1a90a">Optional Parameters</a></li>
|
|
<li><a href="#orgeb2a0d2">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#org8b70a76">Compute</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org278d55b">16. <code>forwardKinematicsApprox</code>: Compute the Approximate Forward Kinematics</a>
|
|
<ul>
|
|
<li><a href="#org8623f0c">Function description</a></li>
|
|
<li><a href="#orgb133a15">Optional Parameters</a></li>
|
|
<li><a href="#orgefe7763">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#orgf17cab9">Computation</a></li>
|
|
</ul>
|
|
</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<p>
|
|
Stewart platforms are generated in multiple steps.
|
|
</p>
|
|
|
|
<p>
|
|
We define 4 important <b>frames</b>:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(\{F\}\): Frame fixed to the <b>Fixed</b> base and located at the center of its bottom surface.
|
|
This is used to fix the Stewart platform to some support.</li>
|
|
<li>\(\{M\}\): Frame fixed to the <b>Moving</b> platform and located at the center of its top surface.
|
|
This is used to place things on top of the Stewart platform.</li>
|
|
<li>\(\{A\}\): Frame fixed to the fixed base.
|
|
It defined the center of rotation of the moving platform.</li>
|
|
<li>\(\{B\}\): Frame fixed to the moving platform.
|
|
The motion of the moving platforms and forces applied to it are defined with respect to this frame \(\{B\}\).</li>
|
|
</ul>
|
|
|
|
<p>
|
|
Then, we define the <b>location of the spherical joints</b>:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(\bm{a}_{i}\) are the position of the spherical joints fixed to the fixed base</li>
|
|
<li>\(\bm{b}_{i}\) are the position of the spherical joints fixed to the moving platform</li>
|
|
</ul>
|
|
|
|
<p>
|
|
We define the <b>rest position</b> of the Stewart platform:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>For simplicity, we suppose that the fixed base and the moving platform are parallel and aligned with the vertical axis at their rest position.</li>
|
|
<li>Thus, to define the rest position of the Stewart platform, we just have to defined its total height \(H\).
|
|
\(H\) corresponds to the distance from the bottom of the fixed base to the top of the moving platform.</li>
|
|
</ul>
|
|
|
|
<p>
|
|
From \(\bm{a}_{i}\) and \(\bm{b}_{i}\), we can determine the <b>length and orientation of each strut</b>:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(l_{i}\) is the length of the strut</li>
|
|
<li>\({}^{A}\hat{\bm{s}}_{i}\) is the unit vector align with the strut</li>
|
|
</ul>
|
|
|
|
<p>
|
|
The position of the Spherical joints can be computed using various methods:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>Cubic configuration</li>
|
|
<li>Circular configuration</li>
|
|
<li>Arbitrary position</li>
|
|
<li>These methods should be easily scriptable and corresponds to specific functions that returns \({}^{F}\bm{a}_{i}\) and \({}^{M}\bm{b}_{i}\).
|
|
The input of these functions are the parameters corresponding to the wanted geometry.</li>
|
|
</ul>
|
|
|
|
<p>
|
|
For Simscape, we need:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>The position and orientation of each spherical joint fixed to the fixed base: \({}^{F}\bm{a}_{i}\) and \({}^{F}\bm{R}_{a_{i}}\)</li>
|
|
<li>The position and orientation of each spherical joint fixed to the moving platform: \({}^{M}\bm{b}_{i}\) and \({}^{M}\bm{R}_{b_{i}}\)</li>
|
|
<li>The rest length of each strut: \(l_{i}\)</li>
|
|
<li>The stiffness and damping of each actuator: \(k_{i}\) and \(c_{i}\)</li>
|
|
<li>The position of the frame \(\{A\}\) with respect to the frame \(\{F\}\): \({}^{F}\bm{O}_{A}\)</li>
|
|
<li>The position of the frame \(\{B\}\) with respect to the frame \(\{M\}\): \({}^{M}\bm{O}_{B}\)</li>
|
|
</ul>
|
|
|
|
<div id="outline-container-orgcaca5e0" class="outline-2">
|
|
<h2 id="orgcaca5e0"><span class="section-number-2">1</span> <code>initializeStewartPlatform</code>: Initialize the Stewart Platform structure</h2>
|
|
<div class="outline-text-2" id="text-1">
|
|
<p>
|
|
<a id="orgcac119a"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeStewartPlatform.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org48f8ee2" class="outline-3">
|
|
<h3 id="org48f8ee2">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org48f8ee2">
|
|
|
|
<div id="orgb77a164" class="figure">
|
|
<p><img src="figs/stewart-frames-position.png" alt="stewart-frames-position.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 1: </span>Definition of the position of the frames</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org96f92cb" class="outline-3">
|
|
<h3 id="org96f92cb">Function description</h3>
|
|
<div class="outline-text-3" id="text-org96f92cb">
|
|
<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">initializeStewartPlatform</span>()
|
|
<span class="org-comment">% initializeStewartPlatform - Initialize the stewart structure</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [stewart] = initializeStewartPlatform(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - stewart - A structure with the following sub-structures:</span>
|
|
<span class="org-comment">% - platform_F -</span>
|
|
<span class="org-comment">% - platform_M -</span>
|
|
<span class="org-comment">% - joints_F -</span>
|
|
<span class="org-comment">% - joints_M -</span>
|
|
<span class="org-comment">% - struts_F -</span>
|
|
<span class="org-comment">% - struts_M -</span>
|
|
<span class="org-comment">% - actuators -</span>
|
|
<span class="org-comment">% - geometry -</span>
|
|
<span class="org-comment">% - properties -</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3622825" class="outline-3">
|
|
<h3 id="org3622825">Initialize the Stewart structure</h3>
|
|
<div class="outline-text-3" id="text-org3622825">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">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();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgac25f89" class="outline-2">
|
|
<h2 id="orgac25f89"><span class="section-number-2">2</span> <code>initializeFramesPositions</code>: Initialize the positions of frames {A}, {B}, {F} and {M}</h2>
|
|
<div class="outline-text-2" id="text-2">
|
|
<p>
|
|
<a id="orga56a5c6"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeFramesPositions.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org8ff1cb0" class="outline-3">
|
|
<h3 id="org8ff1cb0">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org8ff1cb0">
|
|
|
|
<div id="org06297fa" class="figure">
|
|
<p><img src="figs/stewart-frames-position.png" alt="stewart-frames-position.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 2: </span>Definition of the position of the frames</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc23d352" class="outline-3">
|
|
<h3 id="orgc23d352">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgc23d352">
|
|
<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">initializeFramesPositions</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [stewart] = initializeFramesPositions(stewart, args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - args - Can have the following fields:</span>
|
|
<span class="org-comment">% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]</span>
|
|
<span class="org-comment">% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - stewart - A structure with the following fields:</span>
|
|
<span class="org-comment">% - geometry.H [1x1] - Total Height of the Stewart Platform [m]</span>
|
|
<span class="org-comment">% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]</span>
|
|
<span class="org-comment">% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]</span>
|
|
<span class="org-comment">% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8ccdfcd" class="outline-3">
|
|
<h3 id="org8ccdfcd">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org8ccdfcd">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e<span class="org-type">-</span>3
|
|
args.MO_B (1,1) double {mustBeNumeric} = 50e<span class="org-type">-</span>3
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7d50d54" class="outline-3">
|
|
<h3 id="org7d50d54">Compute the position of each frame</h3>
|
|
<div class="outline-text-3" id="text-org7d50d54">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">H = args.H; <span class="org-comment">% Total Height of the Stewart Platform [m]</span>
|
|
|
|
FO_M = [0; 0; H]; <span class="org-comment">% Position of {M} with respect to {F} [m]</span>
|
|
|
|
MO_B = [0; 0; args.MO_B]; <span class="org-comment">% Position of {B} with respect to {M} [m]</span>
|
|
|
|
FO_A = MO_B <span class="org-type">+</span> FO_M; <span class="org-comment">% Position of {A} with respect to {F} [m]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc902066" class="outline-3">
|
|
<h3 id="orgc902066">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orgc902066">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.geometry.H = H;
|
|
stewart.geometry.FO_M = FO_M;
|
|
stewart.platform_M.MO_B = MO_B;
|
|
stewart.platform_F.FO_A = FO_A;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgccb31c6" class="outline-2">
|
|
<h2 id="orgccb31c6"><span class="section-number-2">3</span> <code>generateGeneralConfiguration</code>: Generate a Very General Configuration</h2>
|
|
<div class="outline-text-2" id="text-3">
|
|
<p>
|
|
<a id="org32105b0"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/generateGeneralConfiguration.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org50b57cc" class="outline-3">
|
|
<h3 id="org50b57cc">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org50b57cc">
|
|
<p>
|
|
Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}.
|
|
The radius of the circles can be chosen as well as the angles where the joints are located (see Figure <a href="#org449c886">3</a>).
|
|
</p>
|
|
|
|
|
|
<div id="org449c886" class="figure">
|
|
<p><img src="figs/stewart_bottom_plate.png" alt="stewart_bottom_plate.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 3: </span>Position of the joints</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5edcb6f" class="outline-3">
|
|
<h3 id="org5edcb6f">Function description</h3>
|
|
<div class="outline-text-3" id="text-org5edcb6f">
|
|
<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">generateGeneralConfiguration</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% generateGeneralConfiguration - Generate a Very General Configuration</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - args - Can have the following fields:</span>
|
|
<span class="org-comment">% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]</span>
|
|
<span class="org-comment">% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]</span>
|
|
<span class="org-comment">% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]</span>
|
|
<span class="org-comment">% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]</span>
|
|
<span class="org-comment">% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]</span>
|
|
<span class="org-comment">% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - stewart - updated Stewart structure with the added fields:</span>
|
|
<span class="org-comment">% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}</span>
|
|
<span class="org-comment">% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc271059" class="outline-3">
|
|
<h3 id="orgc271059">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgc271059">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e<span class="org-type">-</span>3
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e<span class="org-type">-</span>3;
|
|
args.FTh (6,1) double {mustBeNumeric} = [<span class="org-type">-</span>10, 10, 120<span class="org-type">-</span>10, 120<span class="org-type">+</span>10, 240<span class="org-type">-</span>10, 240<span class="org-type">+</span>10]<span class="org-type">*</span>(<span class="org-constant">pi</span><span class="org-type">/</span>180);
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e<span class="org-type">-</span>3
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e<span class="org-type">-</span>3;
|
|
args.MTh (6,1) double {mustBeNumeric} = [<span class="org-type">-</span>60<span class="org-type">+</span>10, 60<span class="org-type">-</span>10, 60<span class="org-type">+</span>10, 180<span class="org-type">-</span>10, 180<span class="org-type">+</span>10, <span class="org-type">-</span>60<span class="org-type">-</span>10]<span class="org-type">*</span>(<span class="org-constant">pi</span><span class="org-type">/</span>180);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgefcf050" class="outline-3">
|
|
<h3 id="orgefcf050">Compute the pose</h3>
|
|
<div class="outline-text-3" id="text-orgefcf050">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Fa = zeros(3,6);
|
|
Mb = zeros(3,6);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
|
|
Fa(<span class="org-type">:</span>,<span class="org-constant">i</span>) = [args.FR<span class="org-type">*</span>cos(args.FTh(<span class="org-constant">i</span>)); args.FR<span class="org-type">*</span>sin(args.FTh(<span class="org-constant">i</span>)); args.FH];
|
|
Mb(<span class="org-type">:</span>,<span class="org-constant">i</span>) = [args.MR<span class="org-type">*</span>cos(args.MTh(<span class="org-constant">i</span>)); args.MR<span class="org-type">*</span>sin(args.MTh(<span class="org-constant">i</span>)); <span class="org-type">-</span>args.MH];
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org0b8bbf0" class="outline-3">
|
|
<h3 id="org0b8bbf0">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org0b8bbf0">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.platform_F.Fa = Fa;
|
|
stewart.platform_M.Mb = Mb;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9944c04" class="outline-2">
|
|
<h2 id="org9944c04"><span class="section-number-2">4</span> <code>computeJointsPose</code>: Compute the Pose of the Joints</h2>
|
|
<div class="outline-text-2" id="text-4">
|
|
<p>
|
|
<a id="orgd0bee51"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/computeJointsPose.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org88c006f" class="outline-3">
|
|
<h3 id="org88c006f">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org88c006f">
|
|
|
|
<div id="org20f7106" class="figure">
|
|
<p><img src="figs/stewart-struts.png" alt="stewart-struts.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 4: </span>Position and orientation of the struts</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3a97461" class="outline-3">
|
|
<h3 id="org3a97461">Function description</h3>
|
|
<div class="outline-text-3" id="text-org3a97461">
|
|
<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">computeJointsPose</span>(<span class="org-variable-name">stewart</span>)
|
|
<span class="org-comment">% computeJointsPose -</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [stewart] = computeJointsPose(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">% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}</span>
|
|
<span class="org-comment">% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}</span>
|
|
<span class="org-comment">% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}</span>
|
|
<span class="org-comment">% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}</span>
|
|
<span class="org-comment">% - geometry.FO_M [3x1] - Position of {M} with respect to {F}</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - stewart - A structure with the following added fields</span>
|
|
<span class="org-comment">% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A}</span>
|
|
<span class="org-comment">% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A}</span>
|
|
<span class="org-comment">% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B}</span>
|
|
<span class="org-comment">% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B}</span>
|
|
<span class="org-comment">% - geometry.l [6x1] - The i'th element is the initial length of strut i</span>
|
|
<span class="org-comment">% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A}</span>
|
|
<span class="org-comment">% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}</span>
|
|
<span class="org-comment">% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut</span>
|
|
<span class="org-comment">% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut</span>
|
|
<span class="org-comment">% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}</span>
|
|
<span class="org-comment">% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8e43add" class="outline-3">
|
|
<h3 id="org8e43add">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-org8e43add">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">assert(isfield(stewart.platform_F, <span class="org-string">'Fa'</span>), <span class="org-string">'stewart.platform_F should have attribute Fa'</span>)
|
|
Fa = stewart.platform_F.Fa;
|
|
|
|
assert(isfield(stewart.platform_M, <span class="org-string">'Mb'</span>), <span class="org-string">'stewart.platform_M should have attribute Mb'</span>)
|
|
Mb = stewart.platform_M.Mb;
|
|
|
|
assert(isfield(stewart.platform_F, <span class="org-string">'FO_A'</span>), <span class="org-string">'stewart.platform_F should have attribute FO_A'</span>)
|
|
FO_A = stewart.platform_F.FO_A;
|
|
|
|
assert(isfield(stewart.platform_M, <span class="org-string">'MO_B'</span>), <span class="org-string">'stewart.platform_M should have attribute MO_B'</span>)
|
|
MO_B = stewart.platform_M.MO_B;
|
|
|
|
assert(isfield(stewart.geometry, <span class="org-string">'FO_M'</span>), <span class="org-string">'stewart.geometry should have attribute FO_M'</span>)
|
|
FO_M = stewart.geometry.FO_M;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge87b302" class="outline-3">
|
|
<h3 id="orge87b302">Compute the position of the Joints</h3>
|
|
<div class="outline-text-3" id="text-orge87b302">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Aa = Fa <span class="org-type">-</span> repmat(FO_A, [1, 6]);
|
|
Bb = Mb <span class="org-type">-</span> repmat(MO_B, [1, 6]);
|
|
|
|
Ab = Bb <span class="org-type">-</span> repmat(<span class="org-type">-</span>MO_B<span class="org-type">-</span>FO_M<span class="org-type">+</span>FO_A, [1, 6]);
|
|
Ba = Aa <span class="org-type">-</span> repmat( MO_B<span class="org-type">+</span>FO_M<span class="org-type">-</span>FO_A, [1, 6]);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3a7e3c5" class="outline-3">
|
|
<h3 id="org3a7e3c5">Compute the strut length and orientation</h3>
|
|
<div class="outline-text-3" id="text-org3a7e3c5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">As = (Ab <span class="org-type">-</span> Aa)<span class="org-type">./</span>vecnorm(Ab <span class="org-type">-</span> Aa); <span class="org-comment">% As_i is the i'th vector of As</span>
|
|
|
|
l = vecnorm(Ab <span class="org-type">-</span> Aa)<span class="org-type">'</span>;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Bs = (Bb <span class="org-type">-</span> Ba)<span class="org-type">./</span>vecnorm(Bb <span class="org-type">-</span> Ba);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9e1258f" class="outline-3">
|
|
<h3 id="org9e1258f">Compute the orientation of the Joints</h3>
|
|
<div class="outline-text-3" id="text-org9e1258f">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">FRa = zeros(3,3,6);
|
|
MRb = zeros(3,3,6);
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
|
|
FRa(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = [cross([0;1;0], As(<span class="org-type">:</span>,<span class="org-constant">i</span>)) , cross(As(<span class="org-type">:</span>,<span class="org-constant">i</span>), cross([0;1;0], As(<span class="org-type">:</span>,<span class="org-constant">i</span>))) , As(<span class="org-type">:</span>,<span class="org-constant">i</span>)];
|
|
FRa(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = FRa(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>)<span class="org-type">./</span>vecnorm(FRa(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>));
|
|
|
|
MRb(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = [cross([0;1;0], Bs(<span class="org-type">:</span>,<span class="org-constant">i</span>)) , cross(Bs(<span class="org-type">:</span>,<span class="org-constant">i</span>), cross([0;1;0], Bs(<span class="org-type">:</span>,<span class="org-constant">i</span>))) , Bs(<span class="org-type">:</span>,<span class="org-constant">i</span>)];
|
|
MRb(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = MRb(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>)<span class="org-type">./</span>vecnorm(MRb(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>));
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgdee1da8" class="outline-3">
|
|
<h3 id="orgdee1da8">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orgdee1da8">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">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<span class="org-type">/</span>2;
|
|
stewart.struts_M.l = l<span class="org-type">/</span>2;
|
|
|
|
stewart.platform_F.FRa = FRa;
|
|
stewart.platform_M.MRb = MRb;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1315282" class="outline-2">
|
|
<h2 id="org1315282"><span class="section-number-2">5</span> <code>initializeStewartPose</code>: Determine the initial stroke in each leg to have the wanted pose</h2>
|
|
<div class="outline-text-2" id="text-5">
|
|
<p>
|
|
<a id="org05598b5"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeStewartPose.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org7f4912f" class="outline-3">
|
|
<h3 id="org7f4912f">Function description</h3>
|
|
<div class="outline-text-3" id="text-org7f4912f">
|
|
<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">initializeStewartPose</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose</span>
|
|
<span class="org-comment">% It uses the inverse kinematic</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [stewart] = initializeStewartPose(stewart, args)</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">% - stewart - updated Stewart structure with the added fields:</span>
|
|
<span class="org-comment">% - 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}</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga884eb1" class="outline-3">
|
|
<h3 id="orga884eb1">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orga884eb1">
|
|
<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-orgbb9abb5" class="outline-3">
|
|
<h3 id="orgbb9abb5">Use the Inverse Kinematic function</h3>
|
|
<div class="outline-text-3" id="text-orgbb9abb5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">[Li, dLi] = inverseKinematics(stewart, <span class="org-string">'AP'</span>, args.AP, <span class="org-string">'ARB'</span>, args.ARB);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9f3b0a3" class="outline-3">
|
|
<h3 id="org9f3b0a3">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org9f3b0a3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.actuators.Leq = dLi;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4674203" class="outline-2">
|
|
<h2 id="org4674203"><span class="section-number-2">6</span> <code>initializeCylindricalPlatforms</code>: Initialize the geometry of the Fixed and Mobile Platforms</h2>
|
|
<div class="outline-text-2" id="text-6">
|
|
<p>
|
|
<a id="orgac77ed6"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeCylindricalPlatforms.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf7385de" class="outline-3">
|
|
<h3 id="orgf7385de">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgf7385de">
|
|
<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">initializeCylindricalPlatforms</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [stewart] = initializeCylindricalPlatforms(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - args - Structure with the following fields:</span>
|
|
<span class="org-comment">% - Fpm [1x1] - Fixed Platform Mass [kg]</span>
|
|
<span class="org-comment">% - Fph [1x1] - Fixed Platform Height [m]</span>
|
|
<span class="org-comment">% - Fpr [1x1] - Fixed Platform Radius [m]</span>
|
|
<span class="org-comment">% - Mpm [1x1] - Mobile Platform Mass [kg]</span>
|
|
<span class="org-comment">% - Mph [1x1] - Mobile Platform Height [m]</span>
|
|
<span class="org-comment">% - Mpr [1x1] - Mobile Platform Radius [m]</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - stewart - updated Stewart structure with the added fields:</span>
|
|
<span class="org-comment">% - platform_F [struct] - structure with the following fields:</span>
|
|
<span class="org-comment">% - type = 1</span>
|
|
<span class="org-comment">% - M [1x1] - Fixed Platform Mass [kg]</span>
|
|
<span class="org-comment">% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]</span>
|
|
<span class="org-comment">% - H [1x1] - Fixed Platform Height [m]</span>
|
|
<span class="org-comment">% - R [1x1] - Fixed Platform Radius [m]</span>
|
|
<span class="org-comment">% - platform_M [struct] - structure with the following fields:</span>
|
|
<span class="org-comment">% - M [1x1] - Mobile Platform Mass [kg]</span>
|
|
<span class="org-comment">% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]</span>
|
|
<span class="org-comment">% - H [1x1] - Mobile Platform Height [m]</span>
|
|
<span class="org-comment">% - R [1x1] - Mobile Platform Radius [m]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org737b9ce" class="outline-3">
|
|
<h3 id="org737b9ce">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org737b9ce">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e<span class="org-type">-</span>3
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e<span class="org-type">-</span>3
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e<span class="org-type">-</span>3
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e<span class="org-type">-</span>3
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf654de0" class="outline-3">
|
|
<h3 id="orgf654de0">Compute the Inertia matrices of platforms</h3>
|
|
<div class="outline-text-3" id="text-orgf654de0">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">I_F = diag([1<span class="org-type">/</span>12<span class="org-type">*</span>args.Fpm <span class="org-type">*</span> (3<span class="org-type">*</span>args.Fpr<span class="org-type">^</span>2 <span class="org-type">+</span> args.Fph<span class="org-type">^</span>2), ...
|
|
1<span class="org-type">/</span>12<span class="org-type">*</span>args.Fpm <span class="org-type">*</span> (3<span class="org-type">*</span>args.Fpr<span class="org-type">^</span>2 <span class="org-type">+</span> args.Fph<span class="org-type">^</span>2), ...
|
|
1<span class="org-type">/</span>2 <span class="org-type">*</span>args.Fpm <span class="org-type">*</span> args.Fpr<span class="org-type">^</span>2]);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">I_M = diag([1<span class="org-type">/</span>12<span class="org-type">*</span>args.Mpm <span class="org-type">*</span> (3<span class="org-type">*</span>args.Mpr<span class="org-type">^</span>2 <span class="org-type">+</span> args.Mph<span class="org-type">^</span>2), ...
|
|
1<span class="org-type">/</span>12<span class="org-type">*</span>args.Mpm <span class="org-type">*</span> (3<span class="org-type">*</span>args.Mpr<span class="org-type">^</span>2 <span class="org-type">+</span> args.Mph<span class="org-type">^</span>2), ...
|
|
1<span class="org-type">/</span>2 <span class="org-type">*</span>args.Mpm <span class="org-type">*</span> args.Mpr<span class="org-type">^</span>2]);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd7d42c3" class="outline-3">
|
|
<h3 id="orgd7d42c3">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orgd7d42c3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.platform_F.type = 1;
|
|
|
|
stewart.platform_F.I = I_F;
|
|
stewart.platform_F.M = args.Fpm;
|
|
stewart.platform_F.R = args.Fpr;
|
|
stewart.platform_F.H = args.Fph;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">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;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb0a1d7b" class="outline-2">
|
|
<h2 id="orgb0a1d7b"><span class="section-number-2">7</span> <code>initializeCylindricalStruts</code>: Define the inertia of cylindrical struts</h2>
|
|
<div class="outline-text-2" id="text-7">
|
|
<p>
|
|
<a id="orgfd38bf8"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeCylindricalStruts.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org9e6d34f" class="outline-3">
|
|
<h3 id="org9e6d34f">Function description</h3>
|
|
<div class="outline-text-3" id="text-org9e6d34f">
|
|
<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">initializeCylindricalStruts</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [stewart] = initializeCylindricalStruts(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - args - Structure with the following fields:</span>
|
|
<span class="org-comment">% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]</span>
|
|
<span class="org-comment">% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]</span>
|
|
<span class="org-comment">% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]</span>
|
|
<span class="org-comment">% - Msm [1x1] - Mass of the Mobile part of the struts [kg]</span>
|
|
<span class="org-comment">% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]</span>
|
|
<span class="org-comment">% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - stewart - updated Stewart structure with the added fields:</span>
|
|
<span class="org-comment">% - struts_F [struct] - structure with the following fields:</span>
|
|
<span class="org-comment">% - M [6x1] - Mass of the Fixed part of the struts [kg]</span>
|
|
<span class="org-comment">% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]</span>
|
|
<span class="org-comment">% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]</span>
|
|
<span class="org-comment">% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]</span>
|
|
<span class="org-comment">% - struts_M [struct] - structure with the following fields:</span>
|
|
<span class="org-comment">% - M [6x1] - Mass of the Mobile part of the struts [kg]</span>
|
|
<span class="org-comment">% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]</span>
|
|
<span class="org-comment">% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]</span>
|
|
<span class="org-comment">% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd0165df" class="outline-3">
|
|
<h3 id="orgd0165df">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgd0165df">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e<span class="org-type">-</span>3
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e<span class="org-type">-</span>3
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e<span class="org-type">-</span>3
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e<span class="org-type">-</span>3
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd943059" class="outline-3">
|
|
<h3 id="orgd943059">Compute the properties of the cylindrical struts</h3>
|
|
<div class="outline-text-3" id="text-orgd943059">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Fsm = ones(6,1)<span class="org-type">.*</span>args.Fsm;
|
|
Fsh = ones(6,1)<span class="org-type">.*</span>args.Fsh;
|
|
Fsr = ones(6,1)<span class="org-type">.*</span>args.Fsr;
|
|
|
|
Msm = ones(6,1)<span class="org-type">.*</span>args.Msm;
|
|
Msh = ones(6,1)<span class="org-type">.*</span>args.Msh;
|
|
Msr = ones(6,1)<span class="org-type">.*</span>args.Msr;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">I_F = zeros(3, 3, 6); <span class="org-comment">% Inertia of the "fixed" part of the strut</span>
|
|
I_M = zeros(3, 3, 6); <span class="org-comment">% Inertia of the "mobile" part of the strut</span>
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
|
|
I_F(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = diag([1<span class="org-type">/</span>12 <span class="org-type">*</span> Fsm(<span class="org-constant">i</span>) <span class="org-type">*</span> (3<span class="org-type">*</span>Fsr(<span class="org-constant">i</span>)<span class="org-type">^</span>2 <span class="org-type">+</span> Fsh(<span class="org-constant">i</span>)<span class="org-type">^</span>2), ...
|
|
1<span class="org-type">/</span>12 <span class="org-type">*</span> Fsm(<span class="org-constant">i</span>) <span class="org-type">*</span> (3<span class="org-type">*</span>Fsr(<span class="org-constant">i</span>)<span class="org-type">^</span>2 <span class="org-type">+</span> Fsh(<span class="org-constant">i</span>)<span class="org-type">^</span>2), ...
|
|
1<span class="org-type">/</span>2 <span class="org-type">*</span> Fsm(<span class="org-constant">i</span>) <span class="org-type">*</span> Fsr(<span class="org-constant">i</span>)<span class="org-type">^</span>2]);
|
|
|
|
I_M(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = diag([1<span class="org-type">/</span>12 <span class="org-type">*</span> Msm(<span class="org-constant">i</span>) <span class="org-type">*</span> (3<span class="org-type">*</span>Msr(<span class="org-constant">i</span>)<span class="org-type">^</span>2 <span class="org-type">+</span> Msh(<span class="org-constant">i</span>)<span class="org-type">^</span>2), ...
|
|
1<span class="org-type">/</span>12 <span class="org-type">*</span> Msm(<span class="org-constant">i</span>) <span class="org-type">*</span> (3<span class="org-type">*</span>Msr(<span class="org-constant">i</span>)<span class="org-type">^</span>2 <span class="org-type">+</span> Msh(<span class="org-constant">i</span>)<span class="org-type">^</span>2), ...
|
|
1<span class="org-type">/</span>2 <span class="org-type">*</span> Msm(<span class="org-constant">i</span>) <span class="org-type">*</span> Msr(<span class="org-constant">i</span>)<span class="org-type">^</span>2]);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3ff10a3" class="outline-3">
|
|
<h3 id="org3ff10a3">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org3ff10a3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">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;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">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;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgae8d0dc" class="outline-2">
|
|
<h2 id="orgae8d0dc"><span class="section-number-2">8</span> <code>initializeStrutDynamics</code>: Add Stiffness and Damping properties of each strut</h2>
|
|
<div class="outline-text-2" id="text-8">
|
|
<p>
|
|
<a id="orgd55e892"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeStrutDynamics.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org0eac2ce" class="outline-3">
|
|
<h3 id="org0eac2ce">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org0eac2ce">
|
|
|
|
<div id="org99aef3e" class="figure">
|
|
<p><img src="figs/piezoelectric_stack.jpg" alt="piezoelectric_stack.jpg" width="500px" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 5: </span>Example of a piezoelectric stach actuator (PI)</p>
|
|
</div>
|
|
|
|
<p>
|
|
A simplistic model of such amplified actuator is shown in Figure <a href="#orgd4c4025">6</a> where:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(K\) represent the vertical stiffness of the actuator</li>
|
|
<li>\(C\) represent the vertical damping of the actuator</li>
|
|
<li>\(F\) represents the force applied by the actuator</li>
|
|
<li>\(F_{m}\) represents the total measured force</li>
|
|
<li>\(v_{m}\) represents the absolute velocity of the top part of the actuator</li>
|
|
<li>\(d_{m}\) represents the total relative displacement of the actuator</li>
|
|
</ul>
|
|
|
|
|
|
<div id="orgd4c4025" class="figure">
|
|
<p><img src="figs/actuator_model_simple.png" alt="actuator_model_simple.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 6: </span>Simple model of an Actuator</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8c4942a" class="outline-3">
|
|
<h3 id="org8c4942a">Function description</h3>
|
|
<div class="outline-text-3" id="text-org8c4942a">
|
|
<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">initializeStrutDynamics</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% initializeStrutDynamics - Add Stiffness and Damping properties of each strut</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [stewart] = initializeStrutDynamics(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - args - Structure with the following fields:</span>
|
|
<span class="org-comment">% - K [6x1] - Stiffness of each strut [N/m]</span>
|
|
<span class="org-comment">% - C [6x1] - Damping of each strut [N/(m/s)]</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - stewart - updated Stewart structure with the added fields:</span>
|
|
<span class="org-comment">% - actuators.type = 1</span>
|
|
<span class="org-comment">% - actuators.K [6x1] - Stiffness of each strut [N/m]</span>
|
|
<span class="org-comment">% - actuators.C [6x1] - Damping of each strut [N/(m/s)]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org0436866" class="outline-3">
|
|
<h3 id="org0436866">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org0436866">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.type char {mustBeMember(args.type,{<span class="org-string">'classical'</span>, <span class="org-string">'amplified'</span>})} = <span class="org-string">'classical'</span>
|
|
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6<span class="org-type">*</span>ones(6,1)
|
|
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1<span class="org-type">*</span>ones(6,1)
|
|
args.k1 (6,1) double {mustBeNumeric} = 1e6<span class="org-type">*</span>ones(6,1)
|
|
args.ke (6,1) double {mustBeNumeric} = 5e6<span class="org-type">*</span>ones(6,1)
|
|
args.ka (6,1) double {mustBeNumeric} = 60e6<span class="org-type">*</span>ones(6,1)
|
|
args.c1 (6,1) double {mustBeNumeric} = 10<span class="org-type">*</span>ones(6,1)
|
|
args.F_gain (6,1) double {mustBeNumeric} = 1<span class="org-type">*</span>ones(6,1)
|
|
args.me (6,1) double {mustBeNumeric} = 0.01<span class="org-type">*</span>ones(6,1)
|
|
args.ma (6,1) double {mustBeNumeric} = 0.01<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3c2e550" class="outline-3">
|
|
<h3 id="org3c2e550">Add Stiffness and Damping properties of each strut</h3>
|
|
<div class="outline-text-3" id="text-org3c2e550">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">if</span> strcmp(args.type, <span class="org-string">'classical'</span>)
|
|
stewart.actuators.type = 1;
|
|
<span class="org-keyword">elseif</span> strcmp(args.type, <span class="org-string">'amplified'</span>)
|
|
stewart.actuators.type = 2;
|
|
<span class="org-keyword">end</span>
|
|
|
|
stewart.actuators.K = args.K;
|
|
stewart.actuators.C = args.C;
|
|
|
|
stewart.actuators.k1 = args.k1;
|
|
stewart.actuators.c1 = args.c1;
|
|
|
|
stewart.actuators.ka = args.ka;
|
|
stewart.actuators.ke = args.ke;
|
|
|
|
stewart.actuators.F_gain = args.F_gain;
|
|
|
|
stewart.actuators.ma = args.ma;
|
|
stewart.actuators.me = args.me;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbc5232e" class="outline-2">
|
|
<h2 id="orgbc5232e"><span class="section-number-2">9</span> <code>initializeJointDynamics</code>: Add Stiffness and Damping properties for spherical joints</h2>
|
|
<div class="outline-text-2" id="text-9">
|
|
<p>
|
|
<a id="orga86aa00"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeJointDynamics.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org2568d4c" class="outline-3">
|
|
<h3 id="org2568d4c">Function description</h3>
|
|
<div class="outline-text-3" id="text-org2568d4c">
|
|
<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">initializeJointDynamics</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [stewart] = initializeJointDynamics(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - args - Structure with the following fields:</span>
|
|
<span class="org-comment">% - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'</span>
|
|
<span class="org-comment">% - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'</span>
|
|
<span class="org-comment">% - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]</span>
|
|
<span class="org-comment">% - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]</span>
|
|
<span class="org-comment">% - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]</span>
|
|
<span class="org-comment">% - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]</span>
|
|
<span class="org-comment">% - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]</span>
|
|
<span class="org-comment">% - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]</span>
|
|
<span class="org-comment">% - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]</span>
|
|
<span class="org-comment">% - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - stewart - updated Stewart structure with the added fields:</span>
|
|
<span class="org-comment">% - stewart.joints_F and stewart.joints_M:</span>
|
|
<span class="org-comment">% - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)</span>
|
|
<span class="org-comment">% - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]</span>
|
|
<span class="org-comment">% - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]</span>
|
|
<span class="org-comment">% - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]</span>
|
|
<span class="org-comment">% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]</span>
|
|
<span class="org-comment">% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]</span>
|
|
<span class="org-comment">% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbf466fe" class="outline-3">
|
|
<h3 id="orgbf466fe">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgbf466fe">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.type_F char {mustBeMember(args.type_F,{<span class="org-string">'universal'</span>, <span class="org-string">'spherical'</span>, <span class="org-string">'universal_p'</span>, <span class="org-string">'spherical_p'</span>, <span class="org-string">'universal_3dof'</span>, <span class="org-string">'spherical_3dof'</span>, <span class="org-string">'flexible'</span>})} = <span class="org-string">'universal'</span>
|
|
args.type_M char {mustBeMember(args.type_M,{<span class="org-string">'universal'</span>, <span class="org-string">'spherical'</span>, <span class="org-string">'universal_p'</span>, <span class="org-string">'spherical_p'</span>, <span class="org-string">'universal_3dof'</span>, <span class="org-string">'spherical_3dof'</span>, <span class="org-string">'flexible'</span>})} = <span class="org-string">'spherical'</span>
|
|
args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33<span class="org-type">*</span>ones(6,1)
|
|
args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>4<span class="org-type">*</span>ones(6,1)
|
|
args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236<span class="org-type">*</span>ones(6,1)
|
|
args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>3<span class="org-type">*</span>ones(6,1)
|
|
args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33<span class="org-type">*</span>ones(6,1)
|
|
args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>4<span class="org-type">*</span>ones(6,1)
|
|
args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236<span class="org-type">*</span>ones(6,1)
|
|
args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>3<span class="org-type">*</span>ones(6,1)
|
|
args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8<span class="org-type">*</span>ones(6,1)
|
|
args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1<span class="org-type">*</span>ones(6,1)
|
|
args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7<span class="org-type">*</span>ones(6,1)
|
|
args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1<span class="org-type">*</span>ones(6,1)
|
|
args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8<span class="org-type">*</span>ones(6,1)
|
|
args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1<span class="org-type">*</span>ones(6,1)
|
|
args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7<span class="org-type">*</span>ones(6,1)
|
|
args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1<span class="org-type">*</span>ones(6,1)
|
|
args.K_M double {mustBeNumeric} = zeros(6,6)
|
|
args.M_M double {mustBeNumeric} = zeros(6,6)
|
|
args.n_xyz_M double {mustBeNumeric} = zeros(2,3)
|
|
args.xi_M double {mustBeNumeric} = 0.1
|
|
args.step_file_M char {} = <span class="org-string">''</span>
|
|
args.K_F double {mustBeNumeric} = zeros(6,6)
|
|
args.M_F double {mustBeNumeric} = zeros(6,6)
|
|
args.n_xyz_F double {mustBeNumeric} = zeros(2,3)
|
|
args.xi_F double {mustBeNumeric} = 0.1
|
|
args.step_file_F char {} = <span class="org-string">''</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd5b8278" class="outline-3">
|
|
<h3 id="orgd5b8278">Add Actuator Type</h3>
|
|
<div class="outline-text-3" id="text-orgd5b8278">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">switch</span> <span class="org-constant">args.type_F</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'universal'</span>
|
|
stewart.joints_F.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'spherical'</span>
|
|
stewart.joints_F.type = 2;
|
|
<span class="org-keyword">case</span> <span class="org-string">'universal_p'</span>
|
|
stewart.joints_F.type = 3;
|
|
<span class="org-keyword">case</span> <span class="org-string">'spherical_p'</span>
|
|
stewart.joints_F.type = 4;
|
|
<span class="org-keyword">case</span> <span class="org-string">'flexible'</span>
|
|
stewart.joints_F.type = 5;
|
|
<span class="org-keyword">case</span> <span class="org-string">'universal_3dof'</span>
|
|
stewart.joints_F.type = 6;
|
|
<span class="org-keyword">case</span> <span class="org-string">'spherical_3dof'</span>
|
|
stewart.joints_F.type = 7;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-keyword">switch</span> <span class="org-constant">args.type_M</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'universal'</span>
|
|
stewart.joints_M.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'spherical'</span>
|
|
stewart.joints_M.type = 2;
|
|
<span class="org-keyword">case</span> <span class="org-string">'universal_p'</span>
|
|
stewart.joints_M.type = 3;
|
|
<span class="org-keyword">case</span> <span class="org-string">'spherical_p'</span>
|
|
stewart.joints_M.type = 4;
|
|
<span class="org-keyword">case</span> <span class="org-string">'flexible'</span>
|
|
stewart.joints_M.type = 5;
|
|
<span class="org-keyword">case</span> <span class="org-string">'universal_3dof'</span>
|
|
stewart.joints_M.type = 6;
|
|
<span class="org-keyword">case</span> <span class="org-string">'spherical_3dof'</span>
|
|
stewart.joints_M.type = 7;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org51cf135" class="outline-3">
|
|
<h3 id="org51cf135">Add Stiffness and Damping in Translation of each strut</h3>
|
|
<div class="outline-text-3" id="text-org51cf135">
|
|
<p>
|
|
Axial and Radial (shear) Stiffness
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.joints_M.Ka = args.Ka_M;
|
|
stewart.joints_M.Kr = args.Kr_M;
|
|
|
|
stewart.joints_F.Ka = args.Ka_F;
|
|
stewart.joints_F.Kr = args.Kr_F;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Translation Damping
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.joints_M.Ca = args.Ca_M;
|
|
stewart.joints_M.Cr = args.Cr_M;
|
|
|
|
stewart.joints_F.Ca = args.Ca_F;
|
|
stewart.joints_F.Cr = args.Cr_F;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1e8eceb" class="outline-3">
|
|
<h3 id="org1e8eceb">Add Stiffness and Damping in Rotation of each strut</h3>
|
|
<div class="outline-text-3" id="text-org1e8eceb">
|
|
<p>
|
|
Rotational Stiffness
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.joints_M.Kf = args.Kf_M;
|
|
stewart.joints_M.Kt = args.Kt_M;
|
|
|
|
stewart.joints_F.Kf = args.Kf_F;
|
|
stewart.joints_F.Kt = args.Kt_F;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Rotational Damping
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.joints_M.Cf = args.Cf_M;
|
|
stewart.joints_M.Ct = args.Ct_M;
|
|
|
|
stewart.joints_F.Cf = args.Cf_F;
|
|
stewart.joints_F.Ct = args.Ct_F;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org74a3fc5" class="outline-3">
|
|
<h3 id="org74a3fc5">Stiffness and Mass matrices for flexible joint</h3>
|
|
<div class="outline-text-3" id="text-org74a3fc5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.joints_F.M = args.M_F;
|
|
stewart.joints_F.K = args.K_F;
|
|
stewart.joints_F.n_xyz = args.n_xyz_F;
|
|
stewart.joints_F.xi = args.xi_F;
|
|
stewart.joints_F.xi = args.xi_F;
|
|
stewart.joints_F.step_file = args.step_file_F;
|
|
|
|
stewart.joints_M.M = args.M_M;
|
|
stewart.joints_M.K = args.K_M;
|
|
stewart.joints_M.n_xyz = args.n_xyz_M;
|
|
stewart.joints_M.xi = args.xi_M;
|
|
stewart.joints_M.step_file = args.step_file_M;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3a7f26e" class="outline-2">
|
|
<h2 id="org3a7f26e"><span class="section-number-2">10</span> <code>initializeInertialSensor</code>: Initialize the inertial sensor in each strut</h2>
|
|
<div class="outline-text-2" id="text-10">
|
|
<p>
|
|
<a id="org10af194"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeInertialSensor.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgcfc37af" class="outline-3">
|
|
<h3 id="orgcfc37af">Geophone - Working Principle</h3>
|
|
<div class="outline-text-3" id="text-orgcfc37af">
|
|
<p>
|
|
From the schematic of the Z-axis geophone shown in Figure <a href="#orgcbee0e9">7</a>, we can write the transfer function from the support velocity \(\dot{w}\) to the relative velocity of the inertial mass \(\dot{d}\):
|
|
\[ \frac{\dot{d}}{\dot{w}} = \frac{-\frac{s^2}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
|
|
with:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(\omega_0 = \sqrt{\frac{k}{m}}\)</li>
|
|
<li>\(\xi = \frac{1}{2} \sqrt{\frac{m}{k}}\)</li>
|
|
</ul>
|
|
|
|
|
|
<div id="orgcbee0e9" class="figure">
|
|
<p><img src="figs/inertial_sensor.png" alt="inertial_sensor.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 7: </span>Schematic of a Z-Axis geophone</p>
|
|
</div>
|
|
|
|
<p>
|
|
We see that at frequencies above \(\omega_0\):
|
|
\[ \frac{\dot{d}}{\dot{w}} \approx -1 \]
|
|
</p>
|
|
|
|
<p>
|
|
And thus, the measurement of the relative velocity of the mass with respect to its support gives the absolute velocity of the support.
|
|
</p>
|
|
|
|
<p>
|
|
We generally want to have the smallest resonant frequency \(\omega_0\) to measure low frequency absolute velocity, however there is a trade-off between \(\omega_0\) and the mass of the inertial mass.
|
|
</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org986e38f" class="outline-3">
|
|
<h3 id="org986e38f">Accelerometer - Working Principle</h3>
|
|
<div class="outline-text-3" id="text-org986e38f">
|
|
<p>
|
|
From the schematic of the Z-axis accelerometer shown in Figure <a href="#orgf6281f4">8</a>, we can write the transfer function from the support acceleration \(\ddot{w}\) to the relative position of the inertial mass \(d\):
|
|
\[ \frac{d}{\ddot{w}} = \frac{-\frac{1}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
|
|
with:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(\omega_0 = \sqrt{\frac{k}{m}}\)</li>
|
|
<li>\(\xi = \frac{1}{2} \sqrt{\frac{m}{k}}\)</li>
|
|
</ul>
|
|
|
|
|
|
<div id="orgf6281f4" class="figure">
|
|
<p><img src="figs/inertial_sensor.png" alt="inertial_sensor.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 8: </span>Schematic of a Z-Axis geophone</p>
|
|
</div>
|
|
|
|
<p>
|
|
We see that at frequencies below \(\omega_0\):
|
|
\[ \frac{d}{\ddot{w}} \approx -\frac{1}{{\omega_0}^2} \]
|
|
</p>
|
|
|
|
<p>
|
|
And thus, the measurement of the relative displacement of the mass with respect to its support gives the absolute acceleration of the support.
|
|
</p>
|
|
|
|
<p>
|
|
Note that there is trade-off between:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>the highest measurable acceleration \(\omega_0\)</li>
|
|
<li>the sensitivity of the accelerometer which is equal to \(-\frac{1}{{\omega_0}^2}\)</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8eae4fc" class="outline-3">
|
|
<h3 id="org8eae4fc">Function description</h3>
|
|
<div class="outline-text-3" id="text-org8eae4fc">
|
|
<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">initializeInertialSensor</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% initializeInertialSensor - Initialize the inertial sensor in each strut</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [stewart] = initializeInertialSensor(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - args - Structure with the following fields:</span>
|
|
<span class="org-comment">% - type - 'geophone', 'accelerometer', 'none'</span>
|
|
<span class="org-comment">% - mass [1x1] - Weight of the inertial mass [kg]</span>
|
|
<span class="org-comment">% - freq [1x1] - Cutoff frequency [Hz]</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - stewart - updated Stewart structure with the added fields:</span>
|
|
<span class="org-comment">% - stewart.sensors.inertial</span>
|
|
<span class="org-comment">% - type - 1 (geophone), 2 (accelerometer), 3 (none)</span>
|
|
<span class="org-comment">% - K [1x1] - Stiffness [N/m]</span>
|
|
<span class="org-comment">% - C [1x1] - Damping [N/(m/s)]</span>
|
|
<span class="org-comment">% - M [1x1] - Inertial Mass [kg]</span>
|
|
<span class="org-comment">% - G [1x1] - Gain</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org14e8700" class="outline-3">
|
|
<h3 id="org14e8700">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org14e8700">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.type char {mustBeMember(args.type,{<span class="org-string">'geophone'</span>, <span class="org-string">'accelerometer'</span>, <span class="org-string">'none'</span>})} = <span class="org-string">'none'</span>
|
|
args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>2
|
|
args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1c3d7c8" class="outline-3">
|
|
<h3 id="org1c3d7c8">Compute the properties of the sensor</h3>
|
|
<div class="outline-text-3" id="text-org1c3d7c8">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">sensor = struct();
|
|
|
|
<span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'geophone'</span>
|
|
sensor.type = 1;
|
|
|
|
sensor.M = args.mass;
|
|
sensor.K = sensor.M <span class="org-type">*</span> (2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>args.freq)<span class="org-type">^</span>2;
|
|
sensor.C = 2<span class="org-type">*</span>sqrt(sensor.M <span class="org-type">*</span> sensor.K);
|
|
<span class="org-keyword">case</span> <span class="org-string">'accelerometer'</span>
|
|
sensor.type = 2;
|
|
|
|
sensor.M = args.mass;
|
|
sensor.K = sensor.M <span class="org-type">*</span> (2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>args.freq)<span class="org-type">^</span>2;
|
|
sensor.C = 2<span class="org-type">*</span>sqrt(sensor.M <span class="org-type">*</span> sensor.K);
|
|
sensor.G = <span class="org-type">-</span>sensor.K<span class="org-type">/</span>sensor.M;
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
sensor.type = 3;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org50cda50" class="outline-3">
|
|
<h3 id="org50cda50">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org50cda50">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.sensors.inertial = sensor;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd6baa46" class="outline-2">
|
|
<h2 id="orgd6baa46"><span class="section-number-2">11</span> <code>displayArchitecture</code>: 3D plot of the Stewart platform architecture</h2>
|
|
<div class="outline-text-2" id="text-11">
|
|
<p>
|
|
<a id="org455c4f1"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/displayArchitecture.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf427022" class="outline-3">
|
|
<h3 id="orgf427022">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgf427022">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[]</span> = <span class="org-function-name">displayArchitecture</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% displayArchitecture - 3D plot of the Stewart platform architecture</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [] = displayArchitecture(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - stewart</span>
|
|
<span class="org-comment">% - args - Structure with 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">% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}</span>
|
|
<span class="org-comment">% - F_color [color] - Color used for the Fixed elements</span>
|
|
<span class="org-comment">% - M_color [color] - Color used for the Mobile elements</span>
|
|
<span class="org-comment">% - L_color [color] - Color used for the Legs elements</span>
|
|
<span class="org-comment">% - frames [true/false] - Display the Frames</span>
|
|
<span class="org-comment">% - legs [true/false] - Display the Legs</span>
|
|
<span class="org-comment">% - joints [true/false] - Display the Joints</span>
|
|
<span class="org-comment">% - labels [true/false] - Display the Labels</span>
|
|
<span class="org-comment">% - platforms [true/false] - Display the Platforms</span>
|
|
<span class="org-comment">% - views ['all', 'xy', 'yz', 'xz', 'default'] -</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgaa63f3d" class="outline-3">
|
|
<h3 id="orgaa63f3d">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgaa63f3d">
|
|
<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)
|
|
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} = <span class="org-constant">true</span>
|
|
args.legs logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
args.joints logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
args.labels logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
args.platforms logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
args.views char {mustBeMember(args.views,{<span class="org-string">'all'</span>, <span class="org-string">'xy'</span>, <span class="org-string">'xz'</span>, <span class="org-string">'yz'</span>, <span class="org-string">'default'</span>})} = <span class="org-string">'default'</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb289e7f" class="outline-3">
|
|
<h3 id="orgb289e7f">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-orgb289e7f">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">assert(isfield(stewart.platform_F, <span class="org-string">'FO_A'</span>), <span class="org-string">'stewart.platform_F should have attribute FO_A'</span>)
|
|
FO_A = stewart.platform_F.FO_A;
|
|
|
|
assert(isfield(stewart.platform_M, <span class="org-string">'MO_B'</span>), <span class="org-string">'stewart.platform_M should have attribute MO_B'</span>)
|
|
MO_B = stewart.platform_M.MO_B;
|
|
|
|
assert(isfield(stewart.geometry, <span class="org-string">'H'</span>), <span class="org-string">'stewart.geometry should have attribute H'</span>)
|
|
H = stewart.geometry.H;
|
|
|
|
assert(isfield(stewart.platform_F, <span class="org-string">'Fa'</span>), <span class="org-string">'stewart.platform_F should have attribute Fa'</span>)
|
|
Fa = stewart.platform_F.Fa;
|
|
|
|
assert(isfield(stewart.platform_M, <span class="org-string">'Mb'</span>), <span class="org-string">'stewart.platform_M should have attribute Mb'</span>)
|
|
Mb = stewart.platform_M.Mb;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-orgb11fd92" class="outline-3">
|
|
<h3 id="orgb11fd92">Figure Creation, Frames and Homogeneous transformations</h3>
|
|
<div class="outline-text-3" id="text-orgb11fd92">
|
|
<p>
|
|
The reference frame of the 3d plot corresponds to the frame \(\{F\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">if</span> <span class="org-type">~</span>strcmp(args.views, <span class="org-string">'all'</span>)
|
|
<span class="org-type">figure</span>;
|
|
<span class="org-keyword">else</span>
|
|
f = <span class="org-type">figure</span>(<span class="org-string">'visible'</span>, <span class="org-string">'off'</span>);
|
|
<span class="org-keyword">end</span>
|
|
|
|
hold on;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is \(\{F\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">FTa = [eye(3), FO_A; ...
|
|
zeros<span class="org-type">(1,3), 1];</span>
|
|
ATb = [args.ARB, args.AP; ...
|
|
zeros<span class="org-type">(1,3), 1];</span>
|
|
BTm = [eye(3), <span class="org-type">-</span>MO_B; ...
|
|
zeros<span class="org-type">(1,3), 1];</span>
|
|
|
|
FTm = FTa<span class="org-type">*</span>ATb<span class="org-type">*</span>BTm;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Let’s define a parameter that define the length of the unit vectors used to display the frames.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">d_unit_vector = H<span class="org-type">/</span>4;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Let’s define a parameter used to position the labels with respect to the center of the element.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">d_label = H<span class="org-type">/</span>20;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7cd8fee" class="outline-3">
|
|
<h3 id="org7cd8fee">Fixed Base elements</h3>
|
|
<div class="outline-text-3" id="text-org7cd8fee">
|
|
<p>
|
|
Let’s first plot the frame \(\{F\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Ff = [0, 0, 0];
|
|
<span class="org-keyword">if</span> args.frames
|
|
quiver3(Ff(1)<span class="org-type">*</span>ones(1,3), Ff(2)<span class="org-type">*</span>ones(1,3), Ff(3)<span class="org-type">*</span>ones(1,3), ...
|
|
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], <span class="org-string">'-'</span>, <span class="org-string">'Color'</span>, args.F_color)
|
|
|
|
<span class="org-keyword">if</span> args.labels
|
|
<span class="org-type">text</span>(Ff(1) <span class="org-type">+</span> d_label, ...
|
|
Ff<span class="org-type">(2) + d_label, ...</span>
|
|
Ff(3) <span class="org-type">+</span> d_label, <span class="org-string">'$\{F\}$'</span>, <span class="org-string">'Color'</span>, args.F_color);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Now plot the frame \(\{A\}\) fixed to the Base.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">if</span> args.frames
|
|
quiver3(FO_A(1)<span class="org-type">*</span>ones(1,3), FO_A(2)<span class="org-type">*</span>ones(1,3), FO_A(3)<span class="org-type">*</span>ones(1,3), ...
|
|
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], <span class="org-string">'-'</span>, <span class="org-string">'Color'</span>, args.F_color)
|
|
|
|
<span class="org-keyword">if</span> args.labels
|
|
<span class="org-type">text</span>(FO_A(1) <span class="org-type">+</span> d_label, ...
|
|
FO_A<span class="org-type">(2) + d_label, ...</span>
|
|
FO_A(3) <span class="org-type">+</span> d_label, <span class="org-string">'$\{A\}$'</span>, <span class="org-string">'Color'</span>, args.F_color);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Let’s then plot the circle corresponding to the shape of the Fixed base.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">if</span> args.platforms <span class="org-type">&&</span> stewart.platform_F.type <span class="org-type">==</span> 1
|
|
theta = [0<span class="org-type">:</span>0.01<span class="org-type">:</span>2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">+</span>0.01]; <span class="org-comment">% Angles [rad]</span>
|
|
v = null([0; 0; 1]<span class="org-type">'</span>); <span class="org-comment">% Two vectors that are perpendicular to the circle normal</span>
|
|
center = [0; 0; 0]; <span class="org-comment">% Center of the circle</span>
|
|
radius = stewart.platform_F.R; <span class="org-comment">% Radius of the circle [m]</span>
|
|
|
|
points = center<span class="org-type">*</span>ones(1, length(theta)) <span class="org-type">+</span> radius<span class="org-type">*</span>(v(<span class="org-type">:</span>,1)<span class="org-type">*</span>cos(theta) <span class="org-type">+</span> v(<span class="org-type">:</span>,2)<span class="org-type">*</span>sin(theta));
|
|
|
|
plot3(points(1,<span class="org-type">:</span>), ...
|
|
points<span class="org-type">(2,:), ...</span>
|
|
points(3,<span class="org-type">:</span>), <span class="org-string">'-'</span>, <span class="org-string">'Color'</span>, args.F_color);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Let’s now plot the position and labels of the Fixed Joints
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">if</span> args.joints
|
|
scatter3(Fa(1,<span class="org-type">:</span>), ...
|
|
Fa<span class="org-type">(2,:), ...</span>
|
|
Fa(3,<span class="org-type">:</span>), <span class="org-string">'MarkerEdgeColor'</span>, args.F_color);
|
|
<span class="org-keyword">if</span> args.labels
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:size(Fa,2)</span>
|
|
<span class="org-type">text</span>(Fa(1,<span class="org-constant">i</span>) <span class="org-type">+</span> d_label, ...
|
|
Fa(2,<span class="org-constant">i</span>), ...
|
|
Fa(3,<span class="org-constant">i</span>), sprintf(<span class="org-string">'$a_{%i}$'</span>, <span class="org-constant">i</span>), <span class="org-string">'Color'</span>, args.F_color);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgacb8eb7" class="outline-3">
|
|
<h3 id="orgacb8eb7">Mobile Platform elements</h3>
|
|
<div class="outline-text-3" id="text-orgacb8eb7">
|
|
<p>
|
|
Plot the frame \(\{M\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Fm = FTm<span class="org-type">*</span>[0; 0; 0; 1]; <span class="org-comment">% Get the position of frame {M} w.r.t. {F}</span>
|
|
|
|
<span class="org-keyword">if</span> args.frames
|
|
FM_uv = FTm<span class="org-type">*</span>[d_unit_vector<span class="org-type">*</span>eye(3); zeros(1,3)]; <span class="org-comment">% Rotated Unit vectors</span>
|
|
quiver3(Fm(1)<span class="org-type">*</span>ones(1,3), Fm(2)<span class="org-type">*</span>ones(1,3), Fm(3)<span class="org-type">*</span>ones(1,3), ...
|
|
FM_uv(1,1<span class="org-type">:</span>3), FM_uv(2,1<span class="org-type">:</span>3), FM_uv(3,1<span class="org-type">:</span>3), <span class="org-string">'-'</span>, <span class="org-string">'Color'</span>, args.M_color)
|
|
|
|
<span class="org-keyword">if</span> args.labels
|
|
<span class="org-type">text</span>(Fm(1) <span class="org-type">+</span> d_label, ...
|
|
Fm<span class="org-type">(2) + d_label, ...</span>
|
|
Fm(3) <span class="org-type">+</span> d_label, <span class="org-string">'$\{M\}$'</span>, <span class="org-string">'Color'</span>, args.M_color);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Plot the frame \(\{B\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">FB = FO_A <span class="org-type">+</span> args.AP;
|
|
|
|
<span class="org-keyword">if</span> args.frames
|
|
FB_uv = FTm<span class="org-type">*</span>[d_unit_vector<span class="org-type">*</span>eye(3); zeros(1,3)]; <span class="org-comment">% Rotated Unit vectors</span>
|
|
quiver3(FB(1)<span class="org-type">*</span>ones(1,3), FB(2)<span class="org-type">*</span>ones(1,3), FB(3)<span class="org-type">*</span>ones(1,3), ...
|
|
FB_uv(1,1<span class="org-type">:</span>3), FB_uv(2,1<span class="org-type">:</span>3), FB_uv(3,1<span class="org-type">:</span>3), <span class="org-string">'-'</span>, <span class="org-string">'Color'</span>, args.M_color)
|
|
|
|
<span class="org-keyword">if</span> args.labels
|
|
<span class="org-type">text</span>(FB(1) <span class="org-type">-</span> d_label, ...
|
|
FB<span class="org-type">(2) + d_label, ...</span>
|
|
FB(3) <span class="org-type">+</span> d_label, <span class="org-string">'$\{B\}$'</span>, <span class="org-string">'Color'</span>, args.M_color);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Let’s then plot the circle corresponding to the shape of the Mobile platform.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">if</span> args.platforms <span class="org-type">&&</span> stewart.platform_M.type <span class="org-type">==</span> 1
|
|
theta = [0<span class="org-type">:</span>0.01<span class="org-type">:</span>2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">+</span>0.01]; <span class="org-comment">% Angles [rad]</span>
|
|
v = null((FTm(1<span class="org-type">:</span>3,1<span class="org-type">:</span>3)<span class="org-type">*</span>[0;0;1])<span class="org-type">'</span>); <span class="org-comment">% Two vectors that are perpendicular to the circle normal</span>
|
|
center = Fm(1<span class="org-type">:</span>3); <span class="org-comment">% Center of the circle</span>
|
|
radius = stewart.platform_M.R; <span class="org-comment">% Radius of the circle [m]</span>
|
|
|
|
points = center<span class="org-type">*</span>ones(1, length(theta)) <span class="org-type">+</span> radius<span class="org-type">*</span>(v(<span class="org-type">:</span>,1)<span class="org-type">*</span>cos(theta) <span class="org-type">+</span> v(<span class="org-type">:</span>,2)<span class="org-type">*</span>sin(theta));
|
|
|
|
plot3(points(1,<span class="org-type">:</span>), ...
|
|
points<span class="org-type">(2,:), ...</span>
|
|
points(3,<span class="org-type">:</span>), <span class="org-string">'-'</span>, <span class="org-string">'Color'</span>, args.M_color);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Plot the position and labels of the rotation joints fixed to the mobile platform.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">if</span> args.joints
|
|
Fb = FTm<span class="org-type">*</span>[Mb;ones(1,6)];
|
|
|
|
scatter3(Fb(1,<span class="org-type">:</span>), ...
|
|
Fb<span class="org-type">(2,:), ...</span>
|
|
Fb(3,<span class="org-type">:</span>), <span class="org-string">'MarkerEdgeColor'</span>, args.M_color);
|
|
|
|
<span class="org-keyword">if</span> args.labels
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:size(Fb,2)</span>
|
|
<span class="org-type">text</span>(Fb(1,<span class="org-constant">i</span>) <span class="org-type">+</span> d_label, ...
|
|
Fb(2,<span class="org-constant">i</span>), ...
|
|
Fb(3,<span class="org-constant">i</span>), sprintf(<span class="org-string">'$b_{%i}$'</span>, <span class="org-constant">i</span>), <span class="org-string">'Color'</span>, args.M_color);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7f9320b" class="outline-3">
|
|
<h3 id="org7f9320b">Legs</h3>
|
|
<div class="outline-text-3" id="text-org7f9320b">
|
|
<p>
|
|
Plot the legs connecting the joints of the fixed base to the joints of the mobile platform.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">if</span> args.legs
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
|
|
plot3([Fa(1,<span class="org-constant">i</span>), Fb(1,<span class="org-constant">i</span>)], ...
|
|
[Fa(2,<span class="org-constant">i</span>), Fb(2,<span class="org-constant">i</span>)], ...
|
|
[Fa(3,<span class="org-constant">i</span>), Fb(3,<span class="org-constant">i</span>)], <span class="org-string">'-'</span>, <span class="org-string">'Color'</span>, args.L_color);
|
|
|
|
<span class="org-keyword">if</span> args.labels
|
|
<span class="org-type">text</span>((Fa(1,<span class="org-constant">i</span>)<span class="org-type">+</span>Fb(1,<span class="org-constant">i</span>))<span class="org-type">/</span>2 <span class="org-type">+</span> d_label, ...
|
|
(Fa(2,<span class="org-constant">i</span>)<span class="org-type">+</span>Fb(2,<span class="org-constant">i</span>))<span class="org-type">/</span>2, ...
|
|
(Fa(3,<span class="org-constant">i</span>)<span class="org-type">+</span>Fb(3,<span class="org-constant">i</span>))<span class="org-type">/</span>2, sprintf(<span class="org-string">'$%i$'</span>, <span class="org-constant">i</span>), <span class="org-string">'Color'</span>, args.L_color);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org925a393" class="outline-3">
|
|
<h3 id="org925a393"><span class="section-number-3">11.1</span> Figure parameters</h3>
|
|
<div class="outline-text-3" id="text-11-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">switch</span> <span class="org-constant">args.views</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'default'</span>
|
|
view([1 <span class="org-type">-</span>0.6 0.4]);
|
|
<span class="org-keyword">case</span> <span class="org-string">'xy'</span>
|
|
view([0 0 1]);
|
|
<span class="org-keyword">case</span> <span class="org-string">'xz'</span>
|
|
view([0 <span class="org-type">-</span>1 0]);
|
|
<span class="org-keyword">case</span> <span class="org-string">'yz'</span>
|
|
view([1 0 0]);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-type">axis</span> equal;
|
|
<span class="org-type">axis</span> off;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org44e536d" class="outline-3">
|
|
<h3 id="org44e536d"><span class="section-number-3">11.2</span> Subplots</h3>
|
|
<div class="outline-text-3" id="text-11-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">if</span> strcmp(args.views, <span class="org-string">'all'</span>)
|
|
hAx = findobj(<span class="org-string">'type'</span>, <span class="org-string">'axes'</span>);
|
|
|
|
<span class="org-type">figure</span>;
|
|
s1 = subplot(2,2,1);
|
|
copyobj(<span class="org-type">get</span>(hAx(<span class="org-variable-name">1</span>), <span class="org-string">'Children'</span>), s1);
|
|
view([0 0 1]);
|
|
<span class="org-type">axis</span> equal;
|
|
<span class="org-type">axis</span> off;
|
|
title(<span class="org-string">'Top'</span>)
|
|
|
|
s2 = subplot(2,2,2);
|
|
copyobj(<span class="org-type">get</span>(hAx(<span class="org-variable-name">1</span>), <span class="org-string">'Children'</span>), s2);
|
|
view([1 <span class="org-type">-</span>0.6 0.4]);
|
|
<span class="org-type">axis</span> equal;
|
|
<span class="org-type">axis</span> off;
|
|
|
|
s3 = subplot(2,2,3);
|
|
copyobj(<span class="org-type">get</span>(hAx(<span class="org-variable-name">1</span>), <span class="org-string">'Children'</span>), s3);
|
|
view([1 0 0]);
|
|
<span class="org-type">axis</span> equal;
|
|
<span class="org-type">axis</span> off;
|
|
title(<span class="org-string">'Front'</span>)
|
|
|
|
s4 = subplot(2,2,4);
|
|
copyobj(<span class="org-type">get</span>(hAx(<span class="org-variable-name">1</span>), <span class="org-string">'Children'</span>), s4);
|
|
view([0 <span class="org-type">-</span>1 0]);
|
|
<span class="org-type">axis</span> equal;
|
|
<span class="org-type">axis</span> off;
|
|
title(<span class="org-string">'Side'</span>)
|
|
|
|
close(f);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-orgecfd55f" class="outline-2">
|
|
<h2 id="orgecfd55f"><span class="section-number-2">12</span> <code>describeStewartPlatform</code>: Display some text describing the current defined Stewart Platform</h2>
|
|
<div class="outline-text-2" id="text-12">
|
|
<p>
|
|
<a id="org8cc8939"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/describeStewartPlatform.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf8354f3" class="outline-3">
|
|
<h3 id="orgf8354f3">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgf8354f3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[]</span> = <span class="org-function-name">describeStewartPlatform</span>(<span class="org-variable-name">stewart</span>)
|
|
<span class="org-comment">% describeStewartPlatform - Display some text describing the current defined Stewart Platform</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [] = describeStewartPlatform(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - stewart</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org80e76f7" class="outline-3">
|
|
<h3 id="org80e76f7">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org80e76f7">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1d49caa" class="outline-3">
|
|
<h3 id="org1d49caa"><span class="section-number-3">12.1</span> Geometry</h3>
|
|
<div class="outline-text-3" id="text-12-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf(<span class="org-string">'GEOMETRY:\n'</span>)
|
|
fprintf(<span class="org-string">'- The height between the fixed based and the top platform is %.3g [mm].\n'</span>, 1e3<span class="org-type">*</span>stewart.geometry.H)
|
|
|
|
<span class="org-keyword">if</span> stewart.platform_M.MO_B(3) <span class="org-type">></span> 0
|
|
fprintf(<span class="org-string">'- Frame {A} is located %.3g [mm] above the top platform.\n'</span>, 1e3<span class="org-type">*</span>stewart.platform_M.MO_B(3))
|
|
<span class="org-keyword">else</span>
|
|
fprintf(<span class="org-string">'- Frame {A} is located %.3g [mm] below the top platform.\n'</span>, <span class="org-type">-</span> 1e3<span class="org-type">*</span>stewart.platform_M.MO_B(3))
|
|
<span class="org-keyword">end</span>
|
|
|
|
fprintf(<span class="org-string">'- The initial length of the struts are:\n'</span>)
|
|
fprintf(<span class="org-string">'\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n'</span>, 1e3<span class="org-type">*</span>stewart.geometry.l)
|
|
fprintf(<span class="org-string">'\n'</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgcb66771" class="outline-3">
|
|
<h3 id="orgcb66771"><span class="section-number-3">12.2</span> Actuators</h3>
|
|
<div class="outline-text-3" id="text-12-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf(<span class="org-string">'ACTUATORS:\n'</span>)
|
|
<span class="org-keyword">if</span> stewart.actuators.type <span class="org-type">==</span> 1
|
|
fprintf(<span class="org-string">'- The actuators are classical.\n'</span>)
|
|
fprintf(<span class="org-string">'- The Stiffness and Damping of each actuators is:\n'</span>)
|
|
fprintf(<span class="org-string">'\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n'</span>, stewart.actuators.K(1), stewart.actuators.C(1))
|
|
<span class="org-keyword">elseif</span> stewart.actuators.type <span class="org-type">==</span> 2
|
|
fprintf(<span class="org-string">'- The actuators are mechanicaly amplified.\n'</span>)
|
|
fprintf(<span class="org-string">'- The vertical stiffness and damping contribution of the piezoelectric stack is:\n'</span>)
|
|
fprintf(<span class="org-string">'\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n'</span>, stewart.actuators.Ka(1), stewart.actuators.Ca(1))
|
|
fprintf(<span class="org-string">'- Vertical stiffness when the piezoelectric stack is removed is:\n'</span>)
|
|
fprintf(<span class="org-string">'\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n'</span>, stewart.actuators.Kr(1), stewart.actuators.Cr(1))
|
|
<span class="org-keyword">end</span>
|
|
fprintf(<span class="org-string">'\n'</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4630b77" class="outline-3">
|
|
<h3 id="org4630b77"><span class="section-number-3">12.3</span> Joints</h3>
|
|
<div class="outline-text-3" id="text-12-3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf(<span class="org-string">'JOINTS:\n'</span>)
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Type of the joints on the fixed base.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">switch</span> <span class="org-constant">stewart.joints_F.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-constant">1</span>
|
|
fprintf(<span class="org-string">'- The joints on the fixed based are universal joints\n'</span>)
|
|
<span class="org-keyword">case</span> <span class="org-constant">2</span>
|
|
fprintf(<span class="org-string">'- The joints on the fixed based are spherical joints\n'</span>)
|
|
<span class="org-keyword">case</span> <span class="org-constant">3</span>
|
|
fprintf(<span class="org-string">'- The joints on the fixed based are perfect universal joints\n'</span>)
|
|
<span class="org-keyword">case</span> <span class="org-constant">4</span>
|
|
fprintf(<span class="org-string">'- The joints on the fixed based are perfect spherical joints\n'</span>)
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Type of the joints on the mobile platform.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">switch</span> <span class="org-constant">stewart.joints_M.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-constant">1</span>
|
|
fprintf(<span class="org-string">'- The joints on the mobile based are universal joints\n'</span>)
|
|
<span class="org-keyword">case</span> <span class="org-constant">2</span>
|
|
fprintf(<span class="org-string">'- The joints on the mobile based are spherical joints\n'</span>)
|
|
<span class="org-keyword">case</span> <span class="org-constant">3</span>
|
|
fprintf(<span class="org-string">'- The joints on the mobile based are perfect universal joints\n'</span>)
|
|
<span class="org-keyword">case</span> <span class="org-constant">4</span>
|
|
fprintf(<span class="org-string">'- The joints on the mobile based are perfect spherical joints\n'</span>)
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Position of the fixed joints
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf(<span class="org-string">'- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n'</span>)
|
|
fprintf(<span class="org-string">'\t % .3g \t % .3g \t % .3g\n'</span>, 1e3<span class="org-type">*</span>stewart.platform_F.Fa)
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Position of the mobile joints
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf(<span class="org-string">'- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n'</span>)
|
|
fprintf(<span class="org-string">'\t % .3g \t % .3g \t % .3g\n'</span>, 1e3<span class="org-type">*</span>stewart.platform_M.Mb)
|
|
fprintf(<span class="org-string">'\n'</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org47a9cf0" class="outline-3">
|
|
<h3 id="org47a9cf0"><span class="section-number-3">12.4</span> Kinematics</h3>
|
|
<div class="outline-text-3" id="text-12-4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf(<span class="org-string">'KINEMATICS:\n'</span>)
|
|
|
|
<span class="org-keyword">if</span> isfield(stewart.kinematics, <span class="org-string">'K'</span>)
|
|
fprintf(<span class="org-string">'- The Stiffness matrix K is (in [N/m]):\n'</span>)
|
|
fprintf(<span class="org-string">'\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n'</span>, stewart.kinematics.K)
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-keyword">if</span> isfield(stewart.kinematics, <span class="org-string">'C'</span>)
|
|
fprintf(<span class="org-string">'- The Damping matrix C is (in [m/N]):\n'</span>)
|
|
fprintf(<span class="org-string">'\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n'</span>, stewart.kinematics.C)
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org65fc289" class="outline-2">
|
|
<h2 id="org65fc289"><span class="section-number-2">13</span> <code>generateCubicConfiguration</code>: Generate a Cubic Configuration</h2>
|
|
<div class="outline-text-2" id="text-13">
|
|
<p>
|
|
<a id="org677ea95"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/generateCubicConfiguration.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org3b2822c" class="outline-3">
|
|
<h3 id="org3b2822c">Function description</h3>
|
|
<div class="outline-text-3" id="text-org3b2822c">
|
|
<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">generateCubicConfiguration</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% generateCubicConfiguration - Generate a Cubic Configuration</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [stewart] = generateCubicConfiguration(stewart, args)</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.H [1x1] - Total height of the platform [m]</span>
|
|
<span class="org-comment">% - args - Can have the following fields:</span>
|
|
<span class="org-comment">% - Hc [1x1] - Height of the "useful" part of the cube [m]</span>
|
|
<span class="org-comment">% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]</span>
|
|
<span class="org-comment">% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]</span>
|
|
<span class="org-comment">% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - stewart - updated Stewart structure with the added fields:</span>
|
|
<span class="org-comment">% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}</span>
|
|
<span class="org-comment">% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga88ecd8" class="outline-3">
|
|
<h3 id="orga88ecd8">Documentation</h3>
|
|
<div class="outline-text-3" id="text-orga88ecd8">
|
|
|
|
<div id="org70070f0" class="figure">
|
|
<p><img src="figs/cubic-configuration-definition.png" alt="cubic-configuration-definition.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 9: </span>Cubic Configuration</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org0bd99c9" class="outline-3">
|
|
<h3 id="org0bd99c9">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org0bd99c9">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e<span class="org-type">-</span>3
|
|
args.FOc (1,1) double {mustBeNumeric} = 50e<span class="org-type">-</span>3
|
|
args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e<span class="org-type">-</span>3
|
|
args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e<span class="org-type">-</span>3
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4cb31f0" class="outline-3">
|
|
<h3 id="org4cb31f0">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-org4cb31f0">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">assert(isfield(stewart.geometry, <span class="org-string">'H'</span>), <span class="org-string">'stewart.geometry should have attribute H'</span>)
|
|
H = stewart.geometry.H;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge94a885" class="outline-3">
|
|
<h3 id="orge94a885">Position of the Cube</h3>
|
|
<div class="outline-text-3" id="text-orge94a885">
|
|
<p>
|
|
We define the useful points of the cube with respect to the Cube’s center.
|
|
\({}^{C}C\) are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">sx = [ 2; <span class="org-type">-</span>1; <span class="org-type">-</span>1];
|
|
sy = [ 0; 1; <span class="org-type">-</span>1];
|
|
sz = [ 1; 1; 1];
|
|
|
|
R = [sx, sy, sz]<span class="org-type">./</span>vecnorm([sx, sy, sz]);
|
|
|
|
L = args.Hc<span class="org-type">*</span>sqrt(3);
|
|
|
|
Cc = R<span class="org-type">'*</span>[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] <span class="org-type">-</span> [0;0;1.5<span class="org-type">*</span>args.Hc];
|
|
|
|
CCf = [Cc(<span class="org-type">:</span>,1), Cc(<span class="org-type">:</span>,3), Cc(<span class="org-type">:</span>,3), Cc(<span class="org-type">:</span>,5), Cc(<span class="org-type">:</span>,5), Cc(<span class="org-type">:</span>,1)]; <span class="org-comment">% CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg</span>
|
|
CCm = [Cc(<span class="org-type">:</span>,2), Cc(<span class="org-type">:</span>,2), Cc(<span class="org-type">:</span>,4), Cc(<span class="org-type">:</span>,4), Cc(<span class="org-type">:</span>,6), Cc(<span class="org-type">:</span>,6)]; <span class="org-comment">% CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgffbfeac" class="outline-3">
|
|
<h3 id="orgffbfeac">Compute the pose</h3>
|
|
<div class="outline-text-3" id="text-orgffbfeac">
|
|
<p>
|
|
We can compute the vector of each leg \({}^{C}\hat{\bm{s}}_{i}\) (unit vector from \({}^{C}C_{f}\) to \({}^{C}C_{m}\)).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">CSi = (CCm <span class="org-type">-</span> CCf)<span class="org-type">./</span>vecnorm(CCm <span class="org-type">-</span> CCf);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
We now which to compute the position of the joints \(a_{i}\) and \(b_{i}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Fa = CCf <span class="org-type">+</span> [0; 0; args.FOc] <span class="org-type">+</span> ((args.FHa<span class="org-type">-</span>(args.FOc<span class="org-type">-</span>args.Hc<span class="org-type">/</span>2))<span class="org-type">./</span>CSi(3,<span class="org-type">:</span>))<span class="org-type">.*</span>CSi;
|
|
Mb = CCf <span class="org-type">+</span> [0; 0; args.FOc<span class="org-type">-</span>H] <span class="org-type">+</span> ((H<span class="org-type">-</span>args.MHb<span class="org-type">-</span>(args.FOc<span class="org-type">-</span>args.Hc<span class="org-type">/</span>2))<span class="org-type">./</span>CSi(3,<span class="org-type">:</span>))<span class="org-type">.*</span>CSi;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org99658a2" class="outline-3">
|
|
<h3 id="org99658a2">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org99658a2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.platform_F.Fa = Fa;
|
|
stewart.platform_M.Mb = Mb;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9e8cbfa" class="outline-2">
|
|
<h2 id="org9e8cbfa"><span class="section-number-2">14</span> <code>computeJacobian</code>: Compute the Jacobian Matrix</h2>
|
|
<div class="outline-text-2" id="text-14">
|
|
<p>
|
|
<a id="orgfb113f5"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/computeJacobian.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org9bd1578" class="outline-3">
|
|
<h3 id="org9bd1578">Function description</h3>
|
|
<div class="outline-text-3" id="text-org9bd1578">
|
|
<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>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [stewart] = computeJacobian(stewart)</span>
|
|
<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">% - 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">% - 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-org1e70cf8" class="outline-3">
|
|
<h3 id="org1e70cf8">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-org1e70cf8">
|
|
<div class="org-src-container">
|
|
<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-org9bcd9b9" class="outline-3">
|
|
<h3 id="org9bcd9b9">Compute Jacobian Matrix</h3>
|
|
<div class="outline-text-3" id="text-org9bcd9b9">
|
|
<div class="org-src-container">
|
|
<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-orgf08eda6" class="outline-3">
|
|
<h3 id="orgf08eda6">Compute Stiffness Matrix</h3>
|
|
<div class="outline-text-3" id="text-orgf08eda6">
|
|
<div class="org-src-container">
|
|
<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-orgd164132" class="outline-3">
|
|
<h3 id="orgd164132">Compute Compliance Matrix</h3>
|
|
<div class="outline-text-3" id="text-orgd164132">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">C = inv(K);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgaf3b338" class="outline-3">
|
|
<h3 id="orgaf3b338">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orgaf3b338">
|
|
<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-org03168fc" class="outline-2">
|
|
<h2 id="org03168fc"><span class="section-number-2">15</span> <code>inverseKinematics</code>: Compute Inverse Kinematics</h2>
|
|
<div class="outline-text-2" id="text-15">
|
|
<p>
|
|
<a id="org681bcb5"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/inverseKinematics.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbdc5fb1" class="outline-3">
|
|
<h3 id="orgbdc5fb1">Theory</h3>
|
|
<div class="outline-text-3" id="text-orgbdc5fb1">
|
|
<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>
|
|
|
|
<p>
|
|
From the geometry of the manipulator, the loop closure for each limb, \(i = 1, 2, \dots, 6\) can be written as
|
|
</p>
|
|
\begin{align*}
|
|
l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
|
|
&= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
|
|
\end{align*}
|
|
|
|
<p>
|
|
To obtain the length of each actuator and eliminate \(\hat{\bm{s}}_i\), it is sufficient to dot multiply each side by itself:
|
|
</p>
|
|
\begin{equation}
|
|
l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
|
|
\end{equation}
|
|
|
|
<p>
|
|
Hence, for \(i = 1, 2, \dots, 6\), each limb length can be uniquely determined by:
|
|
</p>
|
|
\begin{equation}
|
|
l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
|
|
\end{equation}
|
|
|
|
<p>
|
|
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.
|
|
</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org17070b1" class="outline-3">
|
|
<h3 id="org17070b1">Function description</h3>
|
|
<div class="outline-text-3" id="text-org17070b1">
|
|
<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">% - 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-orgaf1a90a" class="outline-3">
|
|
<h3 id="orgaf1a90a">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgaf1a90a">
|
|
<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-orgeb2a0d2" class="outline-3">
|
|
<h3 id="orgeb2a0d2">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-orgeb2a0d2">
|
|
<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-org8b70a76" class="outline-3">
|
|
<h3 id="org8b70a76">Compute</h3>
|
|
<div class="outline-text-3" id="text-org8b70a76">
|
|
<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>l;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org278d55b" class="outline-2">
|
|
<h2 id="org278d55b"><span class="section-number-2">16</span> <code>forwardKinematicsApprox</code>: Compute the Approximate Forward Kinematics</h2>
|
|
<div class="outline-text-2" id="text-16">
|
|
<p>
|
|
<a id="org5b15db4"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/forwardKinematicsApprox.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org8623f0c" class="outline-3">
|
|
<h3 id="org8623f0c">Function description</h3>
|
|
<div class="outline-text-3" id="text-org8623f0c">
|
|
<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>
|
|
<span class="org-comment">% the Jacobian Matrix</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)</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">% - 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>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - P [3x1] - The estimated position of {B} with respect to {A}</span>
|
|
<span class="org-comment">% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb133a15" class="outline-3">
|
|
<h3 id="orgb133a15">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgb133a15">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgefe7763" class="outline-3">
|
|
<h3 id="orgefe7763">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-orgefe7763">
|
|
<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-orgf17cab9" class="outline-3">
|
|
<h3 id="orgf17cab9">Computation</h3>
|
|
<div class="outline-text-3" id="text-orgf17cab9">
|
|
<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 = J<span class="org-type">\</span>args.dL;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The position vector corresponds to the first 3 elements.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">P = X(1<span class="org-type">:</span>3);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The next 3 elements are the orientation of {B} with respect to {A} expressed
|
|
using the screw axis.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">theta = norm(X(4<span class="org-type">:</span>6));
|
|
s = X(4<span class="org-type">:</span>6)<span class="org-type">/</span>theta;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
We then compute the corresponding rotation matrix.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">R = [s(1)<span class="org-type">^</span>2<span class="org-type">*</span>(1<span class="org-type">-</span>cos(theta)) <span class="org-type">+</span> cos(theta) , s(1)<span class="org-type">*</span>s(2)<span class="org-type">*</span>(1<span class="org-type">-</span>cos(theta)) <span class="org-type">-</span> s(3)<span class="org-type">*</span>sin(theta), s(1)<span class="org-type">*</span>s(3)<span class="org-type">*</span>(1<span class="org-type">-</span>cos(theta)) <span class="org-type">+</span> s(2)<span class="org-type">*</span>sin(theta);
|
|
s<span class="org-type">(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);</span>
|
|
s<span class="org-type">(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)];</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="postamble" class="status">
|
|
<p class="author">Author: Dehaeze Thomas</p>
|
|
<p class="date">Created: 2020-11-03 mar. 09:45</p>
|
|
</div>
|
|
</body>
|
|
</html>
|