<?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>
