2594 lines
89 KiB
HTML
2594 lines
89 KiB
HTML
<?xml version="1.0" encoding="utf-8"?>
|
|
<?xml version="1.0" encoding="utf-8"?>
|
|
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
|
|
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
|
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
|
<head>
|
|
<!-- 2020-05-05 mar. 10:34 -->
|
|
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
|
<title>Stewart Platform - Simscape Model</title>
|
|
<meta name="generator" content="Org mode" />
|
|
<meta name="author" content="Dehaeze Thomas" />
|
|
<link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
|
<link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
|
<script src="./js/jquery.min.js"></script>
|
|
<script src="./js/bootstrap.min.js"></script>
|
|
<script src="./js/jquery.stickytableheaders.min.js"></script>
|
|
<script src="./js/readtheorg.js"></script>
|
|
<script>MathJax = {
|
|
tex: {
|
|
tags: 'ams',
|
|
macros: {bm: ["\\boldsymbol{#1}",1],}
|
|
}
|
|
};
|
|
</script>
|
|
<script type="text/javascript" src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js"></script>
|
|
</head>
|
|
<body>
|
|
<div id="org-div-home-and-up">
|
|
<a accesskey="h" href="./index.html"> UP </a>
|
|
|
|
|
<a accesskey="H" href="./index.html"> HOME </a>
|
|
</div><div id="content">
|
|
<h1 class="title">Stewart Platform - Simscape Model</h1>
|
|
<div id="table-of-contents">
|
|
<h2>Table of Contents</h2>
|
|
<div id="text-table-of-contents">
|
|
<ul>
|
|
<li><a href="#orgcaca5e0">1. <code>initializeStewartPlatform</code>: Initialize the Stewart Platform structure</a>
|
|
<ul>
|
|
<li><a href="#orgc11609b">Documentation</a></li>
|
|
<li><a href="#org37e6edf">Function description</a></li>
|
|
<li><a href="#org3622825">Initialize the Stewart structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgac25f89">2. <code>initializeFramesPositions</code>: Initialize the positions of frames {A}, {B}, {F} and {M}</a>
|
|
<ul>
|
|
<li><a href="#org2011b55">Documentation</a></li>
|
|
<li><a href="#org40fe9e8">Function description</a></li>
|
|
<li><a href="#org6441308">Optional Parameters</a></li>
|
|
<li><a href="#org7d50d54">Compute the position of each frame</a></li>
|
|
<li><a href="#orgf6ce3ee">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgccb31c6">3. <code>generateGeneralConfiguration</code>: Generate a Very General Configuration</a>
|
|
<ul>
|
|
<li><a href="#org0759816">Documentation</a></li>
|
|
<li><a href="#orgaac37b6">Function description</a></li>
|
|
<li><a href="#org2af2f34">Optional Parameters</a></li>
|
|
<li><a href="#orgfac23f3">Compute the pose</a></li>
|
|
<li><a href="#org69f4916">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org9944c04">4. <code>computeJointsPose</code>: Compute the Pose of the Joints</a>
|
|
<ul>
|
|
<li><a href="#org2d1a6c0">Documentation</a></li>
|
|
<li><a href="#org50f74a1">Function description</a></li>
|
|
<li><a href="#orgf462a70">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#orge87b302">Compute the position of the Joints</a></li>
|
|
<li><a href="#org3a7e3c5">Compute the strut length and orientation</a></li>
|
|
<li><a href="#org9e1258f">Compute the orientation of the Joints</a></li>
|
|
<li><a href="#org2c160bb">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org1315282">5. <code>initializeStewartPose</code>: Determine the initial stroke in each leg to have the wanted pose</a>
|
|
<ul>
|
|
<li><a href="#org1e2fafd">Function description</a></li>
|
|
<li><a href="#org034bf20">Optional Parameters</a></li>
|
|
<li><a href="#orgbb9abb5">Use the Inverse Kinematic function</a></li>
|
|
<li><a href="#orgf124a3e">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org4674203">6. <code>initializeCylindricalPlatforms</code>: Initialize the geometry of the Fixed and Mobile Platforms</a>
|
|
<ul>
|
|
<li><a href="#org84a14a5">Function description</a></li>
|
|
<li><a href="#orge580054">Optional Parameters</a></li>
|
|
<li><a href="#orgf654de0">Compute the Inertia matrices of platforms</a></li>
|
|
<li><a href="#orge34849b">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgb0a1d7b">7. <code>initializeCylindricalStruts</code>: Define the inertia of cylindrical struts</a>
|
|
<ul>
|
|
<li><a href="#org27c9756">Function description</a></li>
|
|
<li><a href="#org060b144">Optional Parameters</a></li>
|
|
<li><a href="#orgd943059">Compute the properties of the cylindrical struts</a></li>
|
|
<li><a href="#org5d7924e">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgae8d0dc">8. <code>initializeStrutDynamics</code>: Add Stiffness and Damping properties of each strut</a>
|
|
<ul>
|
|
<li><a href="#org70888ee">Documentation</a></li>
|
|
<li><a href="#org2fd9286">Function description</a></li>
|
|
<li><a href="#orgebba910">Optional Parameters</a></li>
|
|
<li><a href="#org3c2e550">Add Stiffness and Damping properties of each strut</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org682a09c">9. <code>initializeAmplifiedStrutDynamics</code>: Add Stiffness and Damping properties of each strut for an amplified piezoelectric actuator</a>
|
|
<ul>
|
|
<li><a href="#org70c61fd">Documentation</a></li>
|
|
<li><a href="#org4b8bd84">Function description</a></li>
|
|
<li><a href="#org94cace2">Optional Parameters</a></li>
|
|
<li><a href="#org2e42182">Compute the total stiffness and damping</a></li>
|
|
<li><a href="#orgee22bea">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgbc5232e">10. <code>initializeJointDynamics</code>: Add Stiffness and Damping properties for spherical joints</a>
|
|
<ul>
|
|
<li><a href="#orgc806fca">Function description</a></li>
|
|
<li><a href="#org4403345">Optional Parameters</a></li>
|
|
<li><a href="#orgd5b8278">Add Actuator Type</a></li>
|
|
<li><a href="#org51cf135">Add Stiffness and Damping in Translation of each strut</a></li>
|
|
<li><a href="#org1e8eceb">Add Stiffness and Damping in Rotation of each strut</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org3a7f26e">11. <code>initializeInertialSensor</code>: Initialize the inertial sensor in each strut</a>
|
|
<ul>
|
|
<li><a href="#orgcfc37af">Geophone - Working Principle</a></li>
|
|
<li><a href="#org986e38f">Accelerometer - Working Principle</a></li>
|
|
<li><a href="#orgdf893fa">Function description</a></li>
|
|
<li><a href="#org1a86834">Optional Parameters</a></li>
|
|
<li><a href="#org1c3d7c8">Compute the properties of the sensor</a></li>
|
|
<li><a href="#org5ee57cc">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgd6baa46">12. <code>displayArchitecture</code>: 3D plot of the Stewart platform architecture</a>
|
|
<ul>
|
|
<li><a href="#org763a2e4">Function description</a></li>
|
|
<li><a href="#orgfefbc0b">Optional Parameters</a></li>
|
|
<li><a href="#org49ecb6b">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#orgb11fd92">Figure Creation, Frames and Homogeneous transformations</a></li>
|
|
<li><a href="#org7cd8fee">Fixed Base elements</a></li>
|
|
<li><a href="#orgacb8eb7">Mobile Platform elements</a></li>
|
|
<li><a href="#org7f9320b">Legs</a></li>
|
|
<li><a href="#org925a393">12.1. Figure parameters</a></li>
|
|
<li><a href="#org44e536d">12.2. Subplots</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgecfd55f">13. <code>describeStewartPlatform</code>: Display some text describing the current defined Stewart Platform</a>
|
|
<ul>
|
|
<li><a href="#orgea76862">Function description</a></li>
|
|
<li><a href="#org8ead6fb">Optional Parameters</a></li>
|
|
<li><a href="#org1d49caa">13.1. Geometry</a></li>
|
|
<li><a href="#orgcb66771">13.2. Actuators</a></li>
|
|
<li><a href="#org4630b77">13.3. Joints</a></li>
|
|
<li><a href="#org47a9cf0">13.4. Kinematics</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org65fc289">14. <code>generateCubicConfiguration</code>: Generate a Cubic Configuration</a>
|
|
<ul>
|
|
<li><a href="#org458448a">Function description</a></li>
|
|
<li><a href="#org5817617">Documentation</a></li>
|
|
<li><a href="#org56ed950">Optional Parameters</a></li>
|
|
<li><a href="#org341a5bd">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#orge94a885">Position of the Cube</a></li>
|
|
<li><a href="#orgc9244b9">Compute the pose</a></li>
|
|
<li><a href="#org503addd">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org9e8cbfa">15. <code>computeJacobian</code>: Compute the Jacobian Matrix</a>
|
|
<ul>
|
|
<li><a href="#orga999616">Function description</a></li>
|
|
<li><a href="#orgac50fa7">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#org9bcd9b9">Compute Jacobian Matrix</a></li>
|
|
<li><a href="#orgf08eda6">Compute Stiffness Matrix</a></li>
|
|
<li><a href="#orgd164132">Compute Compliance Matrix</a></li>
|
|
<li><a href="#orgd7c92be">Populate the <code>stewart</code> structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org03168fc">16. <code>inverseKinematics</code>: Compute Inverse Kinematics</a>
|
|
<ul>
|
|
<li><a href="#orgbdc5fb1">Theory</a></li>
|
|
<li><a href="#orgd247ad5">Function description</a></li>
|
|
<li><a href="#org3259665">Optional Parameters</a></li>
|
|
<li><a href="#org4ca8cd1">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#org8b70a76">Compute</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org278d55b">17. <code>forwardKinematicsApprox</code>: Compute the Approximate Forward Kinematics</a>
|
|
<ul>
|
|
<li><a href="#org7d81110">Function description</a></li>
|
|
<li><a href="#orgada2d6f">Optional Parameters</a></li>
|
|
<li><a href="#org02f1320">Check the <code>stewart</code> structure elements</a></li>
|
|
<li><a href="#orgf17cab9">Computation</a></li>
|
|
</ul>
|
|
</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<p>
|
|
Stewart platforms are generated in multiple steps.
|
|
</p>
|
|
|
|
<p>
|
|
We define 4 important <b>frames</b>:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(\{F\}\): Frame fixed to the <b>Fixed</b> base and located at the center of its bottom surface.
|
|
This is used to fix the Stewart platform to some support.</li>
|
|
<li>\(\{M\}\): Frame fixed to the <b>Moving</b> platform and located at the center of its top surface.
|
|
This is used to place things on top of the Stewart platform.</li>
|
|
<li>\(\{A\}\): Frame fixed to the fixed base.
|
|
It defined the center of rotation of the moving platform.</li>
|
|
<li>\(\{B\}\): Frame fixed to the moving platform.
|
|
The motion of the moving platforms and forces applied to it are defined with respect to this frame \(\{B\}\).</li>
|
|
</ul>
|
|
|
|
<p>
|
|
Then, we define the <b>location of the spherical joints</b>:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(\bm{a}_{i}\) are the position of the spherical joints fixed to the fixed base</li>
|
|
<li>\(\bm{b}_{i}\) are the position of the spherical joints fixed to the moving platform</li>
|
|
</ul>
|
|
|
|
<p>
|
|
We define the <b>rest position</b> of the Stewart platform:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>For simplicity, we suppose that the fixed base and the moving platform are parallel and aligned with the vertical axis at their rest position.</li>
|
|
<li>Thus, to define the rest position of the Stewart platform, we just have to defined its total height \(H\).
|
|
\(H\) corresponds to the distance from the bottom of the fixed base to the top of the moving platform.</li>
|
|
</ul>
|
|
|
|
<p>
|
|
From \(\bm{a}_{i}\) and \(\bm{b}_{i}\), we can determine the <b>length and orientation of each strut</b>:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(l_{i}\) is the length of the strut</li>
|
|
<li>\({}^{A}\hat{\bm{s}}_{i}\) is the unit vector align with the strut</li>
|
|
</ul>
|
|
|
|
<p>
|
|
The position of the Spherical joints can be computed using various methods:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>Cubic configuration</li>
|
|
<li>Circular configuration</li>
|
|
<li>Arbitrary position</li>
|
|
<li>These methods should be easily scriptable and corresponds to specific functions that returns \({}^{F}\bm{a}_{i}\) and \({}^{M}\bm{b}_{i}\).
|
|
The input of these functions are the parameters corresponding to the wanted geometry.</li>
|
|
</ul>
|
|
|
|
<p>
|
|
For Simscape, we need:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>The position and orientation of each spherical joint fixed to the fixed base: \({}^{F}\bm{a}_{i}\) and \({}^{F}\bm{R}_{a_{i}}\)</li>
|
|
<li>The position and orientation of each spherical joint fixed to the moving platform: \({}^{M}\bm{b}_{i}\) and \({}^{M}\bm{R}_{b_{i}}\)</li>
|
|
<li>The rest length of each strut: \(l_{i}\)</li>
|
|
<li>The stiffness and damping of each actuator: \(k_{i}\) and \(c_{i}\)</li>
|
|
<li>The position of the frame \(\{A\}\) with respect to the frame \(\{F\}\): \({}^{F}\bm{O}_{A}\)</li>
|
|
<li>The position of the frame \(\{B\}\) with respect to the frame \(\{M\}\): \({}^{M}\bm{O}_{B}\)</li>
|
|
</ul>
|
|
|
|
<div id="outline-container-orgcaca5e0" class="outline-2">
|
|
<h2 id="orgcaca5e0"><span class="section-number-2">1</span> <code>initializeStewartPlatform</code>: Initialize the Stewart Platform structure</h2>
|
|
<div class="outline-text-2" id="text-1">
|
|
<p>
|
|
<a id="orgcac119a"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeStewartPlatform.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc11609b" class="outline-3">
|
|
<h3 id="orgc11609b">Documentation</h3>
|
|
<div class="outline-text-3" id="text-orgc11609b">
|
|
|
|
<div id="orgebd0f49" 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-org37e6edf" class="outline-3">
|
|
<h3 id="org37e6edf">Function description</h3>
|
|
<div class="outline-text-3" id="text-org37e6edf">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = initializeStewartPlatform()
|
|
% initializeStewartPlatform - Initialize the stewart structure
|
|
%
|
|
% Syntax: [stewart] = initializeStewartPlatform(args)
|
|
%
|
|
% Outputs:
|
|
% - stewart - A structure with the following sub-structures:
|
|
% - platform_F -
|
|
% - platform_M -
|
|
% - joints_F -
|
|
% - joints_M -
|
|
% - struts_F -
|
|
% - struts_M -
|
|
% - actuators -
|
|
% - geometry -
|
|
% - properties -
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3622825" class="outline-3">
|
|
<h3 id="org3622825">Initialize the Stewart structure</h3>
|
|
<div class="outline-text-3" id="text-org3622825">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart = struct();
|
|
stewart.platform_F = struct();
|
|
stewart.platform_M = struct();
|
|
stewart.joints_F = struct();
|
|
stewart.joints_M = struct();
|
|
stewart.struts_F = struct();
|
|
stewart.struts_M = struct();
|
|
stewart.actuators = struct();
|
|
stewart.sensors = struct();
|
|
stewart.sensors.inertial = struct();
|
|
stewart.sensors.force = struct();
|
|
stewart.sensors.relative = struct();
|
|
stewart.geometry = struct();
|
|
stewart.kinematics = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgac25f89" class="outline-2">
|
|
<h2 id="orgac25f89"><span class="section-number-2">2</span> <code>initializeFramesPositions</code>: Initialize the positions of frames {A}, {B}, {F} and {M}</h2>
|
|
<div class="outline-text-2" id="text-2">
|
|
<p>
|
|
<a id="orga56a5c6"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeFramesPositions.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org2011b55" class="outline-3">
|
|
<h3 id="org2011b55">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org2011b55">
|
|
|
|
<div id="orgb66cd49" 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-org40fe9e8" class="outline-3">
|
|
<h3 id="org40fe9e8">Function description</h3>
|
|
<div class="outline-text-3" id="text-org40fe9e8">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = initializeFramesPositions(stewart, args)
|
|
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
|
%
|
|
% Syntax: [stewart] = initializeFramesPositions(stewart, args)
|
|
%
|
|
% Inputs:
|
|
% - args - Can have the following fields:
|
|
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
|
|
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
|
|
%
|
|
% Outputs:
|
|
% - stewart - A structure with the following fields:
|
|
% - geometry.H [1x1] - Total Height of the Stewart Platform [m]
|
|
% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]
|
|
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
|
|
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org6441308" class="outline-3">
|
|
<h3 id="org6441308">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org6441308">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
|
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7d50d54" class="outline-3">
|
|
<h3 id="org7d50d54">Compute the position of each frame</h3>
|
|
<div class="outline-text-3" id="text-org7d50d54">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">H = args.H; % Total Height of the Stewart Platform [m]
|
|
|
|
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
|
|
|
|
MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
|
|
|
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf6ce3ee" class="outline-3">
|
|
<h3 id="orgf6ce3ee">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orgf6ce3ee">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.geometry.H = H;
|
|
stewart.geometry.FO_M = FO_M;
|
|
stewart.platform_M.MO_B = MO_B;
|
|
stewart.platform_F.FO_A = FO_A;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgccb31c6" class="outline-2">
|
|
<h2 id="orgccb31c6"><span class="section-number-2">3</span> <code>generateGeneralConfiguration</code>: Generate a Very General Configuration</h2>
|
|
<div class="outline-text-2" id="text-3">
|
|
<p>
|
|
<a id="org32105b0"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/generateGeneralConfiguration.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org0759816" class="outline-3">
|
|
<h3 id="org0759816">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org0759816">
|
|
<p>
|
|
Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}.
|
|
The radius of the circles can be chosen as well as the angles where the joints are located (see Figure <a href="#org449c886">3</a>).
|
|
</p>
|
|
|
|
|
|
<div id="org449c886" class="figure">
|
|
<p><img src="figs/stewart_bottom_plate.png" alt="stewart_bottom_plate.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 3: </span>Position of the joints</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgaac37b6" class="outline-3">
|
|
<h3 id="orgaac37b6">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgaac37b6">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = generateGeneralConfiguration(stewart, args)
|
|
% generateGeneralConfiguration - Generate a Very General Configuration
|
|
%
|
|
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
|
|
%
|
|
% Inputs:
|
|
% - args - Can have the following fields:
|
|
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
|
|
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
|
|
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
|
|
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
|
|
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
|
|
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
|
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2af2f34" class="outline-3">
|
|
<h3 id="org2af2f34">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org2af2f34">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
|
|
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
|
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfac23f3" class="outline-3">
|
|
<h3 id="orgfac23f3">Compute the pose</h3>
|
|
<div class="outline-text-3" id="text-orgfac23f3">
|
|
<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">for i = 1:6
|
|
Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
|
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org69f4916" class="outline-3">
|
|
<h3 id="org69f4916">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org69f4916">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.platform_F.Fa = Fa;
|
|
stewart.platform_M.Mb = Mb;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9944c04" class="outline-2">
|
|
<h2 id="org9944c04"><span class="section-number-2">4</span> <code>computeJointsPose</code>: Compute the Pose of the Joints</h2>
|
|
<div class="outline-text-2" id="text-4">
|
|
<p>
|
|
<a id="orgd0bee51"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/computeJointsPose.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org2d1a6c0" class="outline-3">
|
|
<h3 id="org2d1a6c0">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org2d1a6c0">
|
|
|
|
<div id="org20f7106" class="figure">
|
|
<p><img src="figs/stewart-struts.png" alt="stewart-struts.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 4: </span>Position and orientation of the struts</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org50f74a1" class="outline-3">
|
|
<h3 id="org50f74a1">Function description</h3>
|
|
<div class="outline-text-3" id="text-org50f74a1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = computeJointsPose(stewart)
|
|
% computeJointsPose -
|
|
%
|
|
% Syntax: [stewart] = computeJointsPose(stewart)
|
|
%
|
|
% Inputs:
|
|
% - stewart - A structure with the following fields
|
|
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
|
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
|
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
|
|
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
|
|
% - geometry.FO_M [3x1] - Position of {M} with respect to {F}
|
|
%
|
|
% Outputs:
|
|
% - stewart - A structure with the following added fields
|
|
% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
|
% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
|
% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
|
% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
|
% - geometry.l [6x1] - The i'th element is the initial length of strut i
|
|
% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
|
% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
|
% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut
|
|
% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut
|
|
% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
|
% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf462a70" class="outline-3">
|
|
<h3 id="orgf462a70">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-orgf462a70">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
|
|
Fa = stewart.platform_F.Fa;
|
|
|
|
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
|
|
Mb = stewart.platform_M.Mb;
|
|
|
|
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
|
|
FO_A = stewart.platform_F.FO_A;
|
|
|
|
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
|
|
MO_B = stewart.platform_M.MO_B;
|
|
|
|
assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M')
|
|
FO_M = stewart.geometry.FO_M;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge87b302" class="outline-3">
|
|
<h3 id="orge87b302">Compute the position of the Joints</h3>
|
|
<div class="outline-text-3" id="text-orge87b302">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Aa = Fa - repmat(FO_A, [1, 6]);
|
|
Bb = Mb - repmat(MO_B, [1, 6]);
|
|
|
|
Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
|
|
Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3a7e3c5" class="outline-3">
|
|
<h3 id="org3a7e3c5">Compute the strut length and orientation</h3>
|
|
<div class="outline-text-3" id="text-org3a7e3c5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
|
|
|
|
l = vecnorm(Ab - Aa)';
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Bs = (Bb - Ba)./vecnorm(Bb - Ba);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9e1258f" class="outline-3">
|
|
<h3 id="org9e1258f">Compute the orientation of the Joints</h3>
|
|
<div class="outline-text-3" id="text-org9e1258f">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">FRa = zeros(3,3,6);
|
|
MRb = zeros(3,3,6);
|
|
|
|
for i = 1:6
|
|
FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
|
|
FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
|
|
|
|
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
|
|
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2c160bb" class="outline-3">
|
|
<h3 id="org2c160bb">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org2c160bb">
|
|
<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/2;
|
|
stewart.struts_M.l = l/2;
|
|
|
|
stewart.platform_F.FRa = FRa;
|
|
stewart.platform_M.MRb = MRb;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1315282" class="outline-2">
|
|
<h2 id="org1315282"><span class="section-number-2">5</span> <code>initializeStewartPose</code>: Determine the initial stroke in each leg to have the wanted pose</h2>
|
|
<div class="outline-text-2" id="text-5">
|
|
<p>
|
|
<a id="org05598b5"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeStewartPose.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org1e2fafd" class="outline-3">
|
|
<h3 id="org1e2fafd">Function description</h3>
|
|
<div class="outline-text-3" id="text-org1e2fafd">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = initializeStewartPose(stewart, args)
|
|
% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
|
|
% It uses the inverse kinematic
|
|
%
|
|
% Syntax: [stewart] = initializeStewartPose(stewart, args)
|
|
%
|
|
% Inputs:
|
|
% - stewart - A structure with the following fields
|
|
% - Aa [3x6] - The positions ai expressed in {A}
|
|
% - Bb [3x6] - The positions bi expressed in {B}
|
|
% - args - Can have the following fields:
|
|
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - 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}
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org034bf20" class="outline-3">
|
|
<h3 id="org034bf20">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org034bf20">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbb9abb5" class="outline-3">
|
|
<h3 id="orgbb9abb5">Use the Inverse Kinematic function</h3>
|
|
<div class="outline-text-3" id="text-orgbb9abb5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf124a3e" class="outline-3">
|
|
<h3 id="orgf124a3e">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orgf124a3e">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.actuators.Leq = dLi;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4674203" class="outline-2">
|
|
<h2 id="org4674203"><span class="section-number-2">6</span> <code>initializeCylindricalPlatforms</code>: Initialize the geometry of the Fixed and Mobile Platforms</h2>
|
|
<div class="outline-text-2" id="text-6">
|
|
<p>
|
|
<a id="orgac77ed6"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeCylindricalPlatforms.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org84a14a5" class="outline-3">
|
|
<h3 id="org84a14a5">Function description</h3>
|
|
<div class="outline-text-3" id="text-org84a14a5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = initializeCylindricalPlatforms(stewart, args)
|
|
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
|
|
%
|
|
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
|
% - Fph [1x1] - Fixed Platform Height [m]
|
|
% - Fpr [1x1] - Fixed Platform Radius [m]
|
|
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
|
% - Mph [1x1] - Mobile Platform Height [m]
|
|
% - Mpr [1x1] - Mobile Platform Radius [m]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - platform_F [struct] - structure with the following fields:
|
|
% - type = 1
|
|
% - M [1x1] - Fixed Platform Mass [kg]
|
|
% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
|
% - H [1x1] - Fixed Platform Height [m]
|
|
% - R [1x1] - Fixed Platform Radius [m]
|
|
% - platform_M [struct] - structure with the following fields:
|
|
% - M [1x1] - Mobile Platform Mass [kg]
|
|
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
|
% - H [1x1] - Mobile Platform Height [m]
|
|
% - R [1x1] - Mobile Platform Radius [m]
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge580054" class="outline-3">
|
|
<h3 id="orge580054">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orge580054">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf654de0" class="outline-3">
|
|
<h3 id="orgf654de0">Compute the Inertia matrices of platforms</h3>
|
|
<div class="outline-text-3" id="text-orgf654de0">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
|
1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
|
1/2 *args.Fpm * args.Fpr^2]);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
|
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
|
1/2 *args.Mpm * args.Mpr^2]);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge34849b" class="outline-3">
|
|
<h3 id="orge34849b">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orge34849b">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.platform_F.type = 1;
|
|
|
|
stewart.platform_F.I = I_F;
|
|
stewart.platform_F.M = args.Fpm;
|
|
stewart.platform_F.R = args.Fpr;
|
|
stewart.platform_F.H = args.Fph;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.platform_M.type = 1;
|
|
|
|
stewart.platform_M.I = I_M;
|
|
stewart.platform_M.M = args.Mpm;
|
|
stewart.platform_M.R = args.Mpr;
|
|
stewart.platform_M.H = args.Mph;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb0a1d7b" class="outline-2">
|
|
<h2 id="orgb0a1d7b"><span class="section-number-2">7</span> <code>initializeCylindricalStruts</code>: Define the inertia of cylindrical struts</h2>
|
|
<div class="outline-text-2" id="text-7">
|
|
<p>
|
|
<a id="orgfd38bf8"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeCylindricalStruts.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org27c9756" class="outline-3">
|
|
<h3 id="org27c9756">Function description</h3>
|
|
<div class="outline-text-3" id="text-org27c9756">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = initializeCylindricalStruts(stewart, args)
|
|
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
|
|
%
|
|
% Syntax: [stewart] = initializeCylindricalStruts(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
|
|
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
|
|
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
|
|
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
|
|
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
|
|
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - struts_F [struct] - structure with the following fields:
|
|
% - M [6x1] - Mass of the Fixed part of the struts [kg]
|
|
% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
|
|
% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
|
% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
|
% - struts_M [struct] - structure with the following fields:
|
|
% - M [6x1] - Mass of the Mobile part of the struts [kg]
|
|
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
|
% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
|
% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org060b144" class="outline-3">
|
|
<h3 id="org060b144">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org060b144">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd943059" class="outline-3">
|
|
<h3 id="orgd943059">Compute the properties of the cylindrical struts</h3>
|
|
<div class="outline-text-3" id="text-orgd943059">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Fsm = ones(6,1).*args.Fsm;
|
|
Fsh = ones(6,1).*args.Fsh;
|
|
Fsr = ones(6,1).*args.Fsr;
|
|
|
|
Msm = ones(6,1).*args.Msm;
|
|
Msh = ones(6,1).*args.Msh;
|
|
Msr = ones(6,1).*args.Msr;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
|
|
I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
|
|
|
|
for i = 1:6
|
|
I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
|
1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
|
1/2 * Fsm(i) * Fsr(i)^2]);
|
|
|
|
I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
|
1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
|
1/2 * Msm(i) * Msr(i)^2]);
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5d7924e" class="outline-3">
|
|
<h3 id="org5d7924e">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org5d7924e">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.struts_M.type = 1;
|
|
|
|
stewart.struts_M.I = I_M;
|
|
stewart.struts_M.M = Msm;
|
|
stewart.struts_M.R = Msr;
|
|
stewart.struts_M.H = Msh;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.struts_F.type = 1;
|
|
|
|
stewart.struts_F.I = I_F;
|
|
stewart.struts_F.M = Fsm;
|
|
stewart.struts_F.R = Fsr;
|
|
stewart.struts_F.H = Fsh;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgae8d0dc" class="outline-2">
|
|
<h2 id="orgae8d0dc"><span class="section-number-2">8</span> <code>initializeStrutDynamics</code>: Add Stiffness and Damping properties of each strut</h2>
|
|
<div class="outline-text-2" id="text-8">
|
|
<p>
|
|
<a id="orgd55e892"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeStrutDynamics.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org70888ee" class="outline-3">
|
|
<h3 id="org70888ee">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org70888ee">
|
|
|
|
<div id="org99aef3e" class="figure">
|
|
<p><img src="figs/piezoelectric_stack.jpg" alt="piezoelectric_stack.jpg" width="500px" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 5: </span>Example of a piezoelectric stach actuator (PI)</p>
|
|
</div>
|
|
|
|
<p>
|
|
A simplistic model of such amplified actuator is shown in Figure <a href="#orgd4c4025">6</a> where:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(K\) represent the vertical stiffness of the actuator</li>
|
|
<li>\(C\) represent the vertical damping of the actuator</li>
|
|
<li>\(F\) represents the force applied by the actuator</li>
|
|
<li>\(F_{m}\) represents the total measured force</li>
|
|
<li>\(v_{m}\) represents the absolute velocity of the top part of the actuator</li>
|
|
<li>\(d_{m}\) represents the total relative displacement of the actuator</li>
|
|
</ul>
|
|
|
|
|
|
<div id="orgd4c4025" class="figure">
|
|
<p><img src="figs/actuator_model_simple.png" alt="actuator_model_simple.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 6: </span>Simple model of an Actuator</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2fd9286" class="outline-3">
|
|
<h3 id="org2fd9286">Function description</h3>
|
|
<div class="outline-text-3" id="text-org2fd9286">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = initializeStrutDynamics(stewart, args)
|
|
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
|
|
%
|
|
% Syntax: [stewart] = initializeStrutDynamics(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - K [6x1] - Stiffness of each strut [N/m]
|
|
% - C [6x1] - Damping of each strut [N/(m/s)]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - actuators.type = 1
|
|
% - actuators.K [6x1] - Stiffness of each strut [N/m]
|
|
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgebba910" class="outline-3">
|
|
<h3 id="orgebba910">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgebba910">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
|
|
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3c2e550" class="outline-3">
|
|
<h3 id="org3c2e550">Add Stiffness and Damping properties of each strut</h3>
|
|
<div class="outline-text-3" id="text-org3c2e550">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.actuators.type = 1;
|
|
|
|
stewart.actuators.K = args.K;
|
|
stewart.actuators.C = args.C;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org682a09c" class="outline-2">
|
|
<h2 id="org682a09c"><span class="section-number-2">9</span> <code>initializeAmplifiedStrutDynamics</code>: Add Stiffness and Damping properties of each strut for an amplified piezoelectric actuator</h2>
|
|
<div class="outline-text-2" id="text-9">
|
|
<p>
|
|
<a id="orga43e091"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeAmplifiedStrutDynamics.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org70c61fd" class="outline-3">
|
|
<h3 id="org70c61fd">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org70c61fd">
|
|
<p>
|
|
An amplified piezoelectric actuator is shown in Figure <a href="#orgab58ac0">7</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orgab58ac0" class="figure">
|
|
<p><img src="figs/amplified_piezo_with_displacement_sensor.jpg" alt="amplified_piezo_with_displacement_sensor.jpg" width="500px" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 7: </span>Example of an Amplified piezoelectric actuator with an integrated displacement sensor (Cedrat Technologies)</p>
|
|
</div>
|
|
|
|
<p>
|
|
A simplistic model of such amplified actuator is shown in Figure <a href="#org7ac3e95">8</a> where:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(K_{r}\) represent the vertical stiffness when the piezoelectric stack is removed</li>
|
|
<li>\(K_{a}\) is the vertical stiffness contribution of the piezoelectric stack</li>
|
|
<li>\(F_{i}\) represents the part of the piezoelectric stack that is used as a force actuator</li>
|
|
<li>\(F_{m,i}\) represents the remaining part of the piezoelectric stack that is used as a force sensor</li>
|
|
<li>\(v_{m,i}\) represents the absolute velocity of the top part of the actuator</li>
|
|
<li>\(d_{m,i}\) represents the total relative displacement of the actuator</li>
|
|
</ul>
|
|
|
|
|
|
<div id="org7ac3e95" class="figure">
|
|
<p><img src="figs/iff_1dof.png" alt="iff_1dof.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 8: </span>Model of an amplified actuator</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4b8bd84" class="outline-3">
|
|
<h3 id="org4b8bd84">Function description</h3>
|
|
<div class="outline-text-3" id="text-org4b8bd84">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = initializeAmplifiedStrutDynamics(stewart, args)
|
|
% initializeAmplifiedStrutDynamics - Add Stiffness and Damping properties of each strut
|
|
%
|
|
% Syntax: [stewart] = initializeAmplifiedStrutDynamics(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m]
|
|
% - Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)]
|
|
% - Kr [6x1] - Vertical (residual) stiffness when the piezoelectric stack is removed [N/m]
|
|
% - Cr [6x1] - Vertical (residual) damping when the piezoelectric stack is removed [N/(m/s)]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - actuators.type = 2
|
|
% - actuators.K [6x1] - Total Stiffness of each strut [N/m]
|
|
% - actuators.C [6x1] - Total Damping of each strut [N/(m/s)]
|
|
% - actuators.Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m]
|
|
% - actuators.Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)]
|
|
% - actuators.Kr [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m]
|
|
% - actuators.Cr [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)]
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org94cace2" class="outline-3">
|
|
<h3 id="org94cace2">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org94cace2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.Kr (6,1) double {mustBeNumeric, mustBeNonnegative} = 5e6*ones(6,1)
|
|
args.Cr (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
args.Ka (6,1) double {mustBeNumeric, mustBeNonnegative} = 15e6*ones(6,1)
|
|
args.Ca (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2e42182" class="outline-3">
|
|
<h3 id="org2e42182">Compute the total stiffness and damping</h3>
|
|
<div class="outline-text-3" id="text-org2e42182">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">K = args.Ka + args.Kr;
|
|
C = args.Ca + args.Cr;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgee22bea" class="outline-3">
|
|
<h3 id="orgee22bea">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orgee22bea">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.actuators.type = 2;
|
|
|
|
stewart.actuators.Ka = args.Ka;
|
|
stewart.actuators.Ca = args.Ca;
|
|
|
|
stewart.actuators.Kr = args.Kr;
|
|
stewart.actuators.Cr = args.Cr;
|
|
|
|
stewart.actuators.K = K;
|
|
stewart.actuators.C = K;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbc5232e" class="outline-2">
|
|
<h2 id="orgbc5232e"><span class="section-number-2">10</span> <code>initializeJointDynamics</code>: Add Stiffness and Damping properties for spherical joints</h2>
|
|
<div class="outline-text-2" id="text-10">
|
|
<p>
|
|
<a id="orga86aa00"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeJointDynamics.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc806fca" class="outline-3">
|
|
<h3 id="orgc806fca">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgc806fca">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = initializeJointDynamics(stewart, args)
|
|
% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
|
|
%
|
|
% Syntax: [stewart] = initializeJointDynamics(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
|
|
% - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'
|
|
% - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
|
|
% - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
|
|
% - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
|
|
% - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
|
|
% - Kz_M [6x1] - Translation (Tz) Stiffness for each top joints [N/m]
|
|
% - Cz_M [6x1] - Translation (Tz) Damping of each top joint [N/m]
|
|
% - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
|
|
% - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
|
|
% - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
|
|
% - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
|
|
% - Kz_F [6x1] - Translation (Tz) Stiffness for each bottom joints [N/m]
|
|
% - Cz_F [6x1] - Translation (Tz) Damping of each bottom joint [N/m]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - stewart.joints_F and stewart.joints_M:
|
|
% - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
|
|
% - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
|
|
% - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
|
|
% - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
|
|
% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
|
|
% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
|
|
% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4403345" class="outline-3">
|
|
<h3 id="org4403345">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org4403345">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof'})} = 'universal'
|
|
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'spherical_3dof'})} = 'spherical'
|
|
args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1)
|
|
args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
|
args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1)
|
|
args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
|
args.Kz_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 60e6*ones(6,1)
|
|
args.Cz_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e2*ones(6,1)
|
|
args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1)
|
|
args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
|
args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1)
|
|
args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
|
args.Kz_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 60e6*ones(6,1)
|
|
args.Cz_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e2*ones(6,1)
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd5b8278" class="outline-3">
|
|
<h3 id="orgd5b8278">Add Actuator Type</h3>
|
|
<div class="outline-text-3" id="text-orgd5b8278">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">switch args.type_F
|
|
case 'universal'
|
|
stewart.joints_F.type = 1;
|
|
case 'spherical'
|
|
stewart.joints_F.type = 2;
|
|
case 'universal_p'
|
|
stewart.joints_F.type = 3;
|
|
case 'spherical_p'
|
|
stewart.joints_F.type = 4;
|
|
case 'universal_3dof'
|
|
stewart.joints_F.type = 5;
|
|
end
|
|
|
|
switch args.type_M
|
|
case 'universal'
|
|
stewart.joints_M.type = 1;
|
|
case 'spherical'
|
|
stewart.joints_M.type = 2;
|
|
case 'universal_p'
|
|
stewart.joints_M.type = 3;
|
|
case 'spherical_p'
|
|
stewart.joints_M.type = 4;
|
|
case 'spherical_3dof'
|
|
stewart.joints_M.type = 6;
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org51cf135" class="outline-3">
|
|
<h3 id="org51cf135">Add Stiffness and Damping in Translation of each strut</h3>
|
|
<div class="outline-text-3" id="text-org51cf135">
|
|
<p>
|
|
Translation Stiffness
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.joints_M.Kx = zeros(6,1);
|
|
stewart.joints_M.Ky = zeros(6,1);
|
|
stewart.joints_M.Kz = args.Kz_M;
|
|
|
|
stewart.joints_F.Kx = zeros(6,1);
|
|
stewart.joints_F.Ky = zeros(6,1);
|
|
stewart.joints_F.Kz = args.Kz_F;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Translation Damping
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.joints_M.Cx = zeros(6,1);
|
|
stewart.joints_M.Cy = zeros(6,1);
|
|
stewart.joints_M.Cz = args.Cz_M;
|
|
|
|
stewart.joints_F.Cx = zeros(6,1);
|
|
stewart.joints_F.Cy = zeros(6,1);
|
|
stewart.joints_F.Cz = args.Cz_F;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1e8eceb" class="outline-3">
|
|
<h3 id="org1e8eceb">Add Stiffness and Damping in Rotation of each strut</h3>
|
|
<div class="outline-text-3" id="text-org1e8eceb">
|
|
<p>
|
|
Rotational Stiffness
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.joints_M.Kf = args.Kf_M;
|
|
stewart.joints_M.Kt = args.Kf_M;
|
|
|
|
stewart.joints_F.Kf = args.Kf_F;
|
|
stewart.joints_F.Kt = args.Kf_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.Cf_M;
|
|
|
|
stewart.joints_F.Cf = args.Cf_F;
|
|
stewart.joints_F.Ct = args.Cf_F;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3a7f26e" class="outline-2">
|
|
<h2 id="org3a7f26e"><span class="section-number-2">11</span> <code>initializeInertialSensor</code>: Initialize the inertial sensor in each strut</h2>
|
|
<div class="outline-text-2" id="text-11">
|
|
<p>
|
|
<a id="org10af194"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeInertialSensor.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgcfc37af" class="outline-3">
|
|
<h3 id="orgcfc37af">Geophone - Working Principle</h3>
|
|
<div class="outline-text-3" id="text-orgcfc37af">
|
|
<p>
|
|
From the schematic of the Z-axis geophone shown in Figure <a href="#orgcbee0e9">9</a>, we can write the transfer function from the support velocity \(\dot{w}\) to the relative velocity of the inertial mass \(\dot{d}\):
|
|
\[ \frac{\dot{d}}{\dot{w}} = \frac{-\frac{s^2}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
|
|
with:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(\omega_0 = \sqrt{\frac{k}{m}}\)</li>
|
|
<li>\(\xi = \frac{1}{2} \sqrt{\frac{m}{k}}\)</li>
|
|
</ul>
|
|
|
|
|
|
<div id="orgcbee0e9" class="figure">
|
|
<p><img src="figs/inertial_sensor.png" alt="inertial_sensor.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 9: </span>Schematic of a Z-Axis geophone</p>
|
|
</div>
|
|
|
|
<p>
|
|
We see that at frequencies above \(\omega_0\):
|
|
\[ \frac{\dot{d}}{\dot{w}} \approx -1 \]
|
|
</p>
|
|
|
|
<p>
|
|
And thus, the measurement of the relative velocity of the mass with respect to its support gives the absolute velocity of the support.
|
|
</p>
|
|
|
|
<p>
|
|
We generally want to have the smallest resonant frequency \(\omega_0\) to measure low frequency absolute velocity, however there is a trade-off between \(\omega_0\) and the mass of the inertial mass.
|
|
</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org986e38f" class="outline-3">
|
|
<h3 id="org986e38f">Accelerometer - Working Principle</h3>
|
|
<div class="outline-text-3" id="text-org986e38f">
|
|
<p>
|
|
From the schematic of the Z-axis accelerometer shown in Figure <a href="#orgf6281f4">10</a>, we can write the transfer function from the support acceleration \(\ddot{w}\) to the relative position of the inertial mass \(d\):
|
|
\[ \frac{d}{\ddot{w}} = \frac{-\frac{1}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
|
|
with:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(\omega_0 = \sqrt{\frac{k}{m}}\)</li>
|
|
<li>\(\xi = \frac{1}{2} \sqrt{\frac{m}{k}}\)</li>
|
|
</ul>
|
|
|
|
|
|
<div id="orgf6281f4" class="figure">
|
|
<p><img src="figs/inertial_sensor.png" alt="inertial_sensor.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 10: </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-orgdf893fa" class="outline-3">
|
|
<h3 id="orgdf893fa">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgdf893fa">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = initializeInertialSensor(stewart, args)
|
|
% initializeInertialSensor - Initialize the inertial sensor in each strut
|
|
%
|
|
% Syntax: [stewart] = initializeInertialSensor(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - type - 'geophone', 'accelerometer', 'none'
|
|
% - mass [1x1] - Weight of the inertial mass [kg]
|
|
% - freq [1x1] - Cutoff frequency [Hz]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - stewart.sensors.inertial
|
|
% - type - 1 (geophone), 2 (accelerometer), 3 (none)
|
|
% - K [1x1] - Stiffness [N/m]
|
|
% - C [1x1] - Damping [N/(m/s)]
|
|
% - M [1x1] - Inertial Mass [kg]
|
|
% - G [1x1] - Gain
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1a86834" class="outline-3">
|
|
<h3 id="org1a86834">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org1a86834">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.type char {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
|
|
args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
|
|
args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1c3d7c8" class="outline-3">
|
|
<h3 id="org1c3d7c8">Compute the properties of the sensor</h3>
|
|
<div class="outline-text-3" id="text-org1c3d7c8">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">sensor = struct();
|
|
|
|
switch args.type
|
|
case 'geophone'
|
|
sensor.type = 1;
|
|
|
|
sensor.M = args.mass;
|
|
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
|
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
|
case 'accelerometer'
|
|
sensor.type = 2;
|
|
|
|
sensor.M = args.mass;
|
|
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
|
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
|
sensor.G = -sensor.K/sensor.M;
|
|
case 'none'
|
|
sensor.type = 3;
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5ee57cc" class="outline-3">
|
|
<h3 id="org5ee57cc">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org5ee57cc">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.sensors.inertial = sensor;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd6baa46" class="outline-2">
|
|
<h2 id="orgd6baa46"><span class="section-number-2">12</span> <code>displayArchitecture</code>: 3D plot of the Stewart platform architecture</h2>
|
|
<div class="outline-text-2" id="text-12">
|
|
<p>
|
|
<a id="org455c4f1"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/displayArchitecture.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org763a2e4" class="outline-3">
|
|
<h3 id="org763a2e4">Function description</h3>
|
|
<div class="outline-text-3" id="text-org763a2e4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [] = displayArchitecture(stewart, args)
|
|
% displayArchitecture - 3D plot of the Stewart platform architecture
|
|
%
|
|
% Syntax: [] = displayArchitecture(args)
|
|
%
|
|
% Inputs:
|
|
% - stewart
|
|
% - args - Structure with the following fields:
|
|
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
|
% - F_color [color] - Color used for the Fixed elements
|
|
% - M_color [color] - Color used for the Mobile elements
|
|
% - L_color [color] - Color used for the Legs elements
|
|
% - frames [true/false] - Display the Frames
|
|
% - legs [true/false] - Display the Legs
|
|
% - joints [true/false] - Display the Joints
|
|
% - labels [true/false] - Display the Labels
|
|
% - platforms [true/false] - Display the Platforms
|
|
% - views ['all', 'xy', 'yz', 'xz', 'default'] -
|
|
%
|
|
% Outputs:
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfefbc0b" class="outline-3">
|
|
<h3 id="orgfefbc0b">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgfefbc0b">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
args.F_color = [0 0.4470 0.7410]
|
|
args.M_color = [0.8500 0.3250 0.0980]
|
|
args.L_color = [0 0 0]
|
|
args.frames logical {mustBeNumericOrLogical} = true
|
|
args.legs logical {mustBeNumericOrLogical} = true
|
|
args.joints logical {mustBeNumericOrLogical} = true
|
|
args.labels logical {mustBeNumericOrLogical} = true
|
|
args.platforms logical {mustBeNumericOrLogical} = true
|
|
args.views char {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default'
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org49ecb6b" class="outline-3">
|
|
<h3 id="org49ecb6b">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-org49ecb6b">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
|
|
FO_A = stewart.platform_F.FO_A;
|
|
|
|
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
|
|
MO_B = stewart.platform_M.MO_B;
|
|
|
|
assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H')
|
|
H = stewart.geometry.H;
|
|
|
|
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
|
|
Fa = stewart.platform_F.Fa;
|
|
|
|
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
|
|
Mb = stewart.platform_M.Mb;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-orgb11fd92" class="outline-3">
|
|
<h3 id="orgb11fd92">Figure Creation, Frames and Homogeneous transformations</h3>
|
|
<div class="outline-text-3" id="text-orgb11fd92">
|
|
<p>
|
|
The reference frame of the 3d plot corresponds to the frame \(\{F\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">if ~strcmp(args.views, 'all')
|
|
figure;
|
|
else
|
|
f = figure('visible', 'off');
|
|
end
|
|
|
|
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(1,3), 1];
|
|
ATb = [args.ARB, args.AP; ...
|
|
zeros(1,3), 1];
|
|
BTm = [eye(3), -MO_B; ...
|
|
zeros(1,3), 1];
|
|
|
|
FTm = FTa*ATb*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/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/20;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7cd8fee" class="outline-3">
|
|
<h3 id="org7cd8fee">Fixed Base elements</h3>
|
|
<div class="outline-text-3" id="text-org7cd8fee">
|
|
<p>
|
|
Let’s first plot the frame \(\{F\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Ff = [0, 0, 0];
|
|
if args.frames
|
|
quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ...
|
|
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
|
|
|
|
if args.labels
|
|
text(Ff(1) + d_label, ...
|
|
Ff(2) + d_label, ...
|
|
Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color);
|
|
end
|
|
end
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Now plot the frame \(\{A\}\) fixed to the Base.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">if args.frames
|
|
quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ...
|
|
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
|
|
|
|
if args.labels
|
|
text(FO_A(1) + d_label, ...
|
|
FO_A(2) + d_label, ...
|
|
FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color);
|
|
end
|
|
end
|
|
</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">if args.platforms && stewart.platform_F.type == 1
|
|
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
|
|
v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
|
|
center = [0; 0; 0]; % Center of the circle
|
|
radius = stewart.platform_F.R; % Radius of the circle [m]
|
|
|
|
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
|
|
|
|
plot3(points(1,:), ...
|
|
points(2,:), ...
|
|
points(3,:), '-', 'Color', args.F_color);
|
|
end
|
|
</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">if args.joints
|
|
scatter3(Fa(1,:), ...
|
|
Fa(2,:), ...
|
|
Fa(3,:), 'MarkerEdgeColor', args.F_color);
|
|
if args.labels
|
|
for i = 1:size(Fa,2)
|
|
text(Fa(1,i) + d_label, ...
|
|
Fa(2,i), ...
|
|
Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
|
|
end
|
|
end
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgacb8eb7" class="outline-3">
|
|
<h3 id="orgacb8eb7">Mobile Platform elements</h3>
|
|
<div class="outline-text-3" id="text-orgacb8eb7">
|
|
<p>
|
|
Plot the frame \(\{M\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F}
|
|
|
|
if args.frames
|
|
FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
|
|
quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ...
|
|
FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color)
|
|
|
|
if args.labels
|
|
text(Fm(1) + d_label, ...
|
|
Fm(2) + d_label, ...
|
|
Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color);
|
|
end
|
|
end
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Plot the frame \(\{B\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">FB = FO_A + args.AP;
|
|
|
|
if args.frames
|
|
FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
|
|
quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ...
|
|
FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color)
|
|
|
|
if args.labels
|
|
text(FB(1) - d_label, ...
|
|
FB(2) + d_label, ...
|
|
FB(3) + d_label, '$\{B\}$', 'Color', args.M_color);
|
|
end
|
|
end
|
|
</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">if args.platforms && stewart.platform_M.type == 1
|
|
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
|
|
v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
|
|
center = Fm(1:3); % Center of the circle
|
|
radius = stewart.platform_M.R; % Radius of the circle [m]
|
|
|
|
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
|
|
|
|
plot3(points(1,:), ...
|
|
points(2,:), ...
|
|
points(3,:), '-', 'Color', args.M_color);
|
|
end
|
|
</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">if args.joints
|
|
Fb = FTm*[Mb;ones(1,6)];
|
|
|
|
scatter3(Fb(1,:), ...
|
|
Fb(2,:), ...
|
|
Fb(3,:), 'MarkerEdgeColor', args.M_color);
|
|
|
|
if args.labels
|
|
for i = 1:size(Fb,2)
|
|
text(Fb(1,i) + d_label, ...
|
|
Fb(2,i), ...
|
|
Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color);
|
|
end
|
|
end
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7f9320b" class="outline-3">
|
|
<h3 id="org7f9320b">Legs</h3>
|
|
<div class="outline-text-3" id="text-org7f9320b">
|
|
<p>
|
|
Plot the legs connecting the joints of the fixed base to the joints of the mobile platform.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">if args.legs
|
|
for i = 1:6
|
|
plot3([Fa(1,i), Fb(1,i)], ...
|
|
[Fa(2,i), Fb(2,i)], ...
|
|
[Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
|
|
|
|
if args.labels
|
|
text((Fa(1,i)+Fb(1,i))/2 + d_label, ...
|
|
(Fa(2,i)+Fb(2,i))/2, ...
|
|
(Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
|
|
end
|
|
end
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org925a393" class="outline-3">
|
|
<h3 id="org925a393"><span class="section-number-3">12.1</span> Figure parameters</h3>
|
|
<div class="outline-text-3" id="text-12-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">switch args.views
|
|
case 'default'
|
|
view([1 -0.6 0.4]);
|
|
case 'xy'
|
|
view([0 0 1]);
|
|
case 'xz'
|
|
view([0 -1 0]);
|
|
case 'yz'
|
|
view([1 0 0]);
|
|
end
|
|
axis equal;
|
|
axis off;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org44e536d" class="outline-3">
|
|
<h3 id="org44e536d"><span class="section-number-3">12.2</span> Subplots</h3>
|
|
<div class="outline-text-3" id="text-12-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">if strcmp(args.views, 'all')
|
|
hAx = findobj('type', 'axes');
|
|
|
|
figure;
|
|
s1 = subplot(2,2,1);
|
|
copyobj(get(hAx(1), 'Children'), s1);
|
|
view([0 0 1]);
|
|
axis equal;
|
|
axis off;
|
|
title('Top')
|
|
|
|
s2 = subplot(2,2,2);
|
|
copyobj(get(hAx(1), 'Children'), s2);
|
|
view([1 -0.6 0.4]);
|
|
axis equal;
|
|
axis off;
|
|
|
|
s3 = subplot(2,2,3);
|
|
copyobj(get(hAx(1), 'Children'), s3);
|
|
view([1 0 0]);
|
|
axis equal;
|
|
axis off;
|
|
title('Front')
|
|
|
|
s4 = subplot(2,2,4);
|
|
copyobj(get(hAx(1), 'Children'), s4);
|
|
view([0 -1 0]);
|
|
axis equal;
|
|
axis off;
|
|
title('Side')
|
|
|
|
close(f);
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-orgecfd55f" class="outline-2">
|
|
<h2 id="orgecfd55f"><span class="section-number-2">13</span> <code>describeStewartPlatform</code>: Display some text describing the current defined Stewart Platform</h2>
|
|
<div class="outline-text-2" id="text-13">
|
|
<p>
|
|
<a id="org8cc8939"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/describeStewartPlatform.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgea76862" class="outline-3">
|
|
<h3 id="orgea76862">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgea76862">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [] = describeStewartPlatform(stewart)
|
|
% describeStewartPlatform - Display some text describing the current defined Stewart Platform
|
|
%
|
|
% Syntax: [] = describeStewartPlatform(args)
|
|
%
|
|
% Inputs:
|
|
% - stewart
|
|
%
|
|
% Outputs:
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8ead6fb" class="outline-3">
|
|
<h3 id="org8ead6fb">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org8ead6fb">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1d49caa" class="outline-3">
|
|
<h3 id="org1d49caa"><span class="section-number-3">13.1</span> Geometry</h3>
|
|
<div class="outline-text-3" id="text-13-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('GEOMETRY:\n')
|
|
fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H)
|
|
|
|
if stewart.platform_M.MO_B(3) > 0
|
|
fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n', 1e3*stewart.platform_M.MO_B(3))
|
|
else
|
|
fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3))
|
|
end
|
|
|
|
fprintf('- The initial length of the struts are:\n')
|
|
fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l)
|
|
fprintf('\n')
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgcb66771" class="outline-3">
|
|
<h3 id="orgcb66771"><span class="section-number-3">13.2</span> Actuators</h3>
|
|
<div class="outline-text-3" id="text-13-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('ACTUATORS:\n')
|
|
if stewart.actuators.type == 1
|
|
fprintf('- The actuators are classical.\n')
|
|
fprintf('- The Stiffness and Damping of each actuators is:\n')
|
|
fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.K(1), stewart.actuators.C(1))
|
|
elseif stewart.actuators.type == 2
|
|
fprintf('- The actuators are mechanicaly amplified.\n')
|
|
fprintf('- The vertical stiffness and damping contribution of the piezoelectric stack is:\n')
|
|
fprintf('\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n', stewart.actuators.Ka(1), stewart.actuators.Ca(1))
|
|
fprintf('- Vertical stiffness when the piezoelectric stack is removed is:\n')
|
|
fprintf('\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n', stewart.actuators.Kr(1), stewart.actuators.Cr(1))
|
|
end
|
|
fprintf('\n')
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4630b77" class="outline-3">
|
|
<h3 id="org4630b77"><span class="section-number-3">13.3</span> Joints</h3>
|
|
<div class="outline-text-3" id="text-13-3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('JOINTS:\n')
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Type of the joints on the fixed base.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">switch stewart.joints_F.type
|
|
case 1
|
|
fprintf('- The joints on the fixed based are universal joints\n')
|
|
case 2
|
|
fprintf('- The joints on the fixed based are spherical joints\n')
|
|
case 3
|
|
fprintf('- The joints on the fixed based are perfect universal joints\n')
|
|
case 4
|
|
fprintf('- The joints on the fixed based are perfect spherical joints\n')
|
|
end
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Type of the joints on the mobile platform.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">switch stewart.joints_M.type
|
|
case 1
|
|
fprintf('- The joints on the mobile based are universal joints\n')
|
|
case 2
|
|
fprintf('- The joints on the mobile based are spherical joints\n')
|
|
case 3
|
|
fprintf('- The joints on the mobile based are perfect universal joints\n')
|
|
case 4
|
|
fprintf('- The joints on the mobile based are perfect spherical joints\n')
|
|
end
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Position of the fixed joints
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n')
|
|
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa)
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Position of the mobile joints
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n')
|
|
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb)
|
|
fprintf('\n')
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org47a9cf0" class="outline-3">
|
|
<h3 id="org47a9cf0"><span class="section-number-3">13.4</span> Kinematics</h3>
|
|
<div class="outline-text-3" id="text-13-4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('KINEMATICS:\n')
|
|
|
|
if isfield(stewart.kinematics, 'K')
|
|
fprintf('- The Stiffness matrix K is (in [N/m]):\n')
|
|
fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K)
|
|
end
|
|
|
|
if isfield(stewart.kinematics, 'C')
|
|
fprintf('- The Damping matrix C is (in [m/N]):\n')
|
|
fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C)
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org65fc289" class="outline-2">
|
|
<h2 id="org65fc289"><span class="section-number-2">14</span> <code>generateCubicConfiguration</code>: Generate a Cubic Configuration</h2>
|
|
<div class="outline-text-2" id="text-14">
|
|
<p>
|
|
<a id="org677ea95"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/generateCubicConfiguration.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org458448a" class="outline-3">
|
|
<h3 id="org458448a">Function description</h3>
|
|
<div class="outline-text-3" id="text-org458448a">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = generateCubicConfiguration(stewart, args)
|
|
% generateCubicConfiguration - Generate a Cubic Configuration
|
|
%
|
|
% Syntax: [stewart] = generateCubicConfiguration(stewart, args)
|
|
%
|
|
% Inputs:
|
|
% - stewart - A structure with the following fields
|
|
% - geometry.H [1x1] - Total height of the platform [m]
|
|
% - args - Can have the following fields:
|
|
% - Hc [1x1] - Height of the "useful" part of the cube [m]
|
|
% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
|
|
% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
|
|
% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
|
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5817617" class="outline-3">
|
|
<h3 id="org5817617">Documentation</h3>
|
|
<div class="outline-text-3" id="text-org5817617">
|
|
|
|
<div id="org70070f0" class="figure">
|
|
<p><img src="figs/cubic-configuration-definition.png" alt="cubic-configuration-definition.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 11: </span>Cubic Configuration</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org56ed950" class="outline-3">
|
|
<h3 id="org56ed950">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org56ed950">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
|
|
args.FOc (1,1) double {mustBeNumeric} = 50e-3
|
|
args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
|
|
args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org341a5bd" class="outline-3">
|
|
<h3 id="org341a5bd">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-org341a5bd">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H')
|
|
H = stewart.geometry.H;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge94a885" class="outline-3">
|
|
<h3 id="orge94a885">Position of the Cube</h3>
|
|
<div class="outline-text-3" id="text-orge94a885">
|
|
<p>
|
|
We define the useful points of the cube with respect to the Cube’s center.
|
|
\({}^{C}C\) are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">sx = [ 2; -1; -1];
|
|
sy = [ 0; 1; -1];
|
|
sz = [ 1; 1; 1];
|
|
|
|
R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
|
|
|
|
L = args.Hc*sqrt(3);
|
|
|
|
Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc];
|
|
|
|
CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
|
|
CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc9244b9" class="outline-3">
|
|
<h3 id="orgc9244b9">Compute the pose</h3>
|
|
<div class="outline-text-3" id="text-orgc9244b9">
|
|
<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 - CCf)./vecnorm(CCm - 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 + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
|
Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org503addd" class="outline-3">
|
|
<h3 id="org503addd">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-org503addd">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.platform_F.Fa = Fa;
|
|
stewart.platform_M.Mb = Mb;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9e8cbfa" class="outline-2">
|
|
<h2 id="org9e8cbfa"><span class="section-number-2">15</span> <code>computeJacobian</code>: Compute the Jacobian Matrix</h2>
|
|
<div class="outline-text-2" id="text-15">
|
|
<p>
|
|
<a id="orgfb113f5"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/computeJacobian.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orga999616" class="outline-3">
|
|
<h3 id="orga999616">Function description</h3>
|
|
<div class="outline-text-3" id="text-orga999616">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [stewart] = computeJacobian(stewart)
|
|
% computeJacobian -
|
|
%
|
|
% Syntax: [stewart] = computeJacobian(stewart)
|
|
%
|
|
% Inputs:
|
|
% - stewart - With at least the following fields:
|
|
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
|
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
|
% - actuators.K [6x1] - Total stiffness of the actuators
|
|
%
|
|
% Outputs:
|
|
% - stewart - With the 3 added field:
|
|
% - kinematics.J [6x6] - The Jacobian Matrix
|
|
% - kinematics.K [6x6] - The Stiffness Matrix
|
|
% - kinematics.C [6x6] - The Compliance Matrix
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgac50fa7" class="outline-3">
|
|
<h3 id="orgac50fa7">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-orgac50fa7">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
|
As = stewart.geometry.As;
|
|
|
|
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
|
Ab = stewart.geometry.Ab;
|
|
|
|
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
|
|
Ki = stewart.actuators.K;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-org9bcd9b9" class="outline-3">
|
|
<h3 id="org9bcd9b9">Compute Jacobian Matrix</h3>
|
|
<div class="outline-text-3" id="text-org9bcd9b9">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">J = [As' , cross(Ab, As)'];
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf08eda6" class="outline-3">
|
|
<h3 id="orgf08eda6">Compute Stiffness Matrix</h3>
|
|
<div class="outline-text-3" id="text-orgf08eda6">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">K = J'*diag(Ki)*J;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd164132" class="outline-3">
|
|
<h3 id="orgd164132">Compute Compliance Matrix</h3>
|
|
<div class="outline-text-3" id="text-orgd164132">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">C = inv(K);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd7c92be" class="outline-3">
|
|
<h3 id="orgd7c92be">Populate the <code>stewart</code> structure</h3>
|
|
<div class="outline-text-3" id="text-orgd7c92be">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart.kinematics.J = J;
|
|
stewart.kinematics.K = K;
|
|
stewart.kinematics.C = C;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-org03168fc" class="outline-2">
|
|
<h2 id="org03168fc"><span class="section-number-2">16</span> <code>inverseKinematics</code>: Compute Inverse Kinematics</h2>
|
|
<div class="outline-text-2" id="text-16">
|
|
<p>
|
|
<a id="org681bcb5"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/inverseKinematics.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbdc5fb1" class="outline-3">
|
|
<h3 id="orgbdc5fb1">Theory</h3>
|
|
<div class="outline-text-3" id="text-orgbdc5fb1">
|
|
<p>
|
|
For inverse kinematic analysis, it is assumed that the position \({}^A\bm{P}\) and orientation of the moving platform \({}^A\bm{R}_B\) are given and the problem is to obtain the joint variables, namely, \(\bm{L} = [l_1, l_2, \dots, l_6]^T\).
|
|
</p>
|
|
|
|
<p>
|
|
From the geometry of the manipulator, the loop closure for each limb, \(i = 1, 2, \dots, 6\) can be written as
|
|
</p>
|
|
\begin{align*}
|
|
l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
|
|
&= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
|
|
\end{align*}
|
|
|
|
<p>
|
|
To obtain the length of each actuator and eliminate \(\hat{\bm{s}}_i\), it is sufficient to dot multiply each side by itself:
|
|
</p>
|
|
\begin{equation}
|
|
l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
|
|
\end{equation}
|
|
|
|
<p>
|
|
Hence, for \(i = 1, 2, \dots, 6\), each limb length can be uniquely determined by:
|
|
</p>
|
|
\begin{equation}
|
|
l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
|
|
\end{equation}
|
|
|
|
<p>
|
|
If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
|
|
Otherwise, when the limbs’ lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
|
|
</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd247ad5" class="outline-3">
|
|
<h3 id="orgd247ad5">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgd247ad5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [Li, dLi] = inverseKinematics(stewart, args)
|
|
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
|
%
|
|
% Syntax: [stewart] = inverseKinematics(stewart)
|
|
%
|
|
% Inputs:
|
|
% - stewart - A structure with the following fields
|
|
% - geometry.Aa [3x6] - The positions ai expressed in {A}
|
|
% - geometry.Bb [3x6] - The positions bi expressed in {B}
|
|
% - geometry.l [6x1] - Length of each strut
|
|
% - args - Can have the following fields:
|
|
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
|
%
|
|
% Outputs:
|
|
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
|
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3259665" class="outline-3">
|
|
<h3 id="org3259665">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org3259665">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4ca8cd1" class="outline-3">
|
|
<h3 id="org4ca8cd1">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-org4ca8cd1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
|
|
Aa = stewart.geometry.Aa;
|
|
|
|
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
|
|
Bb = stewart.geometry.Bb;
|
|
|
|
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
|
|
l = stewart.geometry.l;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-org8b70a76" class="outline-3">
|
|
<h3 id="org8b70a76">Compute</h3>
|
|
<div class="outline-text-3" id="text-org8b70a76">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">dLi = Li-l;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org278d55b" class="outline-2">
|
|
<h2 id="org278d55b"><span class="section-number-2">17</span> <code>forwardKinematicsApprox</code>: Compute the Approximate Forward Kinematics</h2>
|
|
<div class="outline-text-2" id="text-17">
|
|
<p>
|
|
<a id="org5b15db4"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/forwardKinematicsApprox.m">here</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org7d81110" class="outline-3">
|
|
<h3 id="org7d81110">Function description</h3>
|
|
<div class="outline-text-3" id="text-org7d81110">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [P, R] = forwardKinematicsApprox(stewart, args)
|
|
% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
|
|
% the Jacobian Matrix
|
|
%
|
|
% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
|
|
%
|
|
% Inputs:
|
|
% - stewart - A structure with the following fields
|
|
% - kinematics.J [6x6] - The Jacobian Matrix
|
|
% - args - Can have the following fields:
|
|
% - dL [6x1] - Displacement of each strut [m]
|
|
%
|
|
% Outputs:
|
|
% - P [3x1] - The estimated position of {B} with respect to {A}
|
|
% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgada2d6f" class="outline-3">
|
|
<h3 id="orgada2d6f">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgada2d6f">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">arguments
|
|
stewart
|
|
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org02f1320" class="outline-3">
|
|
<h3 id="org02f1320">Check the <code>stewart</code> structure elements</h3>
|
|
<div class="outline-text-3" id="text-org02f1320">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J')
|
|
J = stewart.kinematics.J;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf17cab9" class="outline-3">
|
|
<h3 id="orgf17cab9">Computation</h3>
|
|
<div class="outline-text-3" id="text-orgf17cab9">
|
|
<p>
|
|
From a small displacement of each strut \(d\bm{\mathcal{L}}\), we can compute the
|
|
position and orientation of {B} with respect to {A} using the following formula:
|
|
\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">X = J\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: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:6));
|
|
s = X(4:6)/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)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
|
|
s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
|
|
s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="postamble" class="status">
|
|
<p class="author">Author: Dehaeze Thomas</p>
|
|
<p class="date">Created: 2020-05-05 mar. 10:34</p>
|
|
</div>
|
|
</body>
|
|
</html>
|