working parameterisation of toe/foot ratio with lua

LievenPetersen · LievenPetersen · commit 8e864d793ccf · 2024-06-18T15:57:49.000+02:00
diff --git a/bitbots_wolfgang/wolfgang_webots_sim/protos/robots/Wolfgang/Wolfgang.proto b/bitbots_wolfgang/wolfgang_webots_sim/protos/robots/Wolfgang/Wolfgang.proto
@@ -107,6 +107,13 @@ PROTO Wolfgang [
     recognitionColors [
       0.2 0.2 0.2
     ]
+    %{
+      TOE_FRACTION = 0.3
+      FOOT_LENGTH = 0.18
+      FOOT_MASS = 0.26824
+      TOE_LENGTH = FOOT_LENGTH * TOE_FRACTION
+      REAR_FOOT_LENGTH = FOOT_LENGTH * (1-TOE_FRACTION)
+    }%
     %{
       if fields.name.value ~= '' then
         local color = "none"
@@ -1941,7 +1948,7 @@ PROTO Wolfgang [
                                           }
                                           # right sole visual
                                           Transform {
-                                            translation 0.0145000 0.05500 -0.034
+                                            translation 0.0145000 %{=-0.005 + 0.5*REAR_FOOT_LENGTH}% -0.034
                                             rotation -0.000000 -0.000000 -1.000000 1.570800
                                             children [
                                               Shape{
@@ -1951,7 +1958,7 @@ PROTO Wolfgang [
                                                   }
                                                 }
                                                 geometry Box {
-                                                    size 0.12000 0.106000 0.0100
+                                                    size %{=REAR_FOOT_LENGTH}% 0.106000 0.0100
                                                 }
                                               }
                                             ]
@@ -2133,16 +2140,18 @@ PROTO Wolfgang [
                                           # right toe joint
                                           DEF FlexiJointL HingeJoint {
                                             jointParameters HingeJointParameters {
-                                              anchor 0.0 0.115 -0.034
+                                              anchor 0.0 %{=-0.005 + REAR_FOOT_LENGTH}% -0.034
                                               axis 1 0 0
                                               springConstant IS toe-spring-constant
                                               minStop 0
                                               maxStop 0.5
                                             }
                                             # right toe visual, collision & physics
+                                            # endPoint is relative to foot base
                                             endPoint DEF l_flexi Solid {
-                                              translation 0.0145 0.145 -0.034
+                                              translation 0.0145 %{=-0.005 + REAR_FOOT_LENGTH + 0.5*TOE_LENGTH}% -0.034 # y = forward
                                               rotation 0.0 0.0 -1.0 1.5708
+                                              # for children x = forward
                                               children [
                                                 Shape {
                                                   appearance Appearance {
@@ -2151,12 +2160,12 @@ PROTO Wolfgang [
                                                     }
                                                   }
                                                   geometry Box {
-                                                    size 0.06 0.106 0.01
+                                                    size %{=TOE_LENGTH}% 0.106 0.01
                                                   }
                                                 }
                                               ]
                                               boundingObject Box {
-                                                size 0.06 0.106 0.01
+                                                size %{=TOE_LENGTH}% 0.106 0.01
                                               }
                                               physics Physics {
                                                 density -1
@@ -2171,11 +2180,11 @@ PROTO Wolfgang [
                                         boundingObject Group {
                                           children [
                                             Transform {
-                                              translation 0.0145000 0.08500 -0.034
+                                              translation 0.0145000 %{=-0.005 + REAR_FOOT_LENGTH/2}% -0.034
                                               rotation -0.000000 -0.000000 -1.000000 1.570800
                                               children [
                                                 Box {
-                                                   size 0.18000 0.106000 0.0100
+                                                   size %{=REAR_FOOT_LENGTH}% 0.106000 0.0100
                                                 }
                                               ]
                                             }
@@ -3351,7 +3360,7 @@ PROTO Wolfgang [
                                           }
                                           # left foot sole visual
                                           Transform {
-                                            translation -0.0145000 0.05500 -0.034
+                                            translation -0.0145000 %{=-0.005 + 0.5*REAR_FOOT_LENGTH}% -0.034
                                             rotation 0.000000 0.000000 -1.000000 1.570800
                                             children [
                                               Shape{
@@ -3361,7 +3370,7 @@ PROTO Wolfgang [
                                                   }
                                                 }
                                                 geometry Box {
-                                                    size 0.12000 0.106000 0.0100
+                                                    size %{=REAR_FOOT_LENGTH}% 0.106000 0.0100
                                                 }
                                               }
                                             ]
@@ -3559,15 +3568,15 @@ PROTO Wolfgang [
                                           # left toe joint
                                           DEF FlexiJointL HingeJoint {
                                             jointParameters HingeJointParameters {
-                                              anchor 0.0 0.115 -0.034
+                                              anchor 0.0 %{=-0.005 + REAR_FOOT_LENGTH}% -0.034
                                               axis 1 0 0
                                               springConstant IS toe-spring-constant
                                               minStop 0
                                               maxStop 0.5
                                             }
                                             # left toe visual, collision & physics
                                             endPoint DEF l_flexi Solid {
-                                              translation -0.0145 0.145 -0.034
+                                              translation -0.0145 %{=-0.005 + REAR_FOOT_LENGTH + 0.5*TOE_LENGTH}% -0.034 # y = forward
                                               rotation 0.0 0.0 -1.0 1.5708
                                               children [
                                                 Shape {
@@ -3577,12 +3586,12 @@ PROTO Wolfgang [
                                                     }
                                                   }
                                                   geometry Box {
-                                                    size 0.06 0.106 0.01
+                                                    size %{=TOE_LENGTH}% 0.106 0.01
                                                   }
                                                 }
                                               ]
                                               boundingObject Box {
-                                                size 0.06 0.106 0.01
+                                                size %{=TOE_LENGTH}% 0.106 0.01
                                               }
                                               physics Physics {
                                                 density -1
@@ -3597,11 +3606,11 @@ PROTO Wolfgang [
                                         boundingObject Group {
                                           children [
                                             Transform {
-                                              translation -0.0145000 0.05500 -0.034
+                                              translation -0.0145000 %{=-0.005 + REAR_FOOT_LENGTH/2}% -0.034
                                               rotation 0.000000 0.000000 -1.000000 1.570800
                                               children [
                                                 Box {
-                                                   size 0.12000 0.106000 0.0100
+                                                   size %{=REAR_FOOT_LENGTH}% 0.106000 0.0100
                                                 }
                                               ]
                                             }

Original file line number	Diff line number	Diff line change
`@@ -107,6 +107,13 @@ PROTO Wolfgang [`
`107`	`107`	`recognitionColors [`
`108`	`108`	`0.2 0.2 0.2`
`109`	`109`	`]`
	`110`	`+ %{`
	`111`	`+ TOE_FRACTION = 0.3`
	`112`	`+ FOOT_LENGTH = 0.18`
	`113`	`+ FOOT_MASS = 0.26824`
	`114`	`+ TOE_LENGTH = FOOT_LENGTH * TOE_FRACTION`
	`115`	`+ REAR_FOOT_LENGTH = FOOT_LENGTH * (1-TOE_FRACTION)`
	`116`	`+ }%`
`110`	`117`	`%{`
`111`	`118`	`if fields.name.value ~= '' then`
`112`	`119`	`local color = "none"`
`@@ -1941,7 +1948,7 @@ PROTO Wolfgang [`
`1941`	`1948`	`}`
`1942`	`1949`	`# right sole visual`
`1943`	`1950`	`Transform {`
`1944`		`- translation 0.0145000 0.05500 -0.034`
	`1951`	`+ translation 0.0145000 %{=-0.005 + 0.5*REAR_FOOT_LENGTH}% -0.034`
`1945`	`1952`	`rotation -0.000000 -0.000000 -1.000000 1.570800`
`1946`	`1953`	`children [`
`1947`	`1954`	`Shape{`
`@@ -1951,7 +1958,7 @@ PROTO Wolfgang [`
`1951`	`1958`	`}`
`1952`	`1959`	`}`
`1953`	`1960`	`geometry Box {`
`1954`		`- size 0.12000 0.106000 0.0100`
	`1961`	`+ size %{=REAR_FOOT_LENGTH}% 0.106000 0.0100`
`1955`	`1962`	`}`
`1956`	`1963`	`}`
`1957`	`1964`	`]`
`@@ -2133,16 +2140,18 @@ PROTO Wolfgang [`
`2133`	`2140`	`# right toe joint`
`2134`	`2141`	`DEF FlexiJointL HingeJoint {`
`2135`	`2142`	`jointParameters HingeJointParameters {`
`2136`		`- anchor 0.0 0.115 -0.034`
	`2143`	`+ anchor 0.0 %{=-0.005 + REAR_FOOT_LENGTH}% -0.034`
`2137`	`2144`	`axis 1 0 0`
`2138`	`2145`	`springConstant IS toe-spring-constant`
`2139`	`2146`	`minStop 0`
`2140`	`2147`	`maxStop 0.5`
`2141`	`2148`	`}`
`2142`	`2149`	`# right toe visual, collision & physics`
	`2150`	`+ # endPoint is relative to foot base`
`2143`	`2151`	`endPoint DEF l_flexi Solid {`
`2144`		`- translation 0.0145 0.145 -0.034`
	`2152`	`+ translation 0.0145 %{=-0.005 + REAR_FOOT_LENGTH + 0.5*TOE_LENGTH}% -0.034 # y = forward`
`2145`	`2153`	`rotation 0.0 0.0 -1.0 1.5708`
	`2154`	`+ # for children x = forward`
`2146`	`2155`	`children [`
`2147`	`2156`	`Shape {`
`2148`	`2157`	`appearance Appearance {`
`@@ -2151,12 +2160,12 @@ PROTO Wolfgang [`
`2151`	`2160`	`}`
`2152`	`2161`	`}`
`2153`	`2162`	`geometry Box {`
`2154`		`- size 0.06 0.106 0.01`
	`2163`	`+ size %{=TOE_LENGTH}% 0.106 0.01`
`2155`	`2164`	`}`
`2156`	`2165`	`}`
`2157`	`2166`	`]`
`2158`	`2167`	`boundingObject Box {`
`2159`		`- size 0.06 0.106 0.01`
	`2168`	`+ size %{=TOE_LENGTH}% 0.106 0.01`
`2160`	`2169`	`}`
`2161`	`2170`	`physics Physics {`
`2162`	`2171`	`density -1`
`@@ -2171,11 +2180,11 @@ PROTO Wolfgang [`
`2171`	`2180`	`boundingObject Group {`
`2172`	`2181`	`children [`
`2173`	`2182`	`Transform {`
`2174`		`- translation 0.0145000 0.08500 -0.034`
	`2183`	`+ translation 0.0145000 %{=-0.005 + REAR_FOOT_LENGTH/2}% -0.034`
`2175`	`2184`	`rotation -0.000000 -0.000000 -1.000000 1.570800`
`2176`	`2185`	`children [`
`2177`	`2186`	`Box {`
`2178`		`- size 0.18000 0.106000 0.0100`
	`2187`	`+ size %{=REAR_FOOT_LENGTH}% 0.106000 0.0100`
`2179`	`2188`	`}`
`2180`	`2189`	`]`
`2181`	`2190`	`}`
`@@ -3351,7 +3360,7 @@ PROTO Wolfgang [`
`3351`	`3360`	`}`
`3352`	`3361`	`# left foot sole visual`
`3353`	`3362`	`Transform {`
`3354`		`- translation -0.0145000 0.05500 -0.034`
	`3363`	`+ translation -0.0145000 %{=-0.005 + 0.5*REAR_FOOT_LENGTH}% -0.034`
`3355`	`3364`	`rotation 0.000000 0.000000 -1.000000 1.570800`
`3356`	`3365`	`children [`
`3357`	`3366`	`Shape{`
`@@ -3361,7 +3370,7 @@ PROTO Wolfgang [`
`3361`	`3370`	`}`
`3362`	`3371`	`}`
`3363`	`3372`	`geometry Box {`
`3364`		`- size 0.12000 0.106000 0.0100`
	`3373`	`+ size %{=REAR_FOOT_LENGTH}% 0.106000 0.0100`
`3365`	`3374`	`}`
`3366`	`3375`	`}`
`3367`	`3376`	`]`
`@@ -3559,15 +3568,15 @@ PROTO Wolfgang [`
`3559`	`3568`	`# left toe joint`
`3560`	`3569`	`DEF FlexiJointL HingeJoint {`
`3561`	`3570`	`jointParameters HingeJointParameters {`
`3562`		`- anchor 0.0 0.115 -0.034`
	`3571`	`+ anchor 0.0 %{=-0.005 + REAR_FOOT_LENGTH}% -0.034`
`3563`	`3572`	`axis 1 0 0`
`3564`	`3573`	`springConstant IS toe-spring-constant`
`3565`	`3574`	`minStop 0`
`3566`	`3575`	`maxStop 0.5`
`3567`	`3576`	`}`
`3568`	`3577`	`# left toe visual, collision & physics`
`3569`	`3578`	`endPoint DEF l_flexi Solid {`
`3570`		`- translation -0.0145 0.145 -0.034`
	`3579`	`+ translation -0.0145 %{=-0.005 + REAR_FOOT_LENGTH + 0.5*TOE_LENGTH}% -0.034 # y = forward`
`3571`	`3580`	`rotation 0.0 0.0 -1.0 1.5708`
`3572`	`3581`	`children [`
`3573`	`3582`	`Shape {`
`@@ -3577,12 +3586,12 @@ PROTO Wolfgang [`
`3577`	`3586`	`}`
`3578`	`3587`	`}`
`3579`	`3588`	`geometry Box {`
`3580`		`- size 0.06 0.106 0.01`
	`3589`	`+ size %{=TOE_LENGTH}% 0.106 0.01`
`3581`	`3590`	`}`
`3582`	`3591`	`}`
`3583`	`3592`	`]`
`3584`	`3593`	`boundingObject Box {`
`3585`		`- size 0.06 0.106 0.01`
	`3594`	`+ size %{=TOE_LENGTH}% 0.106 0.01`
`3586`	`3595`	`}`
`3587`	`3596`	`physics Physics {`
`3588`	`3597`	`density -1`
`@@ -3597,11 +3606,11 @@ PROTO Wolfgang [`
`3597`	`3606`	`boundingObject Group {`
`3598`	`3607`	`children [`
`3599`	`3608`	`Transform {`
`3600`		`- translation -0.0145000 0.05500 -0.034`
	`3609`	`+ translation -0.0145000 %{=-0.005 + REAR_FOOT_LENGTH/2}% -0.034`
`3601`	`3610`	`rotation 0.000000 0.000000 -1.000000 1.570800`
`3602`	`3611`	`children [`
`3603`	`3612`	`Box {`
`3604`		`- size 0.12000 0.106000 0.0100`
	`3613`	`+ size %{=REAR_FOOT_LENGTH}% 0.106000 0.0100`
`3605`	`3614`	`}`
`3606`	`3615`	`]`
`3607`	`3616`	`}`