From d56ed5891ec9ebf32dd55a30c049e864b3dd5608 Mon Sep 17 00:00:00 2001 From: Wouter Horlings Date: Fri, 3 Jul 2020 15:32:18 +0200 Subject: [PATCH] Add end-effector 3D-model. --- .../submodel/endeffector/endeffector.3dm | 3990 +++++++++++++++++ .../submodel/endeffector/endeffector.emx | 877 ++++ .../submodel/endeffector/gripper.scad | 37 + 3 files changed, 4904 insertions(+) create mode 100644 implementation/submodel/endeffector/endeffector.3dm create mode 100644 implementation/submodel/endeffector/endeffector.emx create mode 100644 implementation/submodel/endeffector/gripper.scad diff --git a/implementation/submodel/endeffector/endeffector.3dm b/implementation/submodel/endeffector/endeffector.3dm new file mode 100644 index 0000000..f32562d --- /dev/null +++ b/implementation/submodel/endeffector/endeffector.3dm @@ -0,0 +1,3990 @@ + + 4.8.2.10124 + + 0.05 + true + 0.05 + false + + true + Obj0 + Reference Frame + + + + + + + Obj1 + Camera + + + 10 + + + 0.5 + + + 0.499999999999999 + + + + + + + -1 + + + + + 1 + + + + + true + true + + + 0.01 + 100 + true + 45 + -7.205759403792789 + 7.205759403792789 + 7.205759403792789 + -7.205759403792789 + false + + + true + + + 1 + + + true + false + 3 + + + Obj2 + Camera + + + -0.1 + + + 10 + + + 2.77555756156289e-017 + + + + + + + -1 + + + + + 1 + + + + + true + true + + + 0.01 + 100 + true + 45 + -0.2283596308329529 + 0.2283596308329529 + 0.2283596308329529 + -0.2283596308329529 + false + + + true + + + 1 + + + true + false + 2 + + + Obj3 + Camera + + + 0.200000000000003 + + + 0.1 + + + 10 + + + + + + + -1 + + + + + 1 + + + + + true + true + + + 0.01 + 100 + true + 45 + -1.214168057641075 + 1.214168057641075 + 1.214168057641075 + -1.214168057641075 + false + + + true + + + 1 + + + true + false + 1 + + + Obj4 + Camera + + + 0.0610631862571911 + + + -0.0685619239998103 + + + 0.0788058886211784 + + + + + + + -0.504674800251261 + + + 0.566650340742276 + + + -0.651314622381544 + + + + + -0.433182264518302 + + + 0.486378312669067 + + + 0.758807790334276 + + + + + + + 0.0005 + 5 + true + 45 + -10 + 10 + 10 + -10 + true + + + true + + + 1 + + + true + true + 0 + true + + + true + Body4 + Reference Frame + + + + + true + true + true + true + true + true + + + KP1\Hinge0 + Reference Frame + + + 0.11 + + + -0.0088 + + + + + + + true + true + true + true + true + true + + + KP1 + Reference Frame + + + + + true + true + true + true + true + true + + + KP1\Hinge1 + Reference Frame + + + + + true + true + true + true + true + true + + + true + true + Body0 + holder + + + + + true + true + true + true + true + true + + + Body0_Sub0 + Body + + + + + true + true + true + true + true + true + + + + + 0.184313725490196 + + + 0.803921568627451 + + + 0.184313725490196 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + + + false + false + false + true + false + + + + + + + + + + + KP2\Hinge0 + Reference Frame + + + + + + + KP2 + Reference Frame + + + 0.1 + + + + + + + 1.5707963267949 + + + + + + + KP2\Hinge1 + Reference Frame + + + + + + + true + Body1 + marker + + + + + true + true + true + true + true + true + + + Body1_Sub0 + Body + + + + + true + true + true + true + true + true + + + + + 0 + + + 0 + + + 1 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.0075 + 0.0075 + 0.135 + false + true + + + Port0\Hinge0 + Frame + + + + + true + true + true + true + true + true + + + true + Port0\Hinge0_Sub0 + Cylinder + + + 0.2 + + + + + + + true + true + true + true + true + true + + + + + + + 0 + + + 0 + + + 1 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0.5 + 1 + true + true + + + true + Port0 + Frame + + + + + true + true + true + true + true + true + + + + + true + Port0\Hinge0_Sub1 + Block + + + 0.4 + + + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 0 + + + 0 + + + 1 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + Center + 1 + 4 + false + + + + + true + Port1\Hinge0 + Frame + + + + + true + true + true + true + true + true + + + true + Port1\Hinge0_Sub0 + Cylinder + + + 0.3 + + + + + + + 1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.1 + + + 0.1 + + + 0.4 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0.5 + 1 + true + true + + + true + Port1 + Frame + + + + + true + true + true + true + true + true + + + + + Port1\Hinge0_Sub1 + Block + + + 0.1 + + + + + + + -1.5707963267949 + + + + + true + true + true + true + true + true + + + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0 + 1 + false + true + + + Port1\Hinge0_Sub2 + Block + + + 0.2 + + + + + + + -1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0.75 + + + 0.75 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0 + 1 + false + true + + + Port1\Hinge0_Sub3 + Cylinder + + + 0.3 + + + + + + + -1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.1 + + + 0.1 + + + 0.4 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0.5 + 1 + true + true + + + Obj9 + Frame + + + + + true + true + true + true + true + true + + + + + Port1\Hinge0_Sub4 + Block + + + 0.1 + + + + + + + 1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0 + 1 + false + true + + + Port1\Hinge0_Sub5 + Block + + + 0.2 + + + + + + + 1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0.75 + + + 0.75 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0 + 1 + false + true + + + Port1\Hinge0_Sub6 + Cylinder + + + 0.3 + + + + + + + true + true + true + true + true + true + + + + + 0.1 + + + 0.1 + + + 0.4 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0.5 + 1 + true + true + + + Obj10 + Frame + + + + + true + true + true + true + true + true + + + + + Port1\Hinge0_Sub7 + Block + + + 0.1 + + + + + + + true + true + true + true + true + true + + + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0 + 0.5 + 1 + false + true + + + Port1\Hinge0_Sub8 + Block + + + 0.2 + + + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0.75 + + + 0.75 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0 + 0.5 + 1 + false + true + + + + + + + + + + + + + KP3\Hinge0 + Reference Frame + + + + + + + KP3 + Reference Frame + + + 0.11 + + + 0.015 + + + + + + + + + KP3\Hinge1 + Reference Frame + + + + + + + true + Body2 + grippertop + + + + + true + true + true + true + true + true + + + Body2_Sub0 + Body + + + + + true + true + true + true + true + true + + + + + 0.07 + + + 0.04 + + + 0.01 + + + + + 0.188235294117647 + + + 0.803921568627451 + + + 0.188235294117647 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + Center + 1 + 4 + true + + + KP0\Hinge0 + Frame + + + 0.05 + + + + + + + true + true + true + true + true + true + + + true + KP0\Hinge0_Sub0 + Cylinder + + + + + + + + + + + 0.596078431372549 + + + 0.984313725490196 + + + 0.596078431372549 + + + 0 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + false + true + 30 + false + + + KP0 + Frame + + + + + true + true + true + true + true + + + KP0\Hinge1 + Frame + + + + + true + true + true + true + true + true + + + true + KP0\Hinge1_Sub0 + Cylinder + + + + + true + true + true + true + true + true + + + + + + + 0.250980392156863 + + + 0.87843137254902 + + + 0.815686274509804 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + false + true + 30 + false + + + true + Body3 + gripperbase + + + -0.05 + + + -0.015 + + + + + + + true + true + true + true + true + true + + + Body3_Sub0 + Body + + + + + true + true + true + true + true + true + + + + + 0.188235294117647 + + + 0.803921568627451 + + + 0.188235294117647 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + + + false + false + false + true + false + + + true + Port4\Hinge0 + Frame + + + + + true + true + true + true + true + true + + + true + Port4\Hinge0_Sub0 + Cylinder + + + 0.2 + + + + + + + true + true + true + true + true + true + + + + + 0.1 + + + 0.1 + + + 0.4 + + + + + 0 + + + 0 + + + 1 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0.5 + 1 + true + true + + + true + true + Port4 + Frame + + + + + true + true + true + true + true + true + + + + + Port4\Hinge0_Sub1 + Block + + + 0.4 + + + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 0 + + + 0 + + + 1 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + Center + 1 + 4 + false + + + + + true + Port5\Hinge0 + Frame + + + + + true + true + true + true + true + true + + + true + Port5\Hinge0_Sub0 + Cylinder + + + 0.3 + + + + + + + 1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.1 + + + 0.1 + + + 0.4 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0.5 + 1 + true + true + + + true + Port5 + Frame + + + + + true + true + true + true + true + true + + + + + Port5\Hinge0_Sub1 + Block + + + 0.1 + + + + + + + -1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0 + 1 + false + true + + + Port5\Hinge0_Sub2 + Block + + + 0.2 + + + + + + + -1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0.75 + + + 0.75 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0 + 1 + false + true + + + Port5\Hinge0_Sub3 + Cylinder + + + 0.3 + + + + + + + -1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.1 + + + 0.1 + + + 0.4 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0.5 + 1 + true + true + + + Obj11 + Frame + + + + + true + true + true + true + true + true + + + + + Port5\Hinge0_Sub4 + Block + + + 0.1 + + + + + + + 1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0 + 1 + false + true + + + Port5\Hinge0_Sub5 + Block + + + 0.2 + + + + + + + 1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0.75 + + + 0.75 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0 + 1 + false + true + + + Port5\Hinge0_Sub6 + Cylinder + + + 0.3 + + + + + + + true + true + true + true + true + true + + + + + 0.1 + + + 0.1 + + + 0.4 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0.5 + 1 + true + true + + + Obj12 + Frame + + + + + true + true + true + true + true + true + + + + + Port5\Hinge0_Sub7 + Block + + + 0.1 + + + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0 + 0.5 + 1 + false + true + + + Port5\Hinge0_Sub8 + Block + + + 0.2 + + + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0.75 + + + 0.75 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0 + 0.5 + 1 + false + true + + + + + + + + + + + + + true + Port2\Hinge0 + Frame + + + + + true + true + true + true + true + true + + + true + Port2\Hinge0_Sub0 + Cylinder + + + 0.2 + + + + + + + true + true + true + true + true + true + + + + + 0.1 + + + 0.1 + + + 0.4 + + + + + 0 + + + 0 + + + 1 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0.5 + 1 + true + true + + + true + Port2 + Frame + + + + + true + true + true + true + true + true + + + + + Port2\Hinge0_Sub1 + Block + + + 0.4 + + + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 0 + + + 0 + + + 1 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + Center + 1 + 4 + false + + + + + true + Port3\Hinge0 + Frame + + + + + true + true + true + true + true + true + + + true + Port3\Hinge0_Sub0 + Cylinder + + + 0.3 + + + + + + + 1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.1 + + + 0.1 + + + 0.4 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0.5 + 1 + true + true + + + true + Port3 + Frame + + + + + true + true + true + true + true + true + + + + + Port3\Hinge0_Sub1 + Block + + + 0.1 + + + + + + + -1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0 + 1 + false + true + + + Port3\Hinge0_Sub2 + Block + + + 0.2 + + + + + + + -1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0.75 + + + 0.75 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0 + 1 + false + true + + + Port3\Hinge0_Sub3 + Cylinder + + + 0.3 + + + + + + + -1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.1 + + + 0.1 + + + 0.4 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0.5 + 1 + true + true + + + Obj13 + Frame + + + + + true + true + true + true + true + true + + + + + Port3\Hinge0_Sub4 + Block + + + 0.1 + + + + + + + 1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0 + 1 + false + true + + + Port3\Hinge0_Sub5 + Block + + + 0.2 + + + + + + + 1.5707963267949 + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0.75 + + + 0.75 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0 + 1 + false + true + + + Port3\Hinge0_Sub6 + Cylinder + + + 0.3 + + + + + + + true + true + true + true + true + true + + + + + 0.1 + + + 0.1 + + + 0.4 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0.5 + 0.5 + 1 + true + true + + + Obj14 + Frame + + + + + true + true + true + true + true + true + + + + + Port3\Hinge0_Sub7 + Block + + + 0.1 + + + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0 + + + 0 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0 + 0.5 + 1 + false + true + + + Port3\Hinge0_Sub8 + Block + + + 0.2 + + + + + + + true + true + true + true + true + true + + + + + 0.2 + + + 0.2 + + + 0.2 + + + + + 1 + + + 0.75 + + + 0.75 + + + 1 + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + 16 + 0 + 0.5 + 1 + false + true + + + + + + + + + + + + + + + + + + + + -9.81 + + + 1 + 1 + 1.0e-5 + 0 + + + Body0 + holder + + + 0.1 + 0.1 + 0.1 + 1.0 + false + + true + true + + + Body1 + marker + + + 1.8e-5 + 2.93095e-7 + 1.8e-5 + 0.011 + false + + false + true + + + Body2 + grippertop + + + 0.1 + 0.1 + 0.1 + 1.0 + false + + false + true + + + Body3 + gripperbase + + + 1.0e-4 + 1.0e-4 + 4.0e-4 + 0.005 + false + + false + false + + + Body4 + World + + + 0.1 + 0.1 + 0.1 + 1.0 + false + + true + false + false + + + Body4 + + + + + KP0 + FreeJoint1 + + + + + KP0\Hinge0 + ConnectionPoint1 + + + + 0.05 + + + + + + + false + false + Body2 + KP0 + + + KP0\Hinge1 + ConnectionPoint2 + + + + 0.05 + + + 0.015 + + + + + + + false + false + Body3 + KP0 + + + + true + PureRotation + 1 + 1 + true + true + false + false + false + false + false + 0.0 + false + + true + 1.0 + true + 1.0 + + + + KP1 + holderWeld + + + + + KP1\Hinge0 + ConnectionPoint1 + + + + 0.11 + + + -0.0088 + + + + + + + false + false + Body4 + KP1 + + + KP1\Hinge1 + ConnectionPoint2 + + + + + + + + false + false + Body0 + KP1 + + + + true + Weld + 2 + 2 + false + false + false + false + false + false + false + false + + + KP2 + markerFree + + + + + KP2\Hinge0 + ConnectionPoint1 + + + + + + + + false + false + Body4 + KP2 + + + KP2\Hinge1 + ConnectionPoint2 + + + + + + + + false + false + Body1 + KP2 + + + + true + FreeMoving + 2 + 2 + false + false + false + false + false + false + false + + 1.0 + 0.0 + 0.0 + 0.0 + -3.4914813388431334e-15 + 1.0 + 0.0 + -1.0 + -3.4914813388431334e-15 + 0.0 + 0.1 + 0.0 + + false + + + KP3 + grippertopFree + + + + + KP3\Hinge0 + ConnectionPoint1 + + + + + + + + false + false + Body4 + KP3 + + + KP3\Hinge1 + ConnectionPoint2 + + + + + + + + false + false + Body2 + KP3 + + + + true + FreeMoving + 2 + 2 + false + false + false + false + false + false + false + + 1.0 + -0.0 + 0.0 + 0.0 + 1.0 + -0.0 + 0.0 + 0.0 + 1.0 + 0.0 + 0.11 + 0.015 + + false + + + KP1 + KP2 + KP3 + + + + + Port0 + H_marker + + + + Port0\Hinge0 + ConnectionPoint1 + + + + + + + + false + false + Body1 + Port0 + + + false + OutputHMatrix + 1 + + + Port1 + marker2 + + + + Port1\Hinge0 + ConnectionPoint1 + + + + + + + + false + false + Body1 + Port1 + + + true + InputWrenchFull + 1 + + + Port2 + H_grippertop + + + + Port2\Hinge0 + ConnectionPoint1 + + + + + + + + false + false + Body2 + Port2 + + + false + OutputHMatrix + 1 + + + Port3 + grippertop2 + + + + Port3\Hinge0 + ConnectionPoint1 + + + + + + + + false + false + Body2 + Port3 + + + true + InputWrenchFull + 1 + + + Port4 + H_gripperbottom + + + + Port4\Hinge0 + ConnectionPoint1 + + + + + + + + false + false + Body3 + Port4 + + + false + OutputHMatrix + 1 + + + Port5 + gripperbottom + + + + Port5\Hinge0 + ConnectionPoint1 + + + + + + + + false + false + Body3 + Port5 + + + true + InputWrenchFull + 1 + + + true + true + false + false + false + false + false + false + + + + + endeffector + false + true + + true + true + 10000.0 + 100.0 + 100000.0 + 100.0 + + + + + + + Obj1 + + + Obj2 + + + Obj3 + + + Obj4 + + + + diff --git a/implementation/submodel/endeffector/endeffector.emx b/implementation/submodel/endeffector/endeffector.emx new file mode 100644 index 0000000..5487900 --- /dev/null +++ b/implementation/submodel/endeffector/endeffector.emx @@ -0,0 +1,877 @@ + + + + + + 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 + + + + + + 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 + + + + + + + + + + + diff --git a/implementation/submodel/endeffector/gripper.scad b/implementation/submodel/endeffector/gripper.scad new file mode 100644 index 0000000..233f4bc --- /dev/null +++ b/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); +}; + + +