diff --git a/figs/inverse_kinematics_approx_validity_x_translation.pdf b/figs/inverse_kinematics_approx_validity_x_translation.pdf new file mode 100644 index 0000000..08dc7cd Binary files /dev/null and b/figs/inverse_kinematics_approx_validity_x_translation.pdf differ diff --git a/figs/inverse_kinematics_approx_validity_x_translation.png b/figs/inverse_kinematics_approx_validity_x_translation.png new file mode 100644 index 0000000..688f2c1 Binary files /dev/null and b/figs/inverse_kinematics_approx_validity_x_translation.png differ diff --git a/figs/inverse_kinematics_approx_validity_x_translation_relative.pdf b/figs/inverse_kinematics_approx_validity_x_translation_relative.pdf new file mode 100644 index 0000000..09e65ba Binary files /dev/null and b/figs/inverse_kinematics_approx_validity_x_translation_relative.pdf differ diff --git a/figs/inverse_kinematics_approx_validity_x_translation_relative.png b/figs/inverse_kinematics_approx_validity_x_translation_relative.png new file mode 100644 index 0000000..f823ab5 Binary files /dev/null and b/figs/inverse_kinematics_approx_validity_x_translation_relative.png differ diff --git a/kinematic-study.html b/kinematic-study.html index fc4132e..13c094a 100644 --- a/kinematic-study.html +++ b/kinematic-study.html @@ -4,7 +4,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
- +computeJacobian
: Compute the Jacobian Matrix
-inverseKinematics
: Compute Inverse Kinematics
+forwardKinematicsApprox
: Compute the Approximate Forward Kinematics
++The kinematic analysis of a parallel manipulator is well described in taghirad13_paral: +
+++ ++Kinematic analysis refers to the study of the geometry of motion of a robot, without considering the forces an torques that cause the motion. +In this analysis, the relation between the geometrical parameters of the manipulator with the final motion of the moving platform is derived and analyzed. +
+
+The current document is divided in the following sections: +
++From taghirad13_paral: +
+++The Jacobian matrix not only reveals the relation between the joint variable velocities of a parallel manipulator to the moving platform linear and angular velocities, it also constructs the transformation needed to find the actuator forces from the forces and moments acting on the moving platform. +
+
+If we note: +
++Then, we can compute the Jacobian with the following equation (the superscript \(A\) is ignored): +
+\begin{equation*} + \bm{J} = \begin{bmatrix} + {\hat{\bm{s}}_1}^T & (\bm{b}_1 \times \hat{\bm{s}}_1)^T \\ + {\hat{\bm{s}}_2}^T & (\bm{b}_2 \times \hat{\bm{s}}_2)^T \\ + {\hat{\bm{s}}_3}^T & (\bm{b}_3 \times \hat{\bm{s}}_3)^T \\ + {\hat{\bm{s}}_4}^T & (\bm{b}_4 \times \hat{\bm{s}}_4)^T \\ + {\hat{\bm{s}}_5}^T & (\bm{b}_5 \times \hat{\bm{s}}_5)^T \\ + {\hat{\bm{s}}_6}^T & (\bm{b}_6 \times \hat{\bm{s}}_6)^T + \end{bmatrix} +\end{equation*} -
+The Jacobian matrix \(\bm{J}\) can be computed using the computeJacobian
function (accessible here).
+For instance:
+
stewart = computeJacobian(stewart); +
+This will add three new matrix to the stewart
structure:
+
J
the Jacobian matrixK
the stiffness matrixC
the compliance matrix+The Jacobian matrix links the input joint rate \(\dot{\bm{\mathcal{L}}} = [ \dot{l}_1, \dot{l}_2, \dot{l}_3, \dot{l}_4, \dot{l}_5, \dot{l}_6 ]^T\) of each strut to the output twist vector of the mobile platform is denoted by \(\dot{\bm{X}} = [^A\bm{v}_p, {}^A\bm{\omega}]^T\): +
+\begin{equation*} + \dot{\bm{\mathcal{L}}} = \bm{J} \dot{\bm{\mathcal{X}}} +\end{equation*} + ++The input joint rate \(\dot{\bm{\mathcal{L}}}\) can be measured by taking the derivative of the relative motion sensor in each strut. +The output twist vector can be measured with a “Transform Sensor” block measuring the relative velocity and relative angular velocity of frame \(\{B\}\) with respect to frame \(\{A\}\). +
+ ++If the Jacobian matrix is inversible, we can also compute \(\dot{\bm{\mathcal{X}}}\) from \(\dot{\bm{\mathcal{L}}}\). +
+\begin{equation*} + \dot{\bm{\mathcal{X}}} = \bm{J}^{-1} \dot{\bm{\mathcal{L}}} +\end{equation*} + ++The Jacobian matrix can also be used to approximate forward and inverse kinematics for small displacements. +This is explained in section 3.3. +
++If we note: +
++We find that the transpose of the Jacobian matrix links the two by the following equation: +
+\begin{equation*} + \bm{\mathcal{F}} = \bm{J}^T \bm{\tau} +\end{equation*} + ++If the Jacobian matrix is inversible, we also have the following relation: +
+\begin{equation*} + \bm{\tau} = \bm{J}^{-T} \bm{\mathcal{F}} +\end{equation*} ++Here, we focus on the deflections of the manipulator moving platform that are the result of the external applied wrench to the mobile platform. +The amount of these deflections are a function of the applied wrench as well as the manipulator structural stiffness. +
+As explain in this document, each Actuator is modeled by 3 elements in parallel: +
++The stiffness of the actuator \(k_i\) links the applied actuator force \(\delta \tau_i\) and the corresponding small deflection \(\delta l_i\): +
+\begin{equation*} + \tau_i = k_i \delta l_i, \quad i = 1,\ \dots,\ 6 +\end{equation*} ++If we combine these 6 relations: +
+\begin{equation*} + \bm{\tau} = \mathcal{K} \delta \bm{\mathcal{L}} \quad \mathcal{K} = \text{diag}\left[ k_1,\ \dots,\ k_6 \right] +\end{equation*} + ++Substituting \(\bm{\tau} = \bm{J}^{-T} \bm{\mathcal{F}}\) and \(\delta \bm{\mathcal{L}} = \bm{J} \cdot \delta \bm{\mathcal{X}}\) gives +
+\begin{equation*} + \bm{\mathcal{F}} = \bm{J}^T \mathcal{K} \bm{J} \cdot \delta \bm{\mathcal{X}} +\end{equation*} ++And then we identify the stiffness matrix \(\bm{K}\): +
+\begin{equation*} + \bm{K} = \bm{J}^T \mathcal{K} \bm{J} +\end{equation*} + ++If the stiffness matrix \(\bm{K}\) is inversible, the compliance matrix of the manipulator is defined as +
+\begin{equation*} + \bm{C} = \bm{K}^{-1} = (\bm{J}^T \mathcal{K} \bm{J})^{-1} +\end{equation*} + ++The compliance matrix of a manipulator shows the mapping of the moving platform wrench applied at \(\bm{O}_B\) to its small deflection by +
+\begin{equation*} + \delta \bm{\mathcal{X}} = \bm{C} \cdot \bm{\mathcal{F}} +\end{equation*} + +
+The stiffness and compliance matrices are computed using the computeJacobian
function (accessible here).
+
++ ++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 \(\bm{\mathcal{L}} = \left[ l_1, l_2, l_3, l_4, l_5, l_6 \right]^T\). +
+
+This problem can be easily solved using the loop closures. +
+ ++The obtain joint variables are: +
+\begin{equation*} + \begin{aligned} + l_i = &\Big[ {}^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 + \dots\\ + &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 \Big]^{1/2} + \end{aligned} +\end{equation*} + ++If the position and orientation of the platform lie in the feasible workspace, the solution is unique. +Otherwise, the solution gives complex numbers. +
+ +
+This inverse kinematic solution can be obtained using the function inverseKinematics
(described here).
+
++ ++In forward kinematic analysis, it is assumed that the vector of limb lengths \(\bm{L}\) is given and the problem is to find the position \({}^A\bm{P}\) and the orientation \({}^A\bm{R}_B\). +
+
+This is a difficult problem that requires to solve nonlinear equations. +
+ ++In a next section, an approximate solution of the forward kinematics problem is proposed for small displacements. +
+For small displacements mobile platform displacement \(\delta \bm{\mathcal{X}} = [\delta x, \delta y, \delta z, \delta \theta_x, \delta \theta_y, \delta \theta_z ]^T\) around \(\bm{\mathcal{X}}_0\), the associated joint displacement can be computed using the Jacobian: +
+\begin{equation*} + \delta\bm{\mathcal{L}} = \bm{J} \delta\bm{\mathcal{X}} +\end{equation*} + ++Similarly, for small joint displacements \(\delta\bm{\mathcal{L}} = [ \delta l_1,\ \dots,\ \delta l_6 ]^T\) around \(\bm{\mathcal{L}}_0\), it is possible to find the induced small displacement of the mobile platform: +
+\begin{equation*} + \delta\bm{\mathcal{X}} = \bm{J}^{-1} \delta\bm{\mathcal{L}} +\end{equation*} + ++These two relations solve the forward and inverse kinematic problems for small displacement in a approximate way. +As the inverse kinematic can be easily solved exactly this is not much useful, however, as the forward kinematic problem is difficult to solve, this approximation can be very useful for small displacements. +
+ +
+The function forwardKinematicsApprox
(described here) can be used to solve the forward kinematic problem using the Jacobian matrix.
+
+As we know how to exactly solve the Inverse kinematic problem, we can compare the exact solution with the approximate solution using the Jacobian matrix. +For small displacements, the approximate solution is expected to work well. +We would like here to determine up to what displacement this approximation can be considered as correct. +
+ ++Then, we can determine the range for which the approximate inverse kinematic is valid. +This will also gives us the range for which the approximate forward kinematic is valid. +
++We first define some general Stewart architecture. +
+stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3); +stewart = generateGeneralConfiguration(stewart); +stewart = computeJointsPose(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart); +stewart = initializeCylindricalStruts(stewart); +stewart = initializeStrutDynamics(stewart); +stewart = initializeJointDynamics(stewart); +stewart = computeJacobian(stewart); ++
+Let’s first compare the perfect and approximate solution of the inverse for pure \(x\) translations. +
+ ++We compute the approximate and exact required strut stroke to have the wanted mobile platform \(x\) displacement. +The estimate required strut stroke for both the approximate and exact solutions are shown in Figure 1. +The relative strut length displacement is shown in Figure 2. +
+Xrs = logspace(-6, -1, 100); % Wanted X translation of the mobile platform [m] + +Ls_approx = zeros(6, length(Xrs)); +Ls_exact = zeros(6, length(Xrs)); + +for i = 1:length(Xrs) + Xr = Xrs(i); + L_approx(:, i) = stewart.J*[Xr; 0; 0; 0; 0; 0;]; + [~, L_exact(:, i)] = inverseKinematics(stewart, 'AP', [Xr; 0; 0]); +end ++
+For small wanted displacements (up to \(\approx 1\%\) of the size of the Hexapod), the approximate inverse kinematic solution using the Jacobian matrix is quite correct. +
++Let’s say one want to design a Stewart platform with some specified mobility (position and orientation). +One may want to determine the required actuator stroke required to obtain the specified mobility. +This is what is analyzed in this section. +
+-The goal is to determine the needed stroke of the actuators to obtain wanted translations and rotations. +Let’s first define the Stewart platform architecture that we want to study.
--We use a cubic architecture. -
-opts = struct(... - 'H_tot', 90, ... % Total height of the Hexapod [mm] - 'L', 200/sqrt(3), ... % Size of the Cube [mm] - 'H', 60, ... % Height between base joints and platform joints [mm] - 'H0', 200/2-60/2 ... % Height between the corner of the cube and the plane containing the base joints [mm] - ); -stewart = initializeCubicConfiguration(opts); -opts = struct(... - 'Jd_pos', [0, 0, 100], ... % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm] - 'Jf_pos', [0, 0, -50] ... % Position of the Jacobian for force location from the top of the mobile platform [mm] - ); -stewart = computeGeometricalProperties(stewart, opts); -opts = struct(... - 'stroke', 50e-6 ... % Maximum stroke of each actuator [m] - ); -stewart = initializeMechanicalElements(stewart, opts); - -save('./mat/stewart.mat', 'stewart'); +stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3); +stewart = generateGeneralConfiguration(stewart); +stewart = computeJointsPose(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart); +stewart = initializeCylindricalStruts(stewart); +stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1)); +stewart = initializeJointDynamics(stewart); +stewart = computeJacobian(stewart);
-We define wanted translations and rotations +Let’s now define the wanted extreme translations and rotations.
Tx_max = 15e-6; % Translation [m] -Ty_max = 15e-6; % Translation [m] -Tz_max = 15e-6; % Translation [m] +Tx_max = 50e-6; % Translation [m] +Ty_max = 50e-6; % Translation [m] +Tz_max = 50e-6; % Translation [m] Rx_max = 30e-6; % Rotation [rad] Ry_max = 30e-6; % Rotation [rad] +Rz_max = 0; % Rotation [rad]
-First, we estimate the needed actuator stroke for “pure” rotations and translation. +As a first estimation, we estimate the needed actuator stroke for “pure” rotations and translation. +We do that using either the Inverse Kinematic solution or the Jacobian matrix as an approximation. +
+ +LTx = stewart.J*[Tx_max 0 0 0 0 0]'; +LTy = stewart.J*[0 Ty_max 0 0 0 0]'; +LTz = stewart.J*[0 0 Tz_max 0 0 0]'; +LRx = stewart.J*[0 0 0 Rx_max 0 0]'; +LRy = stewart.J*[0 0 0 0 Ry_max 0]'; +LRz = stewart.J*[0 0 0 0 0 Rz_max]'; ++
+The obtain required stroke is: +
++From -3.8e-05[m] to 3.8e-05[m]: Total stroke = 76.1[um] ++ + +
+This is surely a low estimation of the required stroke. +
++We know would like to have a more precise estimation. +
+ ++To do so, we may estimate the required actuator stroke for all possible combination of translation and rotation. +
+ ++Let’s first generate all the possible combination of maximum translation and rotations.
LTx = stewart.Jd*[Tx_max 0 0 0 0 0]'; -LTy = stewart.Jd*[0 Ty_max 0 0 0 0]'; -LTz = stewart.Jd*[0 0 Tz_max 0 0 0]'; -LRx = stewart.Jd*[0 0 0 Rx_max 0 0]'; -LRy = stewart.Jd*[0 0 0 0 Ry_max 0]'; +Ps = [2*(dec2bin(0:5^2-1,5)-'0')-1, zeros(5^2, 1)].*[Tx_max Ty_max Tz_max Rx_max Ry_max Rz_max];
Tx [m] | +Ty [m] | +Tz [m] | +Rx [rad] | +Ry [rad] | +Rz [rad] | +
---|---|---|---|---|---|
-5.0e-05 | +-5.0e-05 | +-5.0e-05 | +-3.0e-05 | +-3.0e-05 | +0.0e+00 | +
-5.0e-05 | +-5.0e-05 | +-5.0e-05 | +-3.0e-05 | +3.0e-05 | +0.0e+00 | +
-5.0e-05 | +-5.0e-05 | +-5.0e-05 | +3.0e-05 | +-3.0e-05 | +0.0e+00 | +
-5.0e-05 | +-5.0e-05 | +-5.0e-05 | +3.0e-05 | +3.0e-05 | +0.0e+00 | +
-5.0e-05 | +-5.0e-05 | +5.0e-05 | +-3.0e-05 | +-3.0e-05 | +0.0e+00 | +
-5.0e-05 | +-5.0e-05 | +5.0e-05 | +-3.0e-05 | +3.0e-05 | +0.0e+00 | +
-5.0e-05 | +-5.0e-05 | +5.0e-05 | +3.0e-05 | +-3.0e-05 | +0.0e+00 | +
-5.0e-05 | +-5.0e-05 | +5.0e-05 | +3.0e-05 | +3.0e-05 | +0.0e+00 | +
-5.0e-05 | +5.0e-05 | +-5.0e-05 | +-3.0e-05 | +-3.0e-05 | +0.0e+00 | +
-5.0e-05 | +5.0e-05 | +-5.0e-05 | +-3.0e-05 | +3.0e-05 | +0.0e+00 | +
-5.0e-05 | +5.0e-05 | +-5.0e-05 | +3.0e-05 | +-3.0e-05 | +0.0e+00 | +
-5.0e-05 | +5.0e-05 | +-5.0e-05 | +3.0e-05 | +3.0e-05 | +0.0e+00 | +
-5.0e-05 | +5.0e-05 | +5.0e-05 | +-3.0e-05 | +-3.0e-05 | +0.0e+00 | +
-5.0e-05 | +5.0e-05 | +5.0e-05 | +-3.0e-05 | +3.0e-05 | +0.0e+00 | +
-5.0e-05 | +5.0e-05 | +5.0e-05 | +3.0e-05 | +-3.0e-05 | +0.0e+00 | +
-5.0e-05 | +5.0e-05 | +5.0e-05 | +3.0e-05 | +3.0e-05 | +0.0e+00 | +
5.0e-05 | +-5.0e-05 | +-5.0e-05 | +-3.0e-05 | +-3.0e-05 | +0.0e+00 | +
5.0e-05 | +-5.0e-05 | +-5.0e-05 | +-3.0e-05 | +3.0e-05 | +0.0e+00 | +
5.0e-05 | +-5.0e-05 | +-5.0e-05 | +3.0e-05 | +-3.0e-05 | +0.0e+00 | +
5.0e-05 | +-5.0e-05 | +-5.0e-05 | +3.0e-05 | +3.0e-05 | +0.0e+00 | +
5.0e-05 | +-5.0e-05 | +5.0e-05 | +-3.0e-05 | +-3.0e-05 | +0.0e+00 | +
5.0e-05 | +-5.0e-05 | +5.0e-05 | +-3.0e-05 | +3.0e-05 | +0.0e+00 | +
5.0e-05 | +-5.0e-05 | +5.0e-05 | +3.0e-05 | +-3.0e-05 | +0.0e+00 | +
5.0e-05 | +-5.0e-05 | +5.0e-05 | +3.0e-05 | +3.0e-05 | +0.0e+00 | +
5.0e-05 | +5.0e-05 | +-5.0e-05 | +-3.0e-05 | +-3.0e-05 | +0.0e+00 | +
+For all possible combination, we compute the required actuator stroke using the inverse kinematic solution. +
+L_min = 0; +L_max = 0; + +for i = 1:size(Ps,1) + Rx = [1 0 0; + 0 cos(Ps(i, 4)) -sin(Ps(i, 4)); + 0 sin(Ps(i, 4)) cos(Ps(i, 4))]; + + Ry = [ cos(Ps(i, 5)) 0 sin(Ps(i, 5)); + 0 1 0; + -sin(Ps(i, 5)) 0 cos(Ps(i, 5))]; + + Rz = [cos(Ps(i, 6)) -sin(Ps(i, 6)) 0; + sin(Ps(i, 6)) cos(Ps(i, 6)) 0; + 0 0 1]; + + ARB = Rz*Ry*Rx; + [~, Ls] = inverseKinematics(stewart, 'AP', Ps(i, 1:3)', 'ARB', ARB); + + if min(Ls) < L_min + L_min = min(Ls) + end + if max(Ls) > L_max + L_max = max(Ls) + end +end ++
+We obtain the required actuator stroke: +
-From -1.2e-05[m] to 1.1e-05[m]: Total stroke = 22.9[um] +From -8.9e-05[m] to 8.9e-05[m]: Total stroke = 177.2[um]-
+This is probably a much realistic estimation of the required actuator stroke. +
+Here, from some value of the actuator stroke, we would like to estimate the mobility of the Stewart platform. +
+ ++As explained in section 3, the forward kinematic problem of the Stewart platform is quite difficult to solve. +However, for small displacements, we can use the Jacobian as an approximate solution. +
+Let’s first define the Stewart platform architecture that we want to study. +
+stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3); +stewart = generateGeneralConfiguration(stewart); +stewart = computeJointsPose(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart); +stewart = initializeCylindricalStruts(stewart); +stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1)); +stewart = initializeJointDynamics(stewart); +stewart = computeJacobian(stewart); ++
+Let’s now define the actuator stroke. +
+L_min = -50e-6; % [m] +L_max = 50e-6; % [m] ++
computeJacobian
: Compute the Jacobian MatrixcomputeJacobian
: Compute the Jacobian Matrix@@ -498,9 +1205,9 @@ This Matlab function is accessible here.
function [stewart] = computeJacobian(stewart) % computeJacobian - @@ -522,9 +1229,9 @@ This Matlab function is accessible here.
stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)'];@@ -532,9 +1239,9 @@ This Matlab function is accessible here.
stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J;@@ -542,9 +1249,9 @@ This Matlab function is accessible here.
stewart.C = inv(stewart.K);@@ -553,11 +1260,11 @@ This Matlab function is accessible here.
inverseKinematics
: Compute Inverse KinematicsinverseKinematics
: Compute Inverse Kinematics@@ -565,9 +1272,9 @@ This Matlab function is accessible here.
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} @@ -590,9 +1297,9 @@ This Matlab function is accessible here.
arguments stewart @@ -604,9 +1311,9 @@ This Matlab function is accessible here.
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\).
@@ -640,9 +1347,9 @@ Otherwise, when the limbs’ lengths derived yield complex numbers, then theLi = sqrt(args.AP'*args.AP + diag(stewart.Bb'*stewart.Bb) + diag(stewart.Aa'*stewart.Aa) - (2*args.AP'*stewart.Aa)' + (2*args.AP'*(args.ARB*stewart.Bb))' - diag(2*(args.ARB*stewart.Bb)'*stewart.Aa));@@ -656,11 +1363,11 @@ Otherwise, when the limbs’ lengths derived yield complex numbers, then the
forwardKinematicsApprox
: Compute the Approximate Forward KinematicsforwardKinematicsApprox
: Compute the Approximate Forward Kinematics@@ -668,9 +1375,9 @@ This Matlab function is accessible here<
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 @@ -692,9 +1399,9 @@ This Matlab function is accessible here<
arguments stewart @@ -705,9 +1412,9 @@ This Matlab function is accessible here<
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: @@ -749,10 +1456,16 @@ We then compute the corresponding rotation matrix.
+ +
Created: 2020-01-29 mer. 13:29
+Created: 2020-01-29 mer. 17:48