master-thesis
/
mahd
1
0
Форкнуть 0
Код Задачи 0 Pull Request'ы 0 Релизы 0 Вики Активность
Просмотр исходного кода

Merge branch 'iss13/05_feature_selection' into 'master' 

Add end-effector 3D-model.

See merge request horlingsw/mahd!6
master
Wouter Horlings 5 лет назад
Родитель
e99f7109c8 d56ed5891e
Сommit
4b89fdacd4
3 измененных файлов: 4904 добавлений и 0 удалений
Разделённый вид
Опции Diff
Показать статистику Скачать Patch файл Скачать Diff файл
  1. +3990
    -0
      implementation/submodel/endeffector/endeffector.3dm
  2. +877
    -0
      implementation/submodel/endeffector/endeffector.emx
  3. +37
    -0
      implementation/submodel/endeffector/gripper.scad

+ 3990
- 0
implementation/submodel/endeffector/endeffector.3dm
Разница между файлами не показана из-за своего большого размера
Просмотреть файл


+ 877
- 0
implementation/submodel/endeffector/endeffector.emx Просмотреть файл

@@ -0,0 +1,877 @@
<?xml version="1.0" encoding="UTF-8"?>
<Document>
<Model version="4.8" build="4.8.2.10124">
<Sidops><![CDATA[model 128 184
description '<Information>
<Description>
<Version>4.8</Version>
<IsMainModel>0</IsMainModel>
<KeepParameterValues>False</KeepParameterValues>
<LibraryPath>C:\users\wouter\My Documents\studie\ma\mahd\implementation\submodel\endeffector\endeffector.emx</LibraryPath>
<TimeStamp>2020-7-6 14:39:32</TimeStamp>
</Description>
</Information>';
type Mainmodel
end;
implementation bg
submodels
endeffector 592 296
description '<Description>
<IsMainModel>1</IsMainModel>
<KeepParameterValues>True</KeepParameterValues>
<Version>4.8</Version>
<Description>Model generated by 3D-Mechanics Toolbox
Original Filename: C:\users\wouter\My Documents\studie\ma\mahd\implementation\submodel\endeffector\endeffector.3dm</Description>
<LibraryPath>C:\users\wouter\My Documents\studie\ma\mahd\implementation\submodel\endeffector\endeffector.emx</LibraryPath>
<TimeStamp>2020-7-6 14:12:40</TimeStamp>
</Description>';
type MechanicsModel
ports
signal out H_marker [4,4];
mechanical psi in marker2 [6,1];
signal out H_grippertop [4,4];
mechanical psi in grippertop2 [6,1];
signal out H_gripperbottom [4,4];
mechanical psi in gripperbottom [6,1];
restrictions
causality fixed in marker2;
causality fixed in grippertop2;
causality fixed in gripperbottom;
end;
icon bg bottom
figures
image 'C:\users\wouter\My Documents\studie\ma\mahd\implementation\submodel\endeffector\endeffectorIcon.png' 528 232 128 128;
terminals
marker2 592.5 296.5;
grippertop2 592.5 296.5;
gripperbottom 592.5 296.5;
end;
implementation eq
// 20-sim 3D Mechanics Editor
// C:\users\wouter\My Documents\studie\ma\mahd\implementation\submodel\endeffector\endeffector.3dm

variables
real hidden World\AbsH[4,4];
real hidden World\mass_x_I[6,6];
real hidden holderWeld\ConnectionPoint1\AbsH[4,4];
real hidden holderWeld\AbsH[4,4];
real hidden holderWeld\ConnectionPoint2\AbsH[4,4];
real hidden holder\AbsH[4,4];
real hidden holder\mass_x_I[6,6];
real hidden markerFree\ConnectionPoint1\AbsH[4,4];
real hidden markerFree\AbsH[4,4];
real hidden markerFree\RelH[4,4];
real hidden markerFree\rotationInitial[4,4];
real markerFree\quat[4];
real markerFree\quatDot[4];
real hidden markerFree\quatInitial[4];
real markerFree\QOmega[4,4];
real markerFree\vgen[6];
real markerFree\x[3];
real markerFree\v[3];
real markerFree\vel[3];
real markerFree_Rot[4,4];
real hidden markerFree\ConnectionPoint2\AbsH[4,4];
real hidden marker\AbsH[4,4];
real hidden marker\mass_x_I[6,6];
real hidden H_marker\ConnectionPoint1\AbsH[4,4];
real hidden marker2\ConnectionPoint1\AbsH[4,4];
real hidden grippertopFree\ConnectionPoint1\AbsH[4,4];
real hidden grippertopFree\AbsH[4,4];
real hidden grippertopFree\RelH[4,4];
real hidden grippertopFree\rotationInitial[4,4];
real grippertopFree\quat[4];
real grippertopFree\quatDot[4];
real hidden grippertopFree\quatInitial[4];
real grippertopFree\QOmega[4,4];
real grippertopFree\vgen[6];
real grippertopFree\x[3];
real grippertopFree\v[3];
real grippertopFree\vel[3];
real grippertopFree_Rot[4,4];
real hidden grippertopFree\ConnectionPoint2\AbsH[4,4];
real hidden grippertop\AbsH[4,4];
real hidden grippertop\mass_x_I[6,6];
real hidden FreeJoint1\ConnectionPoint1\AbsH[4,4];
real hidden H_grippertop\ConnectionPoint1\AbsH[4,4];
real hidden grippertop2\ConnectionPoint1\AbsH[4,4];
real hidden gripperbaseFree\ConnectionPoint1\AbsH[4,4];
real hidden gripperbaseFree\AbsH[4,4];
real hidden gripperbaseFree\RelH[4,4];
real hidden gripperbaseFree\rotationInitial[4,4];
real gripperbaseFree\quat[4];
real gripperbaseFree\quatDot[4];
real hidden gripperbaseFree\quatInitial[4];
real gripperbaseFree\QOmega[4,4];
real gripperbaseFree\vgen[6];
real gripperbaseFree\x[3];
real gripperbaseFree\v[3];
real gripperbaseFree\vel[3];
real gripperbaseFree_Rot[4,4];
real hidden gripperbaseFree\ConnectionPoint2\AbsH[4,4];
real hidden gripperbase\AbsH[4,4];
real hidden gripperbase\mass_x_I[6,6];
real hidden FreeJoint1\ConnectionPoint2\AbsH[4,4];
real hidden H_gripperbottom\ConnectionPoint1\AbsH[4,4];
real hidden gripperbottom\ConnectionPoint1\AbsH[4,4];
real v[18];
real P[18];
real PDot[18];
real PDotBodies[18];
real hidden AdjX2[6,6];
real hidden AdjX2_T[6,6];
real hidden adjAdjX2_1[6,6];
real hidden adjAdjX2_2[6,6];
real hidden adjAdjX2_3[6,6];
real hidden adjAdjX2_4[6,6];
real hidden adjAdjX2_5[6,6];
real hidden adjAdjX2_6[6,6];
real hidden AdjX3[6,6];
real hidden AdjX3_T[6,6];
real hidden adjAdjX3_1[6,6];
real hidden adjAdjX3_2[6,6];
real hidden adjAdjX3_3[6,6];
real hidden adjAdjX3_4[6,6];
real hidden adjAdjX3_5[6,6];
real hidden adjAdjX3_6[6,6];
real hidden AdjX4[6,6];
real hidden AdjX4_T[6,6];
real hidden adjAdjX4_1[6,6];
real hidden adjAdjX4_2[6,6];
real hidden adjAdjX4_3[6,6];
real hidden adjAdjX4_4[6,6];
real hidden adjAdjX4_5[6,6];
real hidden adjAdjX4_6[6,6];
real hidden J1[6,18];
real hidden J2[6,18];
real hidden J3[6,18];
real hidden J4[6,18];
real hidden T1_00[6];
real hidden T1_00T[1,6];
real hidden T2_00[6];
real hidden T2_00T[1,6];
real hidden T3_00[6];
real hidden T3_00T[1,6];
real hidden T4_00[6];
real hidden T4_00T[1,6];
real hidden M1[6,6];
real hidden AdjInv1[6,6];
real hidden TM1[1,6];
real hidden M2[6,6];
real hidden AdjInv2[6,6];
real hidden TM2[1,6];
real hidden M3[6,6];
real hidden AdjInv3[6,6];
real hidden TM3[1,6];
real hidden M4[6,6];
real hidden AdjInv4[6,6];
real hidden TM4[1,6];
real MassMatrix[18,18];
real hidden M2_2[6,6];
real hidden M3_3[6,6];
real hidden M4_4[6,6];
real hidden wrenchGrav2[1,6];
real hidden wrenchGrav3[1,6];
real hidden wrenchGrav4[1,6];
real FreeJoint1\ConnectionPoint1\invAdjointH[6,6];
real FreeJoint1\ConnectionPoint1\AbsHinverse[4,4];
real FreeJoint1\ConnectionPoint2\invAdjointH[6,6];
real FreeJoint1\ConnectionPoint2\AbsHinverse[4,4];
real FreeJoint1\ConnectionPoint1_effort[1,6];
real FreeJoint1\ConnectionPoint1_flow[6];
real FreeJoint1\ConnectionPoint2_effort[1,6];
real FreeJoint1\ConnectionPoint2_flow[6];
real FreeJoint1_relH[4,4];
real FreeJoint1_relT[6];
real FreeJoint1_relW[1,6];
real FreeJoint1_f1;
real FreeJoint1_e1;
real FreeJoint1_f2;
real FreeJoint1_e2;
real FreeJoint1_f3;
real FreeJoint1_e3;
real FreeJoint1_f4;
real FreeJoint1_e4;
real FreeJoint1_f5;
real FreeJoint1_e5;
real FreeJoint1_f6;
real FreeJoint1_e6;
real FreeJoint1_EulerXZY[3];
real FreeJoint1_EulerYXZ[3];
real FreeJoint1\SpringForce;
real FreeJoint1\DampingForce;
real markerFree\Bk[6,6];
real hidden markerFree\skewP4to6[3,3];
real TMSum2[1,6];
real grippertopFree\Bk[6,6];
real hidden grippertopFree\skewP4to6[3,3];
real TMSum3[1,6];
real gripperbaseFree\Bk[6,6];
real hidden gripperbaseFree\skewP4to6[3,3];
real TMSum4[1,6];
parameters
real hidden World\H[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real hidden World\I[6,6] = [
0.1, 0, 0, 0, 0, 0;
0, 0.1, 0, 0, 0, 0;
0, 0, 0.1, 0, 0, 0;
0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 1];
real World\mass = 1;
real hidden holderWeld\ConnectionPoint1\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0.11;
0, 0, 1, -0.0088;
0, 0, 0, 1];
real hidden holder\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real hidden holder\I[6,6] = [
0.1, 0, 0, 0, 0, 0;
0, 0.1, 0, 0, 0, 0;
0, 0, 0.1, 0, 0, 0;
0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 1];
real holder\mass = 1;
real hidden markerFree\ConnectionPoint1\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real markerFree\EulerXYZInitial[3] = [1.5707963267949; 0; -0];
real hidden markerFree\xInitial[3] = [0; 0.1; 0];
real hidden marker\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real hidden marker\I[6,6] = [
0.00163636363636364, 0, 0, 0, 0, 0;
0, 2.6645e-005, 0, 0, 0, 0;
0, 0, 0.00163636363636364, 0, 0, 0;
0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 1];
real marker\mass = 0.011;
real hidden H_marker\ConnectionPoint1\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real hidden marker2\ConnectionPoint1\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real hidden grippertopFree\ConnectionPoint1\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real grippertopFree\EulerXYZInitial[3] = [-0; 0; -0];
real hidden grippertopFree\xInitial[3] = [0; 0.11; 0.015];
real hidden grippertop\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real hidden grippertop\I[6,6] = [
0.1, 0, 0, 0, 0, 0;
0, 0.1, 0, 0, 0, 0;
0, 0, 0.1, 0, 0, 0;
0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 1];
real grippertop\mass = 1;
real hidden FreeJoint1\ConnectionPoint1\RelH[4,4] = [
1, 0, 0, 0.05;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real hidden H_grippertop\ConnectionPoint1\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real hidden grippertop2\ConnectionPoint1\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real hidden gripperbaseFree\ConnectionPoint1\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real gripperbaseFree\EulerXYZInitial[3] = [-0; 0; -0];
real hidden gripperbaseFree\xInitial[3] = [0; 0.11; 0];
real hidden gripperbase\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real hidden gripperbase\I[6,6] = [
0.02, 0, 0, 0, 0, 0;
0, 0.02, 0, 0, 0, 0;
0, 0, 0.08, 0, 0, 0;
0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 1];
real gripperbase\mass = 0.005;
real hidden FreeJoint1\ConnectionPoint2\RelH[4,4] = [
1, 0, 0, 0.05;
0, 1, 0, 0;
0, 0, 1, 0.015;
0, 0, 0, 1];
real hidden H_gripperbottom\ConnectionPoint1\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real hidden gripperbottom\ConnectionPoint1\RelH[4,4] = [
1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
real gravity[3] = [0; 0; -9.81];
real constraintRotSpringK = 10000;
real constraintRotDamperD = 100;
real FreeJoint1\Spring = 1;
real FreeJoint1\Damping = 1;
real constraintTransSpringK = 100000;
real constraintTransDamperD = 100;
real hidden PDotInitial[18] = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0];
initialequations
markerFree\rotationInitial = dll ('EulerAngles.dll', 'HMatrixFromEulXYZr', markerFree\EulerXYZInitial);
markerFree\quatInitial = dll ('EulerAngles.dll', 'QuaternionFromHMatrix', markerFree\rotationInitial);
markerFree\RelH[4, 1:3] = 0;
markerFree\RelH[4, 4] = 1;
grippertopFree\rotationInitial = dll ('EulerAngles.dll', 'HMatrixFromEulXYZr', grippertopFree\EulerXYZInitial);
grippertopFree\quatInitial = dll ('EulerAngles.dll', 'QuaternionFromHMatrix', grippertopFree\rotationInitial);
grippertopFree\RelH[4, 1:3] = 0;
grippertopFree\RelH[4, 4] = 1;
gripperbaseFree\rotationInitial = dll ('EulerAngles.dll', 'HMatrixFromEulXYZr', gripperbaseFree\EulerXYZInitial);
gripperbaseFree\quatInitial = dll ('EulerAngles.dll', 'QuaternionFromHMatrix', gripperbaseFree\rotationInitial);
gripperbaseFree\RelH[4, 1:3] = 0;
gripperbaseFree\RelH[4, 4] = 1;
J1[1:6, 1:6] = 0;
J1[1:6, 7:12] = 0;
J1[1:6, 13:18] = 0;
J2[1:6, 7:12] = 0;
J2[1:6, 13:18] = 0;
J3[1:6, 1:6] = 0;
J3[1:6, 13:18] = 0;
J4[1:6, 1:6] = 0;
J4[1:6, 7:12] = 0;
MassMatrix[1:6, 7:12] = 0;
MassMatrix[1:6, 13:18] = 0;
MassMatrix[7:12, 1:6] = 0;
MassMatrix[7:12, 13:18] = 0;
MassMatrix[13:18, 1:6] = 0;
MassMatrix[13:18, 7:12] = 0;
markerFree\Bk[4:6, 4:6] = 0;
grippertopFree\Bk[4:6, 4:6] = 0;
gripperbaseFree\Bk[4:6, 4:6] = 0;
equations
World\AbsH = World\H;
World\mass_x_I = World\mass * World\I;
holderWeld\ConnectionPoint1\AbsH = multiplyH (World\AbsH, holderWeld\ConnectionPoint1\RelH);
holderWeld\AbsH = holderWeld\ConnectionPoint1\AbsH;
holderWeld\ConnectionPoint2\AbsH = holderWeld\AbsH;
holder\AbsH = multiplyH (holderWeld\ConnectionPoint2\AbsH, holder\RelH);
holder\mass_x_I = holder\mass * holder\I;
markerFree\ConnectionPoint1\AbsH = multiplyH (World\AbsH, markerFree\ConnectionPoint1\RelH);
markerFree\QOmega = [0, -markerFree\vgen[3], markerFree\vgen[2], -markerFree\vgen[1];
markerFree\vgen[3], 0, -markerFree\vgen[1], -markerFree\vgen[2];
-markerFree\vgen[2], markerFree\vgen[1], 0, -markerFree\vgen[3];
markerFree\vgen[1], markerFree\vgen[2], markerFree\vgen[3], 0];
markerFree\quat = int (markerFree\quatDot) + markerFree\quatInitial;
markerFree\quatDot = 0.5 * (markerFree\QOmega * markerFree\quat);
markerFree\vel = markerFree\vgen[4:6];
markerFree\v = skew (markerFree\vgen[1:3]) * markerFree\x + markerFree\vel;
markerFree\x = int (markerFree\v) + markerFree\xInitial;
markerFree_Rot = dll ('EulerAngles.dll', 'HMatrixFromQuaternion', markerFree\quat);
markerFree\RelH[1:3, 1:3] = markerFree_Rot[1:3, 1:3];
markerFree\RelH[1:3, 4] = markerFree\x;
markerFree\AbsH = multiplyH (markerFree\ConnectionPoint1\AbsH, markerFree\RelH);
markerFree\ConnectionPoint2\AbsH = markerFree\AbsH;
marker\AbsH = multiplyH (markerFree\ConnectionPoint2\AbsH, marker\RelH);
marker\mass_x_I = marker\mass * marker\I;
H_marker\ConnectionPoint1\AbsH = multiplyH (marker\AbsH, H_marker\ConnectionPoint1\RelH);
marker2\ConnectionPoint1\AbsH = multiplyH (marker\AbsH, marker2\ConnectionPoint1\RelH);
grippertopFree\ConnectionPoint1\AbsH = multiplyH (World\AbsH, grippertopFree\ConnectionPoint1\RelH);
grippertopFree\QOmega = [0, -grippertopFree\vgen[3], grippertopFree\vgen[2], -grippertopFree\vgen[1];
grippertopFree\vgen[3], 0, -grippertopFree\vgen[1], -grippertopFree\vgen[2];
-grippertopFree\vgen[2], grippertopFree\vgen[1], 0, -grippertopFree\vgen[3];
grippertopFree\vgen[1], grippertopFree\vgen[2], grippertopFree\vgen[3], 0];
grippertopFree\quat = int (grippertopFree\quatDot) + grippertopFree\quatInitial;
grippertopFree\quatDot = 0.5 * (grippertopFree\QOmega * grippertopFree\quat);
grippertopFree\vel = grippertopFree\vgen[4:6];
grippertopFree\v = skew (grippertopFree\vgen[1:3]) * grippertopFree\x + grippertopFree\vel;
grippertopFree\x = int (grippertopFree\v) + grippertopFree\xInitial;
grippertopFree_Rot = dll ('EulerAngles.dll', 'HMatrixFromQuaternion', grippertopFree\quat);
grippertopFree\RelH[1:3, 1:3] = grippertopFree_Rot[1:3, 1:3];
grippertopFree\RelH[1:3, 4] = grippertopFree\x;
grippertopFree\AbsH = multiplyH (grippertopFree\ConnectionPoint1\AbsH, grippertopFree\RelH);
grippertopFree\ConnectionPoint2\AbsH = grippertopFree\AbsH;
grippertop\AbsH = multiplyH (grippertopFree\ConnectionPoint2\AbsH, grippertop\RelH);
grippertop\mass_x_I = grippertop\mass * grippertop\I;
FreeJoint1\ConnectionPoint1\AbsH = multiplyH (grippertop\AbsH, FreeJoint1\ConnectionPoint1\RelH);
H_grippertop\ConnectionPoint1\AbsH = multiplyH (grippertop\AbsH, H_grippertop\ConnectionPoint1\RelH);
grippertop2\ConnectionPoint1\AbsH = multiplyH (grippertop\AbsH, grippertop2\ConnectionPoint1\RelH);
gripperbaseFree\ConnectionPoint1\AbsH = multiplyH (World\AbsH, gripperbaseFree\ConnectionPoint1\RelH);
gripperbaseFree\QOmega = [0, -gripperbaseFree\vgen[3], gripperbaseFree\vgen[2], -gripperbaseFree\vgen[1];
gripperbaseFree\vgen[3], 0, -gripperbaseFree\vgen[1], -gripperbaseFree\vgen[2];
-gripperbaseFree\vgen[2], gripperbaseFree\vgen[1], 0, -gripperbaseFree\vgen[3];
gripperbaseFree\vgen[1], gripperbaseFree\vgen[2], gripperbaseFree\vgen[3], 0];
gripperbaseFree\quat = int (gripperbaseFree\quatDot) + gripperbaseFree\quatInitial;
gripperbaseFree\quatDot = 0.5 * (gripperbaseFree\QOmega * gripperbaseFree\quat);
gripperbaseFree\vel = gripperbaseFree\vgen[4:6];
gripperbaseFree\v = skew (gripperbaseFree\vgen[1:3]) * gripperbaseFree\x + gripperbaseFree\vel;
gripperbaseFree\x = int (gripperbaseFree\v) + gripperbaseFree\xInitial;
gripperbaseFree_Rot = dll ('EulerAngles.dll', 'HMatrixFromQuaternion', gripperbaseFree\quat);
gripperbaseFree\RelH[1:3, 1:3] = gripperbaseFree_Rot[1:3, 1:3];
gripperbaseFree\RelH[1:3, 4] = gripperbaseFree\x;
gripperbaseFree\AbsH = multiplyH (gripperbaseFree\ConnectionPoint1\AbsH, gripperbaseFree\RelH);
gripperbaseFree\ConnectionPoint2\AbsH = gripperbaseFree\AbsH;
gripperbase\AbsH = multiplyH (gripperbaseFree\ConnectionPoint2\AbsH, gripperbase\RelH);
gripperbase\mass_x_I = gripperbase\mass * gripperbase\I;
FreeJoint1\ConnectionPoint2\AbsH = multiplyH (gripperbase\AbsH, FreeJoint1\ConnectionPoint2\RelH);
H_gripperbottom\ConnectionPoint1\AbsH = multiplyH (gripperbase\AbsH, H_gripperbottom\ConnectionPoint1\RelH);
gripperbottom\ConnectionPoint1\AbsH = multiplyH (gripperbase\AbsH, gripperbottom\ConnectionPoint1\RelH);
AdjX2 = Adjoint (markerFree\ConnectionPoint1\AbsH);
AdjX2_T = transpose (AdjX2);
adjAdjX2_1 = adjoint (AdjX2[1:6, 1]);
adjAdjX2_2 = adjoint (AdjX2[1:6, 2]);
adjAdjX2_3 = adjoint (AdjX2[1:6, 3]);
adjAdjX2_4 = adjoint (AdjX2[1:6, 4]);
adjAdjX2_5 = adjoint (AdjX2[1:6, 5]);
adjAdjX2_6 = adjoint (AdjX2[1:6, 6]);
AdjX3 = Adjoint (grippertopFree\ConnectionPoint1\AbsH);
AdjX3_T = transpose (AdjX3);
adjAdjX3_1 = adjoint (AdjX3[1:6, 1]);
adjAdjX3_2 = adjoint (AdjX3[1:6, 2]);
adjAdjX3_3 = adjoint (AdjX3[1:6, 3]);
adjAdjX3_4 = adjoint (AdjX3[1:6, 4]);
adjAdjX3_5 = adjoint (AdjX3[1:6, 5]);
adjAdjX3_6 = adjoint (AdjX3[1:6, 6]);
AdjX4 = Adjoint (gripperbaseFree\ConnectionPoint1\AbsH);
AdjX4_T = transpose (AdjX4);
adjAdjX4_1 = adjoint (AdjX4[1:6, 1]);
adjAdjX4_2 = adjoint (AdjX4[1:6, 2]);
adjAdjX4_3 = adjoint (AdjX4[1:6, 3]);
adjAdjX4_4 = adjoint (AdjX4[1:6, 4]);
adjAdjX4_5 = adjoint (AdjX4[1:6, 5]);
adjAdjX4_6 = adjoint (AdjX4[1:6, 6]);
J2[1:6, 1:6] = AdjX2;
J3[1:6, 7:12] = AdjX3;
J4[1:6, 13:18] = AdjX4;
T1_00 = J1 * v;
T1_00T = transpose (T1_00);
T2_00 = J2 * v;
T2_00T = transpose (T2_00);
T3_00 = J3 * v;
T3_00T = transpose (T3_00);
T4_00 = J4 * v;
T4_00T = transpose (T4_00);
AdjInv1 = Adjoint (inverseH (holder\AbsH));
M1 = transpose (AdjInv1) * holder\mass_x_I * AdjInv1;
TM1 = T1_00T * M1;
AdjInv2 = Adjoint (inverseH (marker\AbsH));
M2 = transpose (AdjInv2) * marker\mass_x_I * AdjInv2;
TM2 = T2_00T * M2;
AdjInv3 = Adjoint (inverseH (grippertop\AbsH));
M3 = transpose (AdjInv3) * grippertop\mass_x_I * AdjInv3;
TM3 = T3_00T * M3;
AdjInv4 = Adjoint (inverseH (gripperbase\AbsH));
M4 = transpose (AdjInv4) * gripperbase\mass_x_I * AdjInv4;
TM4 = T4_00T * M4;
M2_2 = AdjX2_T * (M2 * AdjX2);
M3_3 = AdjX3_T * (M3 * AdjX3);
M4_4 = AdjX4_T * (M4 * AdjX4);
MassMatrix[1:6, 1:6] = M2_2;
MassMatrix[7:12, 7:12] = M3_3;
MassMatrix[13:18, 13:18] = M4_4;
wrenchGrav2 = marker\mass * [gravity[3] * marker\AbsH[2, 4] - gravity[2] * marker\AbsH[3, 4], gravity[1] * marker\AbsH[3, 4] - gravity[3] * marker\AbsH[1, 4], gravity[2] * marker\AbsH[1, 4] - gravity[1] * marker\AbsH[2, 4], gravity[1], gravity[2], gravity[3]];
wrenchGrav3 = grippertop\mass * [gravity[3] * grippertop\AbsH[2, 4] - gravity[2] * grippertop\AbsH[3, 4], gravity[1] * grippertop\AbsH[3, 4] - gravity[3] * grippertop\AbsH[1, 4], gravity[2] * grippertop\AbsH[1, 4] - gravity[1] * grippertop\AbsH[2, 4], gravity[1], gravity[2], gravity[3]];
wrenchGrav4 = gripperbase\mass * [gravity[3] * gripperbase\AbsH[2, 4] - gravity[2] * gripperbase\AbsH[3, 4], gravity[1] * gripperbase\AbsH[3, 4] - gravity[3] * gripperbase\AbsH[1, 4], gravity[2] * gripperbase\AbsH[1, 4] - gravity[1] * gripperbase\AbsH[2, 4], gravity[1], gravity[2], gravity[3]];
H_marker = H_marker\ConnectionPoint1\AbsH;
marker2.f = T2_00;
H_grippertop = H_grippertop\ConnectionPoint1\AbsH;
grippertop2.f = T3_00;
H_gripperbottom = H_gripperbottom\ConnectionPoint1\AbsH;
gripperbottom.f = T4_00;
FreeJoint1\ConnectionPoint1\AbsHinverse = inverseH (FreeJoint1\ConnectionPoint1\AbsH);
FreeJoint1\ConnectionPoint1\invAdjointH = Adjoint (FreeJoint1\ConnectionPoint1\AbsHinverse);
FreeJoint1\ConnectionPoint2\AbsHinverse = inverseH (FreeJoint1\ConnectionPoint2\AbsH);
FreeJoint1\ConnectionPoint2\invAdjointH = Adjoint (FreeJoint1\ConnectionPoint2\AbsHinverse);
FreeJoint1\ConnectionPoint1_flow = FreeJoint1\ConnectionPoint1\invAdjointH * T3_00;
FreeJoint1\ConnectionPoint2_flow = FreeJoint1\ConnectionPoint2\invAdjointH * T4_00;
FreeJoint1_relH = multiplyH (inverseH (FreeJoint1\ConnectionPoint1\AbsH), FreeJoint1\ConnectionPoint2\AbsH);
FreeJoint1_relT = Adjoint (FreeJoint1_relH) * FreeJoint1\ConnectionPoint2_flow - FreeJoint1\ConnectionPoint1_flow;
[FreeJoint1_f1;
FreeJoint1_f2;
FreeJoint1_f3;
FreeJoint1_f4;
FreeJoint1_f5;
FreeJoint1_f6] = FreeJoint1_relT;
FreeJoint1_EulerXZY = dll ('EulerAngles.dll', 'EulXZYrFromHMatrix', FreeJoint1_relH);
FreeJoint1_e1 = -constraintRotSpringK * FreeJoint1_EulerXZY[1] - constraintRotDamperD * FreeJoint1_f1;
FreeJoint1_EulerYXZ = dll ('EulerAngles.dll', 'EulYXZrFromHMatrix', FreeJoint1_relH);
FreeJoint1\SpringForce = -(FreeJoint1_EulerYXZ[1] * FreeJoint1\Spring);
FreeJoint1\DampingForce = -(FreeJoint1_f2 * FreeJoint1\Damping);
FreeJoint1_e2 = FreeJoint1\SpringForce + FreeJoint1\DampingForce;
FreeJoint1_e3 = -constraintRotSpringK * FreeJoint1_EulerXZY[2] - constraintRotDamperD * FreeJoint1_f3;
FreeJoint1_e4 = -constraintTransSpringK * FreeJoint1_relH[1, 4] - constraintTransDamperD * FreeJoint1_f4;
FreeJoint1_e5 = -constraintTransSpringK * FreeJoint1_relH[2, 4] - constraintTransDamperD * FreeJoint1_f5;
FreeJoint1_e6 = -constraintTransSpringK * FreeJoint1_relH[3, 4] - constraintTransDamperD * FreeJoint1_f6;
FreeJoint1_relW = [FreeJoint1_e1, FreeJoint1_e2, FreeJoint1_e3, FreeJoint1_e4, FreeJoint1_e5, FreeJoint1_e6];
FreeJoint1\ConnectionPoint1_effort = -FreeJoint1_relW;
FreeJoint1\ConnectionPoint2_effort = FreeJoint1_relW * Adjoint (FreeJoint1_relH);
markerFree\skewP4to6 = skew (P[4:6]);
markerFree\Bk[1:3, 1:3] = skew (P[1:3]);
markerFree\Bk[4:6, 1:3] = markerFree\skewP4to6;
markerFree\Bk[1:3, 4:6] = markerFree\skewP4to6;
TMSum2 = TM2;
PDotBodies[1:6] = -[TMSum2 * (adjAdjX2_1 * T2_00);
TMSum2 * (adjAdjX2_2 * T2_00);
TMSum2 * (adjAdjX2_3 * T2_00);
TMSum2 * (adjAdjX2_4 * T2_00);
TMSum2 * (adjAdjX2_5 * T2_00);
TMSum2 * (adjAdjX2_6 * T2_00)] - markerFree\Bk * v[1:6];
grippertopFree\skewP4to6 = skew (P[10:12]);
grippertopFree\Bk[1:3, 1:3] = skew (P[7:9]);
grippertopFree\Bk[4:6, 1:3] = grippertopFree\skewP4to6;
grippertopFree\Bk[1:3, 4:6] = grippertopFree\skewP4to6;
TMSum3 = TM3;
PDotBodies[7:12] = -[TMSum3 * (adjAdjX3_1 * T3_00);
TMSum3 * (adjAdjX3_2 * T3_00);
TMSum3 * (adjAdjX3_3 * T3_00);
TMSum3 * (adjAdjX3_4 * T3_00);
TMSum3 * (adjAdjX3_5 * T3_00);
TMSum3 * (adjAdjX3_6 * T3_00)] - grippertopFree\Bk * v[7:12];
gripperbaseFree\skewP4to6 = skew (P[16:18]);
gripperbaseFree\Bk[1:3, 1:3] = skew (P[13:15]);
gripperbaseFree\Bk[4:6, 1:3] = gripperbaseFree\skewP4to6;
gripperbaseFree\Bk[1:3, 4:6] = gripperbaseFree\skewP4to6;
TMSum4 = TM4;
PDotBodies[13:18] = -[TMSum4 * (adjAdjX4_1 * T4_00);
TMSum4 * (adjAdjX4_2 * T4_00);
TMSum4 * (adjAdjX4_3 * T4_00);
TMSum4 * (adjAdjX4_4 * T4_00);
TMSum4 * (adjAdjX4_5 * T4_00);
TMSum4 * (adjAdjX4_6 * T4_00)] - gripperbaseFree\Bk * v[13:18];
PDot = PDotBodies + transpose (wrenchGrav2 * J2 + wrenchGrav3 * J3 + wrenchGrav4 * J4 + transpose (marker2.e) * J2 + transpose (grippertop2.e) * J3 + transpose (gripperbottom.e) * J4 + FreeJoint1\ConnectionPoint1_effort * FreeJoint1\ConnectionPoint1\invAdjointH * J3 + FreeJoint1\ConnectionPoint2_effort * FreeJoint1\ConnectionPoint2\invAdjointH * J4);
P = int (PDot) + PDotInitial;
v = linsolve (MassMatrix, P, 'cholesky');
markerFree\vgen = v[1:6];
grippertopFree\vgen = v[7:12];
gripperbaseFree\vgen = v[13:18];
implementation_end;
end;
connections
end;
implementation_end;
]]>
</Sidops>
</Model>
<Experiments>
<DefaultExperiment><![CDATA[Experiment 1]]>
</DefaultExperiment>
<Experiment>
<Name>Experiment 1</Name>
<CreatedBy></CreatedBy>
<Info></Info>
<ExpData>
<VersionNumber>4.8</VersionNumber>
<ModelProperties>
</ModelProperties>
<Variables>
<Parameters>
</Parameters>
</Variables>
<PlotSpecs>
<VarNames>
<VarName>time</VarName>
</VarNames>
<Plots>
<Plot>
<PlotType>GraphPlot</PlotType>
<BasePlot>
<PlotId>1</PlotId>
<UseWindowsBGColor>false</UseWindowsBGColor>
<BGColor>16777215</BGColor>
<PlotIsVisible>true</PlotIsVisible>
</BasePlot>
<Grid>
<DrawGrid>true</DrawGrid>
<GridColor>15780518</GridColor>
<GridBorderLineColor>12624260</GridBorderLineColor>
<GridZeroLineColor>0</GridZeroLineColor>
<XTicks>10</XTicks>
<YTicks>10</YTicks>
<ZTicks>10</ZTicks>
<Use3DLook>false</Use3DLook>
</Grid>
<PlotBGColor>16777215</PlotBGColor>
<ShowPlotTitle>true</ShowPlotTitle>
<TitlePosition>1</TitlePosition>
<PlotTitle>endeffector</PlotTitle>
<ShowXValues>true</ShowXValues>
<Fonts>
<TitleFont>
<Name>Arial</Name>
<Height>12</Height>
<PitchFamily>34</PitchFamily>
<Weight>400</Weight>
<Italic>0</Italic>
<UnderLine>0</UnderLine>
<StrikeOut>0</StrikeOut>
<Color>0</Color>
</TitleFont>
<LabelFont>
<Name>Arial</Name>
<Height>12</Height>
<PitchFamily>34</PitchFamily>
<Weight>400</Weight>
<Italic>0</Italic>
<UnderLine>0</UnderLine>
<StrikeOut>0</StrikeOut>
<Color>0</Color>
</LabelFont>
<ValuesFont>
<Name>Arial</Name>
<Height>10</Height>
<PitchFamily>34</PitchFamily>
<Weight>400</Weight>
<Italic>0</Italic>
<UnderLine>0</UnderLine>
<StrikeOut>0</StrikeOut>
<Color>0</Color>
</ValuesFont>
<LegendFont>
<Name>Arial</Name>
<Height>12</Height>
<PitchFamily>34</PitchFamily>
<Weight>400</Weight>
<Italic>0</Italic>
<UnderLine>0</UnderLine>
<StrikeOut>0</StrikeOut>
<Color>0</Color>
</LegendFont>
</Fonts>
<SharedXAxis>true</SharedXAxis>
<SharedYAxis>true</SharedYAxis>
<SharedZAxis>false</SharedZAxis>
<XAxes>
<Axis>
<Minimum>0.0</Minimum>
<Maximum>10.0</Maximum>
<Linear>true</Linear>
<Scaling>3</Scaling>
<Label>time</Label>
</Axis>
</XAxes>
<YAxes>
<Axis>
<Minimum>0.0</Minimum>
<Maximum>10.0</Maximum>
<Linear>true</Linear>
<Scaling>2</Scaling>
<Label></Label>
</Axis>
</YAxes>
<ZAxes>
</ZAxes>
<Curves>
<Curve>
<LineColor>3355111</LineColor>
<LineStyle>1</LineStyle>
<TickColor>3355111</TickColor>
<TickStyle>0</TickStyle>
<CurveVisible>true</CurveVisible>
<PixelThresshold>1</PixelThresshold>
<LineThickness>1</LineThickness>
<LineOrder>1</LineOrder>
<ShowYValues>true</ShowYValues>
<XCurveData>
<ShowUnit>true</ShowUnit>
<VarName>time</VarName>
</XCurveData>
</Curve>
</Curves>
<Legenda>
<ShowLegenda>true</ShowLegenda>
<BorderPenColor>0</BorderPenColor>
<BackgroundColor>16777215</BackgroundColor>
</Legenda>
</Plot>
</Plots>
<PlotPanels>
<PlotPanel>
<PlotPanelId>1</PlotPanelId>
<PlotPanelVisible>true</PlotPanelVisible>
<Name>Window 1</Name>
<Tiling>0</Tiling>
<PlotIds>
<PlotId>1</PlotId>
</PlotIds>
<ToggleState>Base</ToggleState>
</PlotPanel>
</PlotPanels>
</PlotSpecs>
<RunSpecs>
<SimulatorSettings>
<StartTime>0.0</StartTime>
<FinishTime>10.0</FinishTime>
<AllowPassFinishTime>false</AllowPassFinishTime>
<Warp>false</Warp>
<FPGonio>false</FPGonio>
<UseOutputAfterEach>false</UseOutputAfterEach>
<OutputAfterEach>0.1</OutputAfterEach>
<EventEpsilon>1.0e-6</EventEpsilon>
<AlgebraicTolerance>1.0e-7</AlgebraicTolerance>
<SteadyStateAnalysis>false</SteadyStateAnalysis>
<UpdateHoldInstructions>true</UpdateHoldInstructions>
</SimulatorSettings>
<IntegrationMethods>
<IntegrationMethod>
<Name>Euler</Name>
<StepSize>0.01</StepSize>
<AutoStepSize>false</AutoStepSize>
</IntegrationMethod>
<IntegrationMethod>
<Name>BackwardEuler</Name>
<AbsoluteTolerance>1.0e-5</AbsoluteTolerance>
<RelativeTolerance>1.0e-5</RelativeTolerance>
<AlgebraicAbsoluteTolerance>1.0e-5</AlgebraicAbsoluteTolerance>
<AlgebraicRelativeTolerance>1.0e-5</AlgebraicRelativeTolerance>
<StepSize>0.01</StepSize>
<Alpha>1.0</Alpha>
</IntegrationMethod>
<IntegrationMethod>
<Name>AdamsBashforth</Name>
<StepSize>0.01</StepSize>
<AutoStepSize>false</AutoStepSize>
</IntegrationMethod>
<IntegrationMethod>
<Name>RungeKutta2</Name>
<StepSize>0.01</StepSize>
<AutoStepSize>false</AutoStepSize>
</IntegrationMethod>
<IntegrationMethod>
<Name>RungeKutta4</Name>
<StepSize>0.01</StepSize>
<AutoStepSize>false</AutoStepSize>
</IntegrationMethod>
<IntegrationMethod>
<Name>RungeKutta8</Name>
<UseInitialStepSize>false</UseInitialStepSize>
<InitialStepSize>0.001</InitialStepSize>
<UseMaximumStepSize>false</UseMaximumStepSize>
<MaximumStepSize>1.0</MaximumStepSize>
<AbsoluteTolerance>1.0e-6</AbsoluteTolerance>
<RelativeTolerance>1.0e-6</RelativeTolerance>
<SafetyFactor>0.9</SafetyFactor>
<Factor1>0.33</Factor1>
<Factor2>6.0</Factor2>
<Beta>0.0</Beta>
<UseMaxNrSteps>false</UseMaxNrSteps>
<MaxNrSteps>100000</MaxNrSteps>
<UseStiffDetection>false</UseStiffDetection>
<MaxNrStiffnessSteps>1000</MaxNrStiffnessSteps>
</IntegrationMethod>
<IntegrationMethod>
<Name>RungeKuttaFehlberg</Name>
<UseInitialStepSize>false</UseInitialStepSize>
<InitialStepSize>0.001</InitialStepSize>
<UseMaximumStepSize>false</UseMaximumStepSize>
<MaximumStepSize>1.0</MaximumStepSize>
<AbsoluteTolerance>1.0e-6</AbsoluteTolerance>
<RelativeTolerance>1.0e-6</RelativeTolerance>
</IntegrationMethod>
<IntegrationMethod>
<Name>VodeAdams</Name>
<UseInitialStepSize>false</UseInitialStepSize>
<InitialStepSize>0.001</InitialStepSize>
<UseMaximumStepSize>false</UseMaximumStepSize>
<MaximumStepSize>1.0</MaximumStepSize>
<AbsoluteTolerance>1.0e-6</AbsoluteTolerance>
<RelativeTolerance>1.0e-6</RelativeTolerance>
<VodeUseBDF>true</VodeUseBDF>
<VodeUseNewton>true</VodeUseNewton>
</IntegrationMethod>
<IntegrationMethod>
<Name>BDFMethod</Name>
<AbsoluteTolerance>1.0e-5</AbsoluteTolerance>
<RelativeTolerance>1.0e-5</RelativeTolerance>
<AlgebraicAbsoluteTolerance>1.0e-5</AlgebraicAbsoluteTolerance>
<AlgebraicRelativeTolerance>1.0e-5</AlgebraicRelativeTolerance>
<UseInitialStepSize>false</UseInitialStepSize>
<InitialStepSize>0.001</InitialStepSize>
<UseMaximumStepSize>false</UseMaximumStepSize>
<MaximumStepSize>1.0</MaximumStepSize>
</IntegrationMethod>
<IntegrationMethod>
<Name>MeBDFiMethod</Name>
<AbsoluteTolerance>1.0e-5</AbsoluteTolerance>
<RelativeTolerance>1.0e-5</RelativeTolerance>
<AlgebraicAbsoluteTolerance>1.0e-5</AlgebraicAbsoluteTolerance>
<AlgebraicRelativeTolerance>1.0e-5</AlgebraicRelativeTolerance>
<UseInitialStepSize>false</UseInitialStepSize>
<InitialStepSize>0.001</InitialStepSize>
<UseMaximumStepSize>false</UseMaximumStepSize>
<MaximumStepSize>1.0</MaximumStepSize>
</IntegrationMethod>
<SelectedIntegrationMethod>8</SelectedIntegrationMethod>
</IntegrationMethods>
</RunSpecs>
<MultipleRun>
<NrSteps>10</NrSteps>
<CopyFromStates>false</CopyFromStates>
<JoinParameterVariation>true</JoinParameterVariation>
<ClearAfterRun>true</ClearAfterRun>
<RedrawAfterRun>false</RedrawAfterRun>
<DrawDuringSimulation>true</DrawDuringSimulation>
<ActionBeforeOptimization>0</ActionBeforeOptimization>
<CompareValue>0.0</CompareValue>
<UseCompareValue>true</UseCompareValue>
<MultipleRunType>MultipleRun</MultipleRunType>
<Minimize>true</Minimize>
<OptimizationVariable></OptimizationVariable>
<ResulVarUsage>UseEndValue</ResulVarUsage>
<Tolerance>0.001</Tolerance>
<OptimizationMethod>BroydonFletcherGoldfarbShanno</OptimizationMethod>
<MultipleRunVariables>
</MultipleRunVariables>
</MultipleRun>
<ExportData>
<WriteAsText>true</WriteAsText>
<ReadAsText>true</ReadAsText>
<WriteHeader>true</WriteHeader>
<ReadHeader>true</ReadHeader>
<ReadFilename></ReadFilename>
<WriteFilename></WriteFilename>
<DoWrite>false</DoWrite>
<ExportVariables>
<ExportVariable>
<VariableName>time</VariableName>
</ExportVariable>
</ExportVariables>
<ImportVariables>
</ImportVariables>
</ExportData>
<BreakPoints>
</BreakPoints>
</ExpData>
</Experiment>
</Experiments>
</Document>

+ 37
- 0
implementation/submodel/endeffector/gripper.scad Просмотреть файл

@@ -0,0 +1,37 @@
module my_cylinder(length, diameter, position)
{
translate(position) rotate([90,0,0]) cylinder(h=length,d=diameter, center=true, $fn=15);
}

// marker
my_cylinder(0.15,0.015,[0,0.10,0]);


// holder
offset = 0.0088;
fromboard1 = 0.11;
diameter = 0.01;
width = 0.04;
translate([0,fromboard1,-offset])
translate([0,0,0])
{
my_cylinder(0.02,diameter,[offset, width, 0]);
my_cylinder(0.02,diameter,[-offset, width, 0]);
my_cylinder(0.02,diameter,[offset,-width, 0]);
my_cylinder(0.02,diameter,[-offset,-width, 0]);
my_cylinder(0.04,diameter,[-2*offset,0, 1.4*offset]);
translate([0,0,-0.51])
cube([1,1,1], center=true);
};

//gripper
translate([0,fromboard1,0])
translate([0,0,0]){
my_cylinder(0.03,0.01,[offset, 0, -offset]);
my_cylinder(0.03,0.01,[-offset, 0, -offset]);
translate([0,0,0.015])
cube([0.07,0.04,0.01],center=true);
};




Редактирование Предпросмотр
Загрузка…
Отмена
Сохранить