2523 lines
130 KiB
HTML
2523 lines
130 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>
|
|
<!-- 2021-02-20 sam. 23:09 -->
|
|
<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="https://research.tdehaeze.xyz/css/style.css"/>
|
|
<script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
|
|
<script>
|
|
MathJax = {
|
|
svg: {
|
|
scale: 1,
|
|
fontCache: "global"
|
|
},
|
|
tex: {
|
|
tags: "ams",
|
|
multlineWidth: "%MULTLINEWIDTH",
|
|
tagSide: "right",
|
|
macros: {bm: ["\\boldsymbol{#1}",1],},
|
|
tagIndent: ".8em"
|
|
}
|
|
};
|
|
</script>
|
|
<script id="MathJax-script" async
|
|
src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-svg.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="#orge7f2e4c">1. <code>initializeStewartPlatform</code>: Initialize the Stewart Platform structure</a>
|
|
<ul>
|
|
<li><a href="#org7437904">Documentation</a></li>
|
|
<li><a href="#org18108c7">Function description</a></li>
|
|
<li><a href="#org69ef310">Initialize the Stewart structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgbeccb46">2. <code>initializeFramesPositions</code>: Initialize the positions of frames {A}, {B}, {F} and {M}</a>
|
|
<ul>
|
|
<li><a href="#org54ff91d">Documentation</a></li>
|
|
<li><a href="#orgae517fc">Function description</a></li>
|
|
<li><a href="#org73b6431">Optional Parameters</a></li>
|
|
<li><a href="#orgaa23d94">Compute the position of each frame</a></li>
|
|
<li><a href="#orgb62946f">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orga77e5c5">3. <code>generateGeneralConfiguration</code>: Generate a Very General Configuration</a>
|
|
<ul>
|
|
<li><a href="#org4477edd">Documentation</a></li>
|
|
<li><a href="#org79d56c3">Function description</a></li>
|
|
<li><a href="#org9f060d3">Optional Parameters</a></li>
|
|
<li><a href="#org73e478f">Compute the pose</a></li>
|
|
<li><a href="#org35c894b">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgc957583">4. <code>computeJointsPose</code>: Compute the Pose of the Joints</a>
|
|
<ul>
|
|
<li><a href="#orga3a832b">Documentation</a></li>
|
|
<li><a href="#orgb2d33b9">Function description</a></li>
|
|
<li><a href="#org7e7d25f">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#org18c8305">Compute the position of the Joints</a></li>
|
|
<li><a href="#orgbce62d5">Compute the strut length and orientation</a></li>
|
|
<li><a href="#org1cfa133">Compute the orientation of the Joints</a></li>
|
|
<li><a href="#org01435b1">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgd545bd5">5. <code>initializeStewartPose</code>: Determine the initial stroke in each leg to have the wanted pose</a>
|
|
<ul>
|
|
<li><a href="#orgffc304e">Function description</a></li>
|
|
<li><a href="#orgc2e20c0">Optional Parameters</a></li>
|
|
<li><a href="#orgb65c297">Use the Inverse Kinematic function</a></li>
|
|
<li><a href="#org900faff">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org3f565b9">6. <code>initializeCylindricalPlatforms</code>: Initialize the geometry of the Fixed and Mobile Platforms</a>
|
|
<ul>
|
|
<li><a href="#orga2ae915">Function description</a></li>
|
|
<li><a href="#orge119415">Optional Parameters</a></li>
|
|
<li><a href="#org5e2f937">Compute the Inertia matrices of platforms</a></li>
|
|
<li><a href="#org6836f86">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org62ecd15">7. <code>initializeCylindricalStruts</code>: Define the inertia of cylindrical struts</a>
|
|
<ul>
|
|
<li><a href="#org06296f6">Function description</a></li>
|
|
<li><a href="#org515bf7f">Optional Parameters</a></li>
|
|
<li><a href="#org6a8f756">Compute the properties of the cylindrical struts</a></li>
|
|
<li><a href="#orgca52b33">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org83fc6ab">8. <code>initializeStrutDynamics</code>: Add Stiffness and Damping properties of each strut</a>
|
|
<ul>
|
|
<li><a href="#org5fc3036">Documentation</a></li>
|
|
<li><a href="#orge22b09f">Function description</a></li>
|
|
<li><a href="#orgddbcf42">Optional Parameters</a></li>
|
|
<li><a href="#orgb7fb7ad">Add Stiffness and Damping properties of each strut</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgb3ee6f9">9. <code>initializeJointDynamics</code>: Add Stiffness and Damping properties for spherical joints</a>
|
|
<ul>
|
|
<li><a href="#org0018bc5">Function description</a></li>
|
|
<li><a href="#org5fa604b">Optional Parameters</a></li>
|
|
<li><a href="#org914aea7">Add Actuator Type</a></li>
|
|
<li><a href="#org4d678a7">Add Stiffness and Damping in Translation of each strut</a></li>
|
|
<li><a href="#orgfbfeff9">Add Stiffness and Damping in Rotation of each strut</a></li>
|
|
<li><a href="#org11005ad">Stiffness and Mass matrices for flexible joint</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org311a79b">10. <code>initializeInertialSensor</code>: Initialize the inertial sensor in each strut</a>
|
|
<ul>
|
|
<li><a href="#org62cf4fb">Geophone - Working Principle</a></li>
|
|
<li><a href="#orgeccb960">Accelerometer - Working Principle</a></li>
|
|
<li><a href="#org4a8ead1">Function description</a></li>
|
|
<li><a href="#org1843796">Optional Parameters</a></li>
|
|
<li><a href="#orgea5c452">Compute the properties of the sensor</a></li>
|
|
<li><a href="#orga99b20d">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org611c75c">11. <code>displayArchitecture</code>: 3D plot of the Stewart platform architecture</a>
|
|
<ul>
|
|
<li><a href="#org6a940aa">Function description</a></li>
|
|
<li><a href="#orgdb2c1a7">Optional Parameters</a></li>
|
|
<li><a href="#org14706e8">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#org0f76e3b">Figure Creation, Frames and Homogeneous transformations</a></li>
|
|
<li><a href="#org9cb7062">Fixed Base elements</a></li>
|
|
<li><a href="#orgc272598">Mobile Platform elements</a></li>
|
|
<li><a href="#orgfeaf65a">Legs</a></li>
|
|
<li><a href="#org8ae9c48">11.1. Figure parameters</a></li>
|
|
<li><a href="#orgb92caec">11.2. Subplots</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org334a337">12. <code>describeStewartPlatform</code>: Display some text describing the current defined Stewart Platform</a>
|
|
<ul>
|
|
<li><a href="#orgd7efc3c">Function description</a></li>
|
|
<li><a href="#org193f623">Optional Parameters</a></li>
|
|
<li><a href="#org316408d">12.1. Geometry</a></li>
|
|
<li><a href="#orgdb5d86c">12.2. Actuators</a></li>
|
|
<li><a href="#org48d5b2e">12.3. Joints</a></li>
|
|
<li><a href="#org62135e5">12.4. Kinematics</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org9decc94">13. <code>generateCubicConfiguration</code>: Generate a Cubic Configuration</a>
|
|
<ul>
|
|
<li><a href="#org3485ee4">Function description</a></li>
|
|
<li><a href="#orgedfb402">Documentation</a></li>
|
|
<li><a href="#org0a637a2">Optional Parameters</a></li>
|
|
<li><a href="#org13c71d3">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#org44e0590">Position of the Cube</a></li>
|
|
<li><a href="#org92b12fa">Compute the pose</a></li>
|
|
<li><a href="#orgc91d6aa">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org10a231c">14. <code>computeJacobian</code>: Compute the Jacobian Matrix</a>
|
|
<ul>
|
|
<li><a href="#org7841d59">Function description</a></li>
|
|
<li><a href="#orgf2616df">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#org35ea7eb">Compute Jacobian Matrix</a></li>
|
|
<li><a href="#orgf645c75">Compute Stiffness Matrix</a></li>
|
|
<li><a href="#org21fadda">Compute Compliance Matrix</a></li>
|
|
<li><a href="#orgac75de7">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgcc980cd">15. <code>inverseKinematics</code>: Compute Inverse Kinematics</a>
|
|
<ul>
|
|
<li><a href="#org931c849">Theory</a></li>
|
|
<li><a href="#org5e787de">Function description</a></li>
|
|
<li><a href="#org2a4efbd">Optional Parameters</a></li>
|
|
<li><a href="#org318e667">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#orgeced891">Compute</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org07fbe58">16. <code>forwardKinematicsApprox</code>: Compute the Approximate Forward Kinematics</a>
|
|
<ul>
|
|
<li><a href="#org5c8b01d">Function description</a></li>
|
|
<li><a href="#orgd653374">Optional Parameters</a></li>
|
|
<li><a href="#org1699c2d">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#orgf7dc851">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-orge7f2e4c" class="outline-2">
|
|
<h2 id="orge7f2e4c"><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="orgd31cddc"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeStewartPlatform.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org7437904" class="outline-3">
|
|
<h3 id="org7437904">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org7437904">
|
|
|
|
<div id="orgd348bb2" 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-org18108c7" class="outline-3">
|
|
<h3 id="org18108c7">Function description</h3>
|
|
<div class="outline-text-3" id="text-org18108c7">
|
|
<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-org69ef310" class="outline-3">
|
|
<h3 id="org69ef310">Initialize the Stewart structure</h3>
|
|
<div class="outline-text-3" id="text-org69ef310">
|
|
<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-orgbeccb46" class="outline-2">
|
|
<h2 id="orgbeccb46"><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="org7dba982"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeFramesPositions.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org54ff91d" class="outline-3">
|
|
<h3 id="org54ff91d">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org54ff91d">
|
|
|
|
<div id="orgb60aeab" 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-orgae517fc" class="outline-3">
|
|
<h3 id="orgae517fc">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgae517fc">
|
|
<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-org73b6431" class="outline-3">
|
|
<h3 id="org73b6431">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org73b6431">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-variable-name">args</span>.H (1,1) double {mustBeNumeric, mustBePositive} = 90e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.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-orgaa23d94" class="outline-3">
|
|
<h3 id="orgaa23d94">Compute the position of each frame</h3>
|
|
<div class="outline-text-3" id="text-orgaa23d94">
|
|
<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-orgb62946f" class="outline-3">
|
|
<h3 id="orgb62946f">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orgb62946f">
|
|
<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-orga77e5c5" class="outline-2">
|
|
<h2 id="orga77e5c5"><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="orga3f9dba"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/generateGeneralConfiguration.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org4477edd" class="outline-3">
|
|
<h3 id="org4477edd">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org4477edd">
|
|
<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="#org1fedc41">3</a>).
|
|
</p>
|
|
|
|
|
|
<div id="org1fedc41" 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-org79d56c3" class="outline-3">
|
|
<h3 id="org79d56c3">Function description</h3>
|
|
<div class="outline-text-3" id="text-org79d56c3">
|
|
<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-org9f060d3" class="outline-3">
|
|
<h3 id="org9f060d3">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org9f060d3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-variable-name">args</span>.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e<span class="org-type">-</span>3;
|
|
<span class="org-variable-name">args</span>.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);
|
|
<span class="org-variable-name">args</span>.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e<span class="org-type">-</span>3;
|
|
<span class="org-variable-name">args</span>.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-org73e478f" class="outline-3">
|
|
<h3 id="org73e478f">Compute the pose</h3>
|
|
<div class="outline-text-3" id="text-org73e478f">
|
|
<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-org35c894b" class="outline-3">
|
|
<h3 id="org35c894b">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org35c894b">
|
|
<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-orgc957583" class="outline-2">
|
|
<h2 id="orgc957583"><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="orga11f6a6"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/computeJointsPose.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orga3a832b" class="outline-3">
|
|
<h3 id="orga3a832b">Documentation</h3>
|
|
<div class="outline-text-3" id="text-orga3a832b">
|
|
|
|
<div id="orgbdc241f" 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-orgb2d33b9" class="outline-3">
|
|
<h3 id="orgb2d33b9">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgb2d33b9">
|
|
<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-org7e7d25f" class="outline-3">
|
|
<h3 id="org7e7d25f">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-org7e7d25f">
|
|
<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-org18c8305" class="outline-3">
|
|
<h3 id="org18c8305">Compute the position of the Joints</h3>
|
|
<div class="outline-text-3" id="text-org18c8305">
|
|
<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-orgbce62d5" class="outline-3">
|
|
<h3 id="orgbce62d5">Compute the strut length and orientation</h3>
|
|
<div class="outline-text-3" id="text-orgbce62d5">
|
|
<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-org1cfa133" class="outline-3">
|
|
<h3 id="org1cfa133">Compute the orientation of the Joints</h3>
|
|
<div class="outline-text-3" id="text-org1cfa133">
|
|
<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-org01435b1" class="outline-3">
|
|
<h3 id="org01435b1">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org01435b1">
|
|
<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-orgd545bd5" class="outline-2">
|
|
<h2 id="orgd545bd5"><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="orgf11f163"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeStewartPose.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgffc304e" class="outline-3">
|
|
<h3 id="orgffc304e">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgffc304e">
|
|
<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-orgc2e20c0" class="outline-3">
|
|
<h3 id="orgc2e20c0">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgc2e20c0">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-variable-name">args</span>.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
<span class="org-variable-name">args</span>.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb65c297" class="outline-3">
|
|
<h3 id="orgb65c297">Use the Inverse Kinematic function</h3>
|
|
<div class="outline-text-3" id="text-orgb65c297">
|
|
<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-org900faff" class="outline-3">
|
|
<h3 id="org900faff">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org900faff">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> stewart.actuators.Leq = dLi;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3f565b9" class="outline-2">
|
|
<h2 id="org3f565b9"><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="org4eaf7b4"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeCylindricalPlatforms.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orga2ae915" class="outline-3">
|
|
<h3 id="orga2ae915">Function description</h3>
|
|
<div class="outline-text-3" id="text-orga2ae915">
|
|
<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-orge119415" class="outline-3">
|
|
<h3 id="orge119415">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orge119415">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-variable-name">args</span>.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
<span class="org-variable-name">args</span>.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
<span class="org-variable-name">args</span>.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.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-org5e2f937" class="outline-3">
|
|
<h3 id="org5e2f937">Compute the Inertia matrices of platforms</h3>
|
|
<div class="outline-text-3" id="text-org5e2f937">
|
|
<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-org6836f86" class="outline-3">
|
|
<h3 id="org6836f86">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org6836f86">
|
|
<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-org62ecd15" class="outline-2">
|
|
<h2 id="org62ecd15"><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="org24298b7"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeCylindricalStruts.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org06296f6" class="outline-3">
|
|
<h3 id="org06296f6">Function description</h3>
|
|
<div class="outline-text-3" id="text-org06296f6">
|
|
<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-org515bf7f" class="outline-3">
|
|
<h3 id="org515bf7f">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org515bf7f">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-variable-name">args</span>.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
<span class="org-variable-name">args</span>.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
<span class="org-variable-name">args</span>.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.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-org6a8f756" class="outline-3">
|
|
<h3 id="org6a8f756">Compute the properties of the cylindrical struts</h3>
|
|
<div class="outline-text-3" id="text-org6a8f756">
|
|
<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-orgca52b33" class="outline-3">
|
|
<h3 id="orgca52b33">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orgca52b33">
|
|
<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-org83fc6ab" class="outline-2">
|
|
<h2 id="org83fc6ab"><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="org06fbb6c"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeStrutDynamics.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org5fc3036" class="outline-3">
|
|
<h3 id="org5fc3036">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org5fc3036">
|
|
|
|
<div id="org71d2515" 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="#orge5212d2">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="orge5212d2" 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-orge22b09f" class="outline-3">
|
|
<h3 id="orge22b09f">Function description</h3>
|
|
<div class="outline-text-3" id="text-orge22b09f">
|
|
<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-orgddbcf42" class="outline-3">
|
|
<h3 id="orgddbcf42">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgddbcf42">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-variable-name">args</span>.type char {mustBeMember(args.type,{<span class="org-string">'classical'</span>, <span class="org-string">'amplified'</span>})} = <span class="org-string">'classical'</span>
|
|
<span class="org-variable-name">args</span>.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.k1 (6,1) double {mustBeNumeric} = 1e6<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.ke (6,1) double {mustBeNumeric} = 5e6<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.ka (6,1) double {mustBeNumeric} = 60e6<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.c1 (6,1) double {mustBeNumeric} = 10<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.F_gain (6,1) double {mustBeNumeric} = 1<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.me (6,1) double {mustBeNumeric} = 0.01<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.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-orgb7fb7ad" class="outline-3">
|
|
<h3 id="orgb7fb7ad">Add Stiffness and Damping properties of each strut</h3>
|
|
<div class="outline-text-3" id="text-orgb7fb7ad">
|
|
<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-orgb3ee6f9" class="outline-2">
|
|
<h2 id="orgb3ee6f9"><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="orgbaeb9f9"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeJointDynamics.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org0018bc5" class="outline-3">
|
|
<h3 id="org0018bc5">Function description</h3>
|
|
<div class="outline-text-3" id="text-org0018bc5">
|
|
<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-org5fa604b" class="outline-3">
|
|
<h3 id="org5fa604b">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org5fa604b">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-variable-name">args</span>.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>
|
|
<span class="org-variable-name">args</span>.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>
|
|
<span class="org-variable-name">args</span>.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>4<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>3<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>4<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>3<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.K_M double {mustBeNumeric} = zeros(6,6)
|
|
<span class="org-variable-name">args</span>.M_M double {mustBeNumeric} = zeros(6,6)
|
|
<span class="org-variable-name">args</span>.n_xyz_M double {mustBeNumeric} = zeros(2,3)
|
|
<span class="org-variable-name">args</span>.xi_M double {mustBeNumeric} = 0.1
|
|
<span class="org-variable-name">args</span>.step_file_M char {} = <span class="org-string">''</span>
|
|
<span class="org-variable-name">args</span>.K_F double {mustBeNumeric} = zeros(6,6)
|
|
<span class="org-variable-name">args</span>.M_F double {mustBeNumeric} = zeros(6,6)
|
|
<span class="org-variable-name">args</span>.n_xyz_F double {mustBeNumeric} = zeros(2,3)
|
|
<span class="org-variable-name">args</span>.xi_F double {mustBeNumeric} = 0.1
|
|
<span class="org-variable-name">args</span>.step_file_F char {} = <span class="org-string">''</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org914aea7" class="outline-3">
|
|
<h3 id="org914aea7">Add Actuator Type</h3>
|
|
<div class="outline-text-3" id="text-org914aea7">
|
|
<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-org4d678a7" class="outline-3">
|
|
<h3 id="org4d678a7">Add Stiffness and Damping in Translation of each strut</h3>
|
|
<div class="outline-text-3" id="text-org4d678a7">
|
|
<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-orgfbfeff9" class="outline-3">
|
|
<h3 id="orgfbfeff9">Add Stiffness and Damping in Rotation of each strut</h3>
|
|
<div class="outline-text-3" id="text-orgfbfeff9">
|
|
<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-org11005ad" class="outline-3">
|
|
<h3 id="org11005ad">Stiffness and Mass matrices for flexible joint</h3>
|
|
<div class="outline-text-3" id="text-org11005ad">
|
|
<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-org311a79b" class="outline-2">
|
|
<h2 id="org311a79b"><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="org79381ea"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeInertialSensor.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org62cf4fb" class="outline-3">
|
|
<h3 id="org62cf4fb">Geophone - Working Principle</h3>
|
|
<div class="outline-text-3" id="text-org62cf4fb">
|
|
<p>
|
|
From the schematic of the Z-axis geophone shown in Figure <a href="#orgdaf8258">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="orgdaf8258" 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-orgeccb960" class="outline-3">
|
|
<h3 id="orgeccb960">Accelerometer - Working Principle</h3>
|
|
<div class="outline-text-3" id="text-orgeccb960">
|
|
<p>
|
|
From the schematic of the Z-axis accelerometer shown in Figure <a href="#org4065027">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="org4065027" 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-org4a8ead1" class="outline-3">
|
|
<h3 id="org4a8ead1">Function description</h3>
|
|
<div class="outline-text-3" id="text-org4a8ead1">
|
|
<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-org1843796" class="outline-3">
|
|
<h3 id="org1843796">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org1843796">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-variable-name">args</span>.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>
|
|
<span class="org-variable-name">args</span>.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>2
|
|
<span class="org-variable-name">args</span>.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgea5c452" class="outline-3">
|
|
<h3 id="orgea5c452">Compute the properties of the sensor</h3>
|
|
<div class="outline-text-3" id="text-orgea5c452">
|
|
<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-orga99b20d" class="outline-3">
|
|
<h3 id="orga99b20d">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orga99b20d">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> stewart.sensors.inertial = sensor;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org611c75c" class="outline-2">
|
|
<h2 id="org611c75c"><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="org3c576cf"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/displayArchitecture.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org6a940aa" class="outline-3">
|
|
<h3 id="org6a940aa">Function description</h3>
|
|
<div class="outline-text-3" id="text-org6a940aa">
|
|
<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-orgdb2c1a7" class="outline-3">
|
|
<h3 id="orgdb2c1a7">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgdb2c1a7">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-variable-name">args</span>.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
<span class="org-variable-name">args</span>.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
<span class="org-variable-name">args</span>.F_color = [0 0.4470 0.7410]
|
|
<span class="org-variable-name">args</span>.M_color = [0.8500 0.3250 0.0980]
|
|
<span class="org-variable-name">args</span>.L_color = [0 0 0]
|
|
<span class="org-variable-name">args</span>.frames logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-variable-name">args</span>.legs logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-variable-name">args</span>.joints logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-variable-name">args</span>.labels logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-variable-name">args</span>.platforms logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-variable-name">args</span>.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-org14706e8" class="outline-3">
|
|
<h3 id="org14706e8">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-org14706e8">
|
|
<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-org0f76e3b" class="outline-3">
|
|
<h3 id="org0f76e3b">Figure Creation, Frames and Homogeneous transformations</h3>
|
|
<div class="outline-text-3" id="text-org0f76e3b">
|
|
<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-org9cb7062" class="outline-3">
|
|
<h3 id="org9cb7062">Fixed Base elements</h3>
|
|
<div class="outline-text-3" id="text-org9cb7062">
|
|
<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-orgc272598" class="outline-3">
|
|
<h3 id="orgc272598">Mobile Platform elements</h3>
|
|
<div class="outline-text-3" id="text-orgc272598">
|
|
<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-orgfeaf65a" class="outline-3">
|
|
<h3 id="orgfeaf65a">Legs</h3>
|
|
<div class="outline-text-3" id="text-orgfeaf65a">
|
|
<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-org8ae9c48" class="outline-3">
|
|
<h3 id="org8ae9c48"><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-orgb92caec" class="outline-3">
|
|
<h3 id="orgb92caec"><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-org334a337" class="outline-2">
|
|
<h2 id="org334a337"><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="org84ac7ad"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/describeStewartPlatform.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd7efc3c" class="outline-3">
|
|
<h3 id="orgd7efc3c">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgd7efc3c">
|
|
<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-org193f623" class="outline-3">
|
|
<h3 id="org193f623">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org193f623">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org316408d" class="outline-3">
|
|
<h3 id="org316408d"><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-orgdb5d86c" class="outline-3">
|
|
<h3 id="orgdb5d86c"><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-org48d5b2e" class="outline-3">
|
|
<h3 id="org48d5b2e"><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-org62135e5" class="outline-3">
|
|
<h3 id="org62135e5"><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-org9decc94" class="outline-2">
|
|
<h2 id="org9decc94"><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="orgeb6daaf"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/generateCubicConfiguration.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org3485ee4" class="outline-3">
|
|
<h3 id="org3485ee4">Function description</h3>
|
|
<div class="outline-text-3" id="text-org3485ee4">
|
|
<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-orgedfb402" class="outline-3">
|
|
<h3 id="orgedfb402">Documentation</h3>
|
|
<div class="outline-text-3" id="text-orgedfb402">
|
|
|
|
<div id="orga5dae44" 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-org0a637a2" class="outline-3">
|
|
<h3 id="org0a637a2">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org0a637a2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-variable-name">args</span>.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.FOc (1,1) double {mustBeNumeric} = 50e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.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-org13c71d3" class="outline-3">
|
|
<h3 id="org13c71d3">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-org13c71d3">
|
|
<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-org44e0590" class="outline-3">
|
|
<h3 id="org44e0590">Position of the Cube</h3>
|
|
<div class="outline-text-3" id="text-org44e0590">
|
|
<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-org92b12fa" class="outline-3">
|
|
<h3 id="org92b12fa">Compute the pose</h3>
|
|
<div class="outline-text-3" id="text-org92b12fa">
|
|
<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-orgc91d6aa" class="outline-3">
|
|
<h3 id="orgc91d6aa">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orgc91d6aa">
|
|
<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-org10a231c" class="outline-2">
|
|
<h2 id="org10a231c"><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="orgea11d49"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/computeJacobian.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org7841d59" class="outline-3">
|
|
<h3 id="org7841d59">Function description</h3>
|
|
<div class="outline-text-3" id="text-org7841d59">
|
|
<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-orgf2616df" class="outline-3">
|
|
<h3 id="orgf2616df">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-orgf2616df">
|
|
<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-org35ea7eb" class="outline-3">
|
|
<h3 id="org35ea7eb">Compute Jacobian Matrix</h3>
|
|
<div class="outline-text-3" id="text-org35ea7eb">
|
|
<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-orgf645c75" class="outline-3">
|
|
<h3 id="orgf645c75">Compute Stiffness Matrix</h3>
|
|
<div class="outline-text-3" id="text-orgf645c75">
|
|
<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-org21fadda" class="outline-3">
|
|
<h3 id="org21fadda">Compute Compliance Matrix</h3>
|
|
<div class="outline-text-3" id="text-org21fadda">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> C = inv(K);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgac75de7" class="outline-3">
|
|
<h3 id="orgac75de7">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orgac75de7">
|
|
<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-orgcc980cd" class="outline-2">
|
|
<h2 id="orgcc980cd"><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="org7c6158b"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/inverseKinematics.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org931c849" class="outline-3">
|
|
<h3 id="org931c849">Theory</h3>
|
|
<div class="outline-text-3" id="text-org931c849">
|
|
<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-org5e787de" class="outline-3">
|
|
<h3 id="org5e787de">Function description</h3>
|
|
<div class="outline-text-3" id="text-org5e787de">
|
|
<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-org2a4efbd" class="outline-3">
|
|
<h3 id="org2a4efbd">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org2a4efbd">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-variable-name">args</span>.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
<span class="org-variable-name">args</span>.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org318e667" class="outline-3">
|
|
<h3 id="org318e667">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-org318e667">
|
|
<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-orgeced891" class="outline-3">
|
|
<h3 id="orgeced891">Compute</h3>
|
|
<div class="outline-text-3" id="text-orgeced891">
|
|
<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-org07fbe58" class="outline-2">
|
|
<h2 id="org07fbe58"><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="orgbaa3db0"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/forwardKinematicsApprox.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org5c8b01d" class="outline-3">
|
|
<h3 id="org5c8b01d">Function description</h3>
|
|
<div class="outline-text-3" id="text-org5c8b01d">
|
|
<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-orgd653374" class="outline-3">
|
|
<h3 id="orgd653374">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgd653374">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">stewart</span>
|
|
<span class="org-variable-name">args</span>.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1699c2d" class="outline-3">
|
|
<h3 id="org1699c2d">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-org1699c2d">
|
|
<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-orgf7dc851" class="outline-3">
|
|
<h3 id="orgf7dc851">Computation</h3>
|
|
<div class="outline-text-3" id="text-orgf7dc851">
|
|
<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: 2021-02-20 sam. 23:09</p>
|
|
</div>
|
|
</body>
|
|
</html>
|