first commit

Control.m

@@ -0,0 +1,15 @@
+G = -9.81;
+A = [0 1;
+     0 0];
+B = [0;
+     G];
+C = [1 0];
+D = [0 0];
+w0 = 1.5;
+xi = 0.7;
+s = tf('s');
+H = w0^2/(s^2+2*xi*w0*s+w0^2);
+p = pole(H);
+K = place(A,B,p);
+L = place(A',C',10*p);
+L = L';

Platform.xml

@@ -0,0 +1,198 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
+<SimscapeMultibodyImportXML version="2.0" xmlns="urn:mathworks:SimscapeMultibody:import" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+<Created by="" on="12/18/22||13:12:01" using="Simscape Multibody Link Version 6.1" from="SolidWorks 28.0.0"/>
+<ModelUnits mass="kilogram" length="millimeter"/>
+<DataUnits mass="kilogram" length="meter"/>
+<RootAssembly name="Platform" uid="Platform" version="135">
+<AssemblyFile name="Platform.SLDASM" type="SolidWorks Assembly"/>
+<InstanceTree>
+<Instance name="Base-1" uid="Base-1" grounded="true" entityUid="Base*:*Default">
+<Transform>
+<Rotation>1 0 0 0 1 0 0 0 1</Rotation>
+<Translation>-0.021765449182217414 0.125 -0.0018898144499543182</Translation>
+</Transform>
+</Instance>
+<Instance name="Joint-1" uid="Joint-1" entityUid="Joint*:*Default">
+<Transform>
+<Rotation>0 0.022140924559497285 0.99975485968293776 0 0.99975485968293776 -0.022140924559497285 -1 0 0</Rotation>
+<Translation>-0.021377983002426187 0.25999571004445138 -0.0018898144499543111</Translation>
+</Transform>
+</Instance>
+<Instance name="Plane-1" uid="Plane-1" entityUid="Plane*:*Default">
+<Transform>
+<Rotation>0.0013462565538018573 0.022099957774635921 -0.99975485968293787 0.060789084413647353 0.99790503890690985 0.022140924559497174 0.99814972564713045 -0.060803989923015729 0</Rotation>
+<Translation>-0.018389675105629159 0.39493024423806822 -0.0029538842736070753</Translation>
+</Transform>
+</Instance>
+</InstanceTree>
+<Constraints>
+<Parallel name="Parallel2">
+<ConstraintGeometry geomType="plane">
+<InstancePath>
+<Uid>Base-1</Uid>
+</InstancePath>
+<Position>0 0 0</Position>
+<Axis>0 0 1</Axis>
+</ConstraintGeometry>
+<ConstraintGeometry geomType="plane">
+<InstancePath>
+<Uid>Platform</Uid>
+</InstancePath>
+<Position>0 0 0</Position>
+<Axis>0 0 1</Axis>
+</ConstraintGeometry>
+</Parallel>
+<Parallel name="Parallel3">
+<ConstraintGeometry geomType="plane">
+<InstancePath>
+<Uid>Base-1</Uid>
+</InstancePath>
+<Position>0 0 0</Position>
+<Axis>0 1 0</Axis>
+</ConstraintGeometry>
+<ConstraintGeometry geomType="plane">
+<InstancePath>
+<Uid>Platform</Uid>
+</InstancePath>
+<Position>0 0 0</Position>
+<Axis>0 1 0</Axis>
+</ConstraintGeometry>
+</Parallel>
+<Parallel name="Parallel4">
+<ConstraintGeometry geomType="plane">
+<InstancePath>
+<Uid>Base-1</Uid>
+</InstancePath>
+<Position>0 0 0</Position>
+<Axis>1 0 0</Axis>
+</ConstraintGeometry>
+<ConstraintGeometry geomType="plane">
+<InstancePath>
+<Uid>Platform</Uid>
+</InstancePath>
+<Position>0 0 0</Position>
+<Axis>1 0 0</Axis>
+</ConstraintGeometry>
+</Parallel>
+<Concentric name="Concentric1">
+<ConstraintGeometry geomType="cylinder">
+<InstancePath>
+<Uid>Base-1</Uid>
+</InstancePath>
+<Position>0 0.11749999999999999 0.0074999999999999997</Position>
+<Axis>0 0 1</Axis>
+</ConstraintGeometry>
+<ConstraintGeometry geomType="cylinder">
+<InstancePath>
+<Uid>Joint-1</Uid>
+</InstancePath>
+<Position>-0.032500000000000001 -0.017499999999999988 0</Position>
+<Axis>1 0 0</Axis>
+</ConstraintGeometry>
+</Concentric>
+<Coincident name="Coincident2">
+<ConstraintGeometry geomType="plane">
+<InstancePath>
+<Uid>Base-1</Uid>
+</InstancePath>
+<Position>-0.012500000000000004 0.125 0.032500000000000001</Position>
+<Axis>0 0 1</Axis>
+</ConstraintGeometry>
+<ConstraintGeometry geomType="plane">
+<InstancePath>
+<Uid>Joint-1</Uid>
+</InstancePath>
+<Position>-0.032500000000000001 -0.017499999999999988 0</Position>
+<Axis>-1 0 0</Axis>
+</ConstraintGeometry>
+</Coincident>
+<Concentric name="Concentric2">
+<ConstraintGeometry geomType="cylinder">
+<InstancePath>
+<Uid>Joint-1</Uid>
+</InstancePath>
+<Position>0 0.11749999999999994 0.0074999999999999997</Position>
+<Axis>0 0 1.0000000000000002</Axis>
+</ConstraintGeometry>
+<ConstraintGeometry geomType="cylinder">
+<InstancePath>
+<Uid>Plane-1</Uid>
+</InstancePath>
+<Position>0 -0.017500000000000016 -0.032500000000000015</Position>
+<Axis>0 0 1.0000000000000002</Axis>
+</ConstraintGeometry>
+</Concentric>
+<Coincident name="Coincident3">
+<ConstraintGeometry geomType="plane">
+<InstancePath>
+<Uid>Joint-1</Uid>
+</InstancePath>
+<Position>-0.012500000000000006 0.125 0.032500000000000001</Position>
+<Axis>0 0 1.0000000000000002</Axis>
+</ConstraintGeometry>
+<ConstraintGeometry geomType="plane">
+<InstancePath>
+<Uid>Plane-1</Uid>
+</InstancePath>
+<Position>0 -0.017500000000000016 -0.032500000000000015</Position>
+<Axis>0 0 -1.0000000000000002</Axis>
+</ConstraintGeometry>
+</Coincident>
+</Constraints>
+</RootAssembly>
+<Assemblies>
+</Assemblies>
+<Parts>
+<Part name="Base" uid="Base*:*Default" version="128">
+<ModelUnits mass="kilogram" length="millimeter"/>
+<PartFile name="Base.SLDPRT" type="SolidWorks Part"/>
+<MassProperties>
+<Mass>0.80073110641889567</Mass>
+<CenterOfMass>0 0.051162188725439268 0</CenterOfMass>
+<Inertia>0.0012113939537175692 0.00099246946776490683 0.0012023610379077075 0 1.612156259958321e-11 0</Inertia>
+</MassProperties>
+<GeometryFile name="Base_Default_sldprt.STEP" type="STEP"/>
+<VisualProperties>
+<Ambient r="0.84705882352941175" g="0.47843137254901957" b="0" a="1"/>
+<Diffuse r="0.84705882352941175" g="0.47843137254901957" b="0" a="1"/>
+<Specular r="0.42352941176470588" g="0.23921568627450979" b="0" a="1"/>
+<Emissive r="0" g="0" b="0" a="1"/>
+<Shininess>0.40000000596046448</Shininess>
+</VisualProperties>
+</Part>
+<Part name="Joint" uid="Joint*:*Default" version="199">
+<ModelUnits mass="kilogram" length="millimeter"/>
+<PartFile name="Joint.SLDPRT" type="SolidWorks Part"/>
+<MassProperties>
+<Mass>0.82109570110675179</Mass>
+<CenterOfMass>-3.0704449587902081e-11 0.049584824808285605 -3.3082346009854229e-10</CenterOfMass>
+<Inertia>0.0012935363418924133 0.00099629533653762276 0.0012865858354413212 -2.1631210516772468e-11 2.0220514764215077e-11 -3.4661855293543975e-12</Inertia>
+</MassProperties>
+<GeometryFile name="Joint_Default_sldprt.STEP" type="STEP"/>
+<VisualProperties>
+<Ambient r="1" g="0.98431372549019602" b="0.12549019607843137" a="1"/>
+<Diffuse r="1" g="0.98431372549019602" b="0.12549019607843137" a="1"/>
+<Specular r="0.5" g="0.49215686274509801" b="0.062745098039215685" a="1"/>
+<Emissive r="0" g="0" b="0" a="1"/>
+<Shininess>0.40000000596046448</Shininess>
+</VisualProperties>
+</Part>
+<Part name="Plane" uid="Plane*:*Default" version="130">
+<ModelUnits mass="kilogram" length="millimeter"/>
+<PartFile name="Plane.SLDPRT" type="SolidWorks Part"/>
+<MassProperties>
+<Mass>120.52036476349195</Mass>
+<CenterOfMass>0 0.058494236676340919 0</CenterOfMass>
+<Inertia>12.119408326280547 23.850212159261865 12.11940624392318 0 -1.1143197965166505e-12 -6.8020978052460138e-16</Inertia>
+</MassProperties>
+<GeometryFile name="Plane_Default_sldprt.STEP" type="STEP"/>
+<VisualProperties>
+<Ambient r="0.41176470588235292" g="1" b="0.9882352941176471" a="1"/>
+<Diffuse r="0.41176470588235292" g="1" b="0.9882352941176471" a="1"/>
+<Specular r="0.20588235294117646" g="0.5" b="0.49411764705882355" a="1"/>
+<Emissive r="0" g="0" b="0" a="1"/>
+<Shininess>0.40000000596046448</Shininess>
+</VisualProperties>
+</Part>
+</Parts>
+</SimscapeMultibodyImportXML>

Platform_DataFile.m

@@ -0,0 +1,140 @@
+% Simscape(TM) Multibody(TM) version: 6.1
+
+% This is a model data file derived from a Simscape Multibody Import XML file using the smimport function.
+% The data in this file sets the block parameter values in an imported Simscape Multibody model.
+% For more information on this file, see the smimport function help page in the Simscape Multibody documentation.
+% You can modify numerical values, but avoid any other changes to this file.
+% Do not add code to this file. Do not edit the physical units shown in comments.
+
+%%%VariableName:smiData
+
+
+%============= RigidTransform =============%
+
+%Initialize the RigidTransform structure array by filling in null values.
+smiData.RigidTransform(7).translation = [0.0 0.0 0.0];
+smiData.RigidTransform(7).angle = 0.0;
+smiData.RigidTransform(7).axis = [0.0 0.0 0.0];
+smiData.RigidTransform(7).ID = '';
+
+%Translation Method - Cartesian
+%Rotation Method - Arbitrary Axis
+smiData.RigidTransform(1).translation = [0 0 0];  % mm
+smiData.RigidTransform(1).angle = 0;  % rad
+smiData.RigidTransform(1).axis = [0 0 0];
+smiData.RigidTransform(1).ID = 'B[Base-1:-:]';
+
+%Translation Method - Cartesian
+%Rotation Method - Arbitrary Axis
+smiData.RigidTransform(2).translation = [-21.765449182217413 125 -1.8898144499543181];  % mm
+smiData.RigidTransform(2).angle = 0;  % rad
+smiData.RigidTransform(2).axis = [0 0 0];
+smiData.RigidTransform(2).ID = 'F[Base-1:-:]';
+
+%Translation Method - Cartesian
+%Rotation Method - Arbitrary Axis
+smiData.RigidTransform(3).translation = [0 117.49999999999994 7.5];  % mm
+smiData.RigidTransform(3).angle = 0;  % rad
+smiData.RigidTransform(3).axis = [0 0 0];
+smiData.RigidTransform(3).ID = 'B[Joint-1:-:Plane-1]';
+
+%Translation Method - Cartesian
+%Rotation Method - Arbitrary Axis
+smiData.RigidTransform(4).translation = [-6.1284310959308641e-14 -17.500000000000796 -7.5000000000000497];  % mm
+smiData.RigidTransform(4).angle = 3.1415926535897931;  % rad
+smiData.RigidTransform(4).axis = [1 -5.7714994301193547e-33 -1.1263298435210474e-16];
+smiData.RigidTransform(4).ID = 'F[Joint-1:-:Plane-1]';
+
+%Translation Method - Cartesian
+%Rotation Method - Arbitrary Axis
+smiData.RigidTransform(5).translation = [0 117.5 7.5];  % mm
+smiData.RigidTransform(5).angle = 0;  % rad
+smiData.RigidTransform(5).axis = [0 0 0];
+smiData.RigidTransform(5).ID = 'B[Base-1:-:Joint-1]';
+
+%Translation Method - Cartesian
+%Rotation Method - Arbitrary Axis
+smiData.RigidTransform(6).translation = [-7.4999999999999947 -17.499999999999947 -2.5757174171303632e-14];  % mm
+smiData.RigidTransform(6).angle = 2.0943951023931957;  % rad
+smiData.RigidTransform(6).axis = [-0.57735026918962584 -0.57735026918962573 0.57735026918962573];
+smiData.RigidTransform(6).ID = 'F[Base-1:-:Joint-1]';
+
+%Translation Method - Cartesian
+%Rotation Method - Arbitrary Axis
+smiData.RigidTransform(7).translation = [-21.765449182217413 125 -1.8898144499543181];  % mm
+smiData.RigidTransform(7).angle = 0;  % rad
+smiData.RigidTransform(7).axis = [0 0 0];
+smiData.RigidTransform(7).ID = 'RootGround[Base-1]';
+
+
+%============= Solid =============%
+%Center of Mass (CoM) %Moments of Inertia (MoI) %Product of Inertia (PoI)
+
+%Initialize the Solid structure array by filling in null values.
+smiData.Solid(3).mass = 0.0;
+smiData.Solid(3).CoM = [0.0 0.0 0.0];
+smiData.Solid(3).MoI = [0.0 0.0 0.0];
+smiData.Solid(3).PoI = [0.0 0.0 0.0];
+smiData.Solid(3).color = [0.0 0.0 0.0];
+smiData.Solid(3).opacity = 0.0;
+smiData.Solid(3).ID = '';
+
+%Inertia Type - Custom
+%Visual Properties - Simple
+smiData.Solid(1).mass = 0.80073110641889567;  % kg
+smiData.Solid(1).CoM = [0 51.162188725439272 0];  % mm
+smiData.Solid(1).MoI = [1211.3939537175693 992.46946776490688 1202.3610379077074];  % kg*mm^2
+smiData.Solid(1).PoI = [0 1.612156259958321e-05 0];  % kg*mm^2
+smiData.Solid(1).color = [0.84705882352941175 0.47843137254901957 0];
+smiData.Solid(1).opacity = 1;
+smiData.Solid(1).ID = 'Base*:*Default';
+
+%Inertia Type - Custom
+%Visual Properties - Simple
+smiData.Solid(2).mass = 0.82109570110675179;  % kg
+smiData.Solid(2).CoM = [-3.0704449587902079e-08 49.584824808285603 -3.3082346009854228e-07];  % mm
+smiData.Solid(2).MoI = [1293.5363418924132 996.2953365376228 1286.5858354413213];  % kg*mm^2
+smiData.Solid(2).PoI = [-2.1631210516772468e-05 2.0220514764215076e-05 -3.4661855293543976e-06];  % kg*mm^2
+smiData.Solid(2).color = [1 0.98431372549019602 0.12549019607843137];
+smiData.Solid(2).opacity = 1;
+smiData.Solid(2).ID = 'Joint*:*Default';
+
+%Inertia Type - Custom
+%Visual Properties - Simple
+smiData.Solid(3).mass = 120.52036476349195;  % kg
+smiData.Solid(3).CoM = [0 58.494236676340918 0];  % mm
+smiData.Solid(3).MoI = [12119408.326280547 23850212.159261864 12119406.24392318];  % kg*mm^2
+smiData.Solid(3).PoI = [0 -1.1143197965166504e-06 -6.8020978052460138e-10];  % kg*mm^2
+smiData.Solid(3).color = [0.41176470588235292 1 0.9882352941176471];
+smiData.Solid(3).opacity = 1;
+smiData.Solid(3).ID = 'Plane*:*Default';
+
+
+%============= Joint =============%
+%X Revolute Primitive (Rx) %Y Revolute Primitive (Ry) %Z Revolute Primitive (Rz)
+%X Prismatic Primitive (Px) %Y Prismatic Primitive (Py) %Z Prismatic Primitive (Pz) %Spherical Primitive (S)
+%Constant Velocity Primitive (CV) %Lead Screw Primitive (LS)
+%Position Target (Pos)
+
+%Initialize the CartesianJoint structure array by filling in null values.
+smiData.CartesianJoint(1).Px.Pos = 0.0;
+smiData.CartesianJoint(1).Py.Pos = 0.0;
+smiData.CartesianJoint(1).Pz.Pos = 0.0;
+smiData.CartesianJoint(1).ID = '';
+
+smiData.CartesianJoint(1).Px.Pos = 0;  % m
+smiData.CartesianJoint(1).Py.Pos = 0;  % m
+smiData.CartesianJoint(1).Pz.Pos = 0;  % m
+smiData.CartesianJoint(1).ID = '[Base-1:-:]';
+
+
+%Initialize the RevoluteJoint structure array by filling in null values.
+smiData.RevoluteJoint(2).Rz.Pos = 0.0;
+smiData.RevoluteJoint(2).ID = '';
+
+smiData.RevoluteJoint(1).Rz.Pos = 176.51403773901069;  % deg
+smiData.RevoluteJoint(1).ID = '[Joint-1:-:Plane-1]';
+
+smiData.RevoluteJoint(2).Rz.Pos = 88.731314797898051;  % deg
+smiData.RevoluteJoint(2).ID = '[Base-1:-:Joint-1]';
+