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);
+};
+
+
+