4.8 0 False C:\users\wouter\My Documents\studie\ma\mahd\implementation\submodel\endeffector\endeffector.emx 2020-7-6 14:39:32 '; type Mainmodel end; implementation bg submodels endeffector 592 296 description ' 1 True 4.8 Model generated by 3D-Mechanics Toolbox Original Filename: C:\users\wouter\My Documents\studie\ma\mahd\implementation\submodel\endeffector\endeffector.3dm C:\users\wouter\My Documents\studie\ma\mahd\implementation\submodel\endeffector\endeffector.emx 2020-7-6 14:12:40 '; 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; ]]> Experiment 1 4.8 time GraphPlot 1 false 16777215 true true 15780518 12624260 0 10 10 10 false 16777215 true 1 endeffector true Arial 12 34 400 0 0 0 0 Arial 12 34 400 0 0 0 0 Arial 10 34 400 0 0 0 0 Arial 12 34 400 0 0 0 0 true true false 0.0 10.0 true 3 time 0.0 10.0 true 2 3355111 1 3355111 0 true 1 1 1 true true time true 0 16777215 1 true Window 1 0 1 Base 0.0 10.0 false false false false 0.1 1.0e-6 1.0e-7 false true Euler 0.01 false BackwardEuler 1.0e-5 1.0e-5 1.0e-5 1.0e-5 0.01 1.0 AdamsBashforth 0.01 false RungeKutta2 0.01 false RungeKutta4 0.01 false RungeKutta8 false 0.001 false 1.0 1.0e-6 1.0e-6 0.9 0.33 6.0 0.0 false 100000 false 1000 RungeKuttaFehlberg false 0.001 false 1.0 1.0e-6 1.0e-6 VodeAdams false 0.001 false 1.0 1.0e-6 1.0e-6 true true BDFMethod 1.0e-5 1.0e-5 1.0e-5 1.0e-5 false 0.001 false 1.0 MeBDFiMethod 1.0e-5 1.0e-5 1.0e-5 1.0e-5 false 0.001 false 1.0 8 10 false true true false true 0 0.0 true MultipleRun true UseEndValue 0.001 BroydonFletcherGoldfarbShanno true true true true false time