diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs
new file mode 100644
index 0000000000000000000000000000000000000000..dffa53b188767c58268bf8ef2b6878d7584967d1
--- /dev/null
+++ b/.git-blame-ignore-revs
@@ -0,0 +1,5 @@
+# format (Guilhem Saurel, 2022-04-05)
+aa23fd0fac11a4a5a6611be70d437062f8034b4d
+
+# [format] (Pierre Fernbach, 2020-05-20)
+26f7d9778468ea72a4f449abab947398c18b8716
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7b987f2bfa041659dcd813b2d39b7cb3d86bd5f0
--- /dev/null
+++ b/.pre-commit-config.yaml
@@ -0,0 +1,32 @@
+repos:
+- repo: https://github.com/pre-commit/mirrors-clang-format
+ rev: v13.0.1
+ hooks:
+ - id: clang-format
+ args: [-i, --style=Google]
+- repo: https://github.com/pre-commit/pre-commit-hooks
+ rev: v4.1.0
+ hooks:
+ - id: check-added-large-files
+ - id: check-ast
+ - id: check-executables-have-shebangs
+ - id: check-json
+ - id: check-merge-conflict
+ - id: check-symlinks
+ - id: check-toml
+ - id: check-yaml
+ - id: debug-statements
+ - id: destroyed-symlinks
+ - id: detect-private-key
+ - id: end-of-file-fixer
+ - id: fix-byte-order-marker
+ - id: mixed-line-ending
+ - id: trailing-whitespace
+- repo: https://github.com/psf/black
+ rev: 22.3.0
+ hooks:
+ - id: black
+- repo: https://github.com/PyCQA/flake8
+ rev: 4.0.1
+ hooks:
+ - id: flake8
diff --git a/README.md b/README.md
index fe668cd0e07a00d74f39a53938f67438b9841822..1e6737d51b3e012ba53636b6332f1460f6d960ab 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,9 @@
# anymal-rbprm
[](https://gitlab.laas.fr/humanoid-path-planner/anymal-rbprm/commits/master)
-[](http://projects.laas.fr/gepetto/doc/humanoid-path-planner/anymal-rbprm/master/coverage/)
+[](https://gepettoweb.laas.fr/doc/humanoid-path-planner/anymal-rbprm/master/coverage/)
+[](https://github.com/psf/black)
+[](https://results.pre-commit.ci/latest/github/humanoid-path-planner/anymal-rbprm)
File database for anymal robot using the hpp-rbprm framework
diff --git a/anymal_rbprm/__init__.py b/anymal_rbprm/__init__.py
index 9f9a265599b4299c98b1835663e4c426ab38f1e9..e2ae02652143a786e57f8de0146a291f578ab23d 100644
--- a/anymal_rbprm/__init__.py
+++ b/anymal_rbprm/__init__.py
@@ -2,5 +2,6 @@ from pathlib import Path
def prefix():
- """$prefix/lib/pythonX.Y/site-packages/$module/__init__.py: extract prefix from module"""
+ """$prefix/lib/pythonX.Y/site-packages/$module/__init__.py:
+ extract prefix from module"""
return Path(__file__).parent.parent.parent.parent.parent
diff --git a/anymal_rbprm/anymal.py b/anymal_rbprm/anymal.py
index 8ee84146e5ab1a58a52be2ee2a62e94096c538cd..3f9b88576917c7f7d7d03b3f182fcda4c9082f99 100644
--- a/anymal_rbprm/anymal.py
+++ b/anymal_rbprm/anymal.py
@@ -34,27 +34,27 @@ class Robot(Parent):
srdfSuffix = ""
# Information about the names of thes joints defining the limbs of the robot
- rLegId = 'RFleg'
- rleg = 'RF_HAA'
- rfoot = 'RF_ADAPTER_TO_FOOT'
- lLegId = 'LFleg'
- lleg = 'LF_HAA'
- lfoot = 'LF_ADAPTER_TO_FOOT'
- lArmId = 'LHleg'
- larm = 'LH_HAA'
- lhand = 'LH_ADAPTER_TO_FOOT'
- rArmId = 'RHleg'
- rarm = 'RH_HAA'
- rhand = 'RH_ADAPTER_TO_FOOT'
+ rLegId = "RFleg"
+ rleg = "RF_HAA"
+ rfoot = "RF_ADAPTER_TO_FOOT"
+ lLegId = "LFleg"
+ lleg = "LF_HAA"
+ lfoot = "LF_ADAPTER_TO_FOOT"
+ lArmId = "LHleg"
+ larm = "LH_HAA"
+ lhand = "LH_ADAPTER_TO_FOOT"
+ rArmId = "RHleg"
+ rarm = "RH_HAA"
+ rhand = "RH_ADAPTER_TO_FOOT"
referenceConfig_asymetric = [
- 0.,
- 0.,
+ 0.0,
+ 0.0,
0.461,
- 0.,
- 0.,
- 0.,
- 1., # FF
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0, # FF
0.0,
0.611,
-1.0452,
@@ -85,7 +85,25 @@ class Robot(Parent):
]
"""
referenceConfig = [
- 0, 0, 0.47, 0, 0, 0, 1, -0.12, 0.724, -1.082, -0.12, -0.724, 1.082, 0.12, 0.724, -1.082, 0.12, -0.724, 1.082
+ 0,
+ 0,
+ 0.47,
+ 0,
+ 0,
+ 0,
+ 1,
+ -0.12,
+ 0.724,
+ -1.082,
+ -0.12,
+ -0.724,
+ 1.082,
+ 0.12,
+ 0.724,
+ -1.082,
+ 0.12,
+ -0.724,
+ 1.082,
]
postureWeights = [
0,
@@ -94,18 +112,18 @@ class Robot(Parent):
0,
0,
0, # FF
- 100.,
- 1.,
- 20.,
- 100.,
- 1.,
- 20.,
- 100.,
- 1.,
- 20.,
- 100.,
- 1.,
- 20.,
+ 100.0,
+ 1.0,
+ 20.0,
+ 100.0,
+ 1.0,
+ 20.0,
+ 100.0,
+ 1.0,
+ 20.0,
+ 100.0,
+ 1.0,
+ 20.0,
]
DEFAULT_COM_HEIGHT = 0.445
@@ -114,7 +132,7 @@ class Robot(Parent):
nbSamples = 50000
octreeSize = 0.002
cType = "_3_DOF"
- offset = [0., 0., -0.005] # was 0.005
+ offset = [0.0, 0.0, -0.005] # was 0.005
rLegLimbOffset = [0.373, -0.264, -0.448]
lLegLimbOffset = [0.373, 0.264, -0.448]
@@ -124,10 +142,14 @@ class Robot(Parent):
legx = 0.02
legy = 0.02
import anymal_rbprm
+
kinematic_constraints_path = str(
- anymal_rbprm.prefix() / "share/anymal-rbprm/com_inequalities/feet_quasi_flat/anymal_")
+ anymal_rbprm.prefix()
+ / "share/anymal-rbprm/com_inequalities/feet_quasi_flat/anymal_"
+ )
relative_feet_constraints_path = str(
- anymal_rbprm.prefix() / "share/anymal-rbprm/relative_effector_positions/anymal_")
+ anymal_rbprm.prefix() / "share/anymal-rbprm/relative_effector_positions/anymal_"
+ )
minDist = 0.2
@@ -135,164 +157,203 @@ class Robot(Parent):
rLegId: rLegLimbOffset,
lLegId: lLegLimbOffset,
rArmId: rArmLimbOffset,
- lArmId: lArmLimbOffset
+ lArmId: lArmLimbOffset,
}
# data used by scripts :,,,
# limbs_names = [rArmId,lLegId,lArmId,rLegId]
- # reverse default order to try to remove contacts at the beginning of the contact plan
+ # reverse default order to try to remove contacts
+ # at the beginning of the contact plan
# limbs_names = [lLegId,rArmId,rLegId,lArmId]
# default order to try to remove contacts at the beginning of the contact plan
limbs_names = [rArmId, rLegId, lArmId, lLegId]
dict_limb_rootJoint = {rLegId: rleg, lLegId: lleg, rArmId: rarm, lArmId: larm}
dict_limb_joint = {rLegId: rfoot, lLegId: lfoot, rArmId: rhand, lArmId: lhand}
- dict_limb_color_traj = {rfoot: [0, 1, 0, 1], lfoot: [1, 0, 0, 1], rhand: [0, 0, 1, 1], lhand: [0.9, 0.5, 0, 1]}
+ dict_limb_color_traj = {
+ rfoot: [0, 1, 0, 1],
+ lfoot: [1, 0, 0, 1],
+ rhand: [0, 0, 1, 1],
+ lhand: [0.9, 0.5, 0, 1],
+ }
FOOT_SAFETY_SIZE = 0.01
# size of the contact surface (x,y)
- dict_size = {rfoot: [0.01, 0.01], lfoot: [0.01, 0.01], rhand: [0.01, 0.01], lhand: [0.01, 0.01]}
- # dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
+ dict_size = {
+ rfoot: [0.01, 0.01],
+ lfoot: [0.01, 0.01],
+ rhand: [0.01, 0.01],
+ lhand: [0.01, 0.01],
+ }
+ # dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],
+ # rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
# various offset used by scripts
MRsole_offset = SE3.Identity()
MRsole_offset.translation = np.matrix(offset).T
MLsole_offset = MRsole_offset.copy()
MRhand_offset = MRsole_offset.copy()
MLhand_offset = MRsole_offset.copy()
- dict_offset = {rfoot: MRsole_offset, lfoot: MLsole_offset, rhand: MRhand_offset, lhand: MLhand_offset}
- dict_limb_offset = {rLegId: rLegLimbOffset, lLegId: lLegLimbOffset, rArmId: rArmLimbOffset, lArmId: lArmLimbOffset}
+ dict_offset = {
+ rfoot: MRsole_offset,
+ lfoot: MLsole_offset,
+ rhand: MRhand_offset,
+ lhand: MLhand_offset,
+ }
+ dict_limb_offset = {
+ rLegId: rLegLimbOffset,
+ lLegId: lLegLimbOffset,
+ rArmId: rArmLimbOffset,
+ lArmId: lArmLimbOffset,
+ }
dict_normal = {rfoot: normal, lfoot: normal, rhand: normal, lhand: normal}
# display transform :
MRsole_display = SE3.Identity()
MLsole_display = SE3.Identity()
MRhand_display = SE3.Identity()
MLhand_display = SE3.Identity()
- dict_display_offset = {rfoot: MRsole_display, lfoot: MLsole_display, rhand: MRhand_display, lhand: MLhand_display}
+ dict_display_offset = {
+ rfoot: MRsole_display,
+ lfoot: MLsole_display,
+ rhand: MRhand_display,
+ lhand: MLhand_display,
+ }
kneeIds = {"LF": 9, "LH": 12, "RF": 15, "RH": 18}
def __init__(self, name=None, load=True, client=None, clientRbprm=None):
if name is not None:
self.name = name
- Parent.__init__(self, self.name, self.rootJointType, load, client, None, clientRbprm)
+ Parent.__init__(
+ self, self.name, self.rootJointType, load, client, None, clientRbprm
+ )
# save original bounds of the urdf for futur reset
- self.LF_HAA_bounds = self.getJointBounds('LF_HAA')
- self.LF_HFE_bounds = self.getJointBounds('LF_HFE')
- self.LF_KFE_bounds = self.getJointBounds('LF_KFE')
-
- self.RF_HAA_bounds = self.getJointBounds('RF_HAA')
- self.RF_HFE_bounds = self.getJointBounds('RF_HFE')
- self.RF_KFE_bounds = self.getJointBounds('RF_KFE')
-
- self.LH_HAA_bounds = self.getJointBounds('LH_HAA')
- self.LH_HFE_bounds = self.getJointBounds('LH_HFE')
- self.LH_KFE_bounds = self.getJointBounds('LH_KFE')
-
- self.RH_HAA_bounds = self.getJointBounds('RH_HAA')
- self.RH_HFE_bounds = self.getJointBounds('RH_HFE')
- self.RH_KFE_bounds = self.getJointBounds('RH_KFE')
-
- def loadAllLimbs(self,
- heuristic,
- analysis=None,
- nbSamples=nbSamples,
- octreeSize=octreeSize,
- disableEffectorCollision=False):
- if isinstance(heuristic, str): # only one heuristic name given assign it to all the limbs
+ self.LF_HAA_bounds = self.getJointBounds("LF_HAA")
+ self.LF_HFE_bounds = self.getJointBounds("LF_HFE")
+ self.LF_KFE_bounds = self.getJointBounds("LF_KFE")
+
+ self.RF_HAA_bounds = self.getJointBounds("RF_HAA")
+ self.RF_HFE_bounds = self.getJointBounds("RF_HFE")
+ self.RF_KFE_bounds = self.getJointBounds("RF_KFE")
+
+ self.LH_HAA_bounds = self.getJointBounds("LH_HAA")
+ self.LH_HFE_bounds = self.getJointBounds("LH_HFE")
+ self.LH_KFE_bounds = self.getJointBounds("LH_KFE")
+
+ self.RH_HAA_bounds = self.getJointBounds("RH_HAA")
+ self.RH_HFE_bounds = self.getJointBounds("RH_HFE")
+ self.RH_KFE_bounds = self.getJointBounds("RH_KFE")
+
+ def loadAllLimbs(
+ self,
+ heuristic,
+ analysis=None,
+ nbSamples=nbSamples,
+ octreeSize=octreeSize,
+ disableEffectorCollision=False,
+ ):
+ if isinstance(
+ heuristic, str
+ ): # only one heuristic name given assign it to all the limbs
dict_heuristic = {}
for id in self.limbs_names:
dict_heuristic.update({id: heuristic})
elif isinstance(heuristic, dict):
dict_heuristic = heuristic
else:
- raise Exception("heuristic should be either a string or a map limbId:string")
- # dict_heuristic = {self.rLegId:"static", self.lLegId:"static", self.rArmId:"fixedStep04",
+ raise Exception(
+ "heuristic should be either a string or a map limbId:string"
+ )
+ # dict_heuristic = {self.rLegId:"static", self.lLegId:"static",
+ # self.rArmId:"fixedStep04",
# self.lArmId:"fixedStep04"}
for id in self.limbs_names:
print("add limb : ", id)
eff = self.dict_limb_joint[id]
print("effector name = ", eff)
- self.addLimb(id,
- self.dict_limb_rootJoint[id],
- eff,
- self.dict_offset[eff].translation.tolist(),
- self.dict_normal[eff],
- self.dict_size[eff][0] / 2.,
- self.dict_size[eff][1] / 2.,
- nbSamples,
- dict_heuristic[id],
- octreeSize,
- self.cType,
- disableEffectorCollision=disableEffectorCollision,
- kinematicConstraintsPath=self.kinematicConstraintsPath + self.dict_limb_rootJoint[id] +
- "_06_com_constraints.obj",
- limbOffset=self.dict_limb_offset[id],
- kinematicConstraintsMin=self.minDist)
+ self.addLimb(
+ id,
+ self.dict_limb_rootJoint[id],
+ eff,
+ self.dict_offset[eff].translation.tolist(),
+ self.dict_normal[eff],
+ self.dict_size[eff][0] / 2.0,
+ self.dict_size[eff][1] / 2.0,
+ nbSamples,
+ dict_heuristic[id],
+ octreeSize,
+ self.cType,
+ disableEffectorCollision=disableEffectorCollision,
+ kinematicConstraintsPath=self.kinematicConstraintsPath
+ + self.dict_limb_rootJoint[id]
+ + "_06_com_constraints.obj",
+ limbOffset=self.dict_limb_offset[id],
+ kinematicConstraintsMin=self.minDist,
+ )
if analysis:
self.runLimbSampleAnalysis(id, analysis, True)
def setSlightlyConstrainedJointsBounds(self):
- self.setJointBounds('LF_HAA', [-1., 1.])
- self.setJointBounds('LF_HFE', [-0.25, 2.35])
- self.setJointBounds('LF_KFE', [-2.35, 0.])
+ self.setJointBounds("LF_HAA", [-1.0, 1.0])
+ self.setJointBounds("LF_HFE", [-0.25, 2.35])
+ self.setJointBounds("LF_KFE", [-2.35, 0.0])
- self.setJointBounds('RF_HAA', [-1., 1.])
- self.setJointBounds('RF_HFE', [-0.4, 2.35])
- self.setJointBounds('RF_KFE', [-2.35, 0.])
+ self.setJointBounds("RF_HAA", [-1.0, 1.0])
+ self.setJointBounds("RF_HFE", [-0.4, 2.35])
+ self.setJointBounds("RF_KFE", [-2.35, 0.0])
- self.setJointBounds('LH_HAA', [-1., 1.])
- self.setJointBounds('LH_HFE', [-2.35, 0.4])
- self.setJointBounds('LH_KFE', [0., 2.35])
+ self.setJointBounds("LH_HAA", [-1.0, 1.0])
+ self.setJointBounds("LH_HFE", [-2.35, 0.4])
+ self.setJointBounds("LH_KFE", [0.0, 2.35])
- self.setJointBounds('RH_HAA', [-1., 1.])
- self.setJointBounds('RH_HFE', [-2.35, 0.25])
- self.setJointBounds('RH_KFE', [0., 2.35])
+ self.setJointBounds("RH_HAA", [-1.0, 1.0])
+ self.setJointBounds("RH_HFE", [-2.35, 0.25])
+ self.setJointBounds("RH_KFE", [0.0, 2.35])
def setConstrainedJointsBounds(self):
- self.setJointBounds('LF_HAA', [-0.6, 0.6])
- self.setJointBounds('LF_HFE', [0.25, 1.])
- self.setJointBounds('LF_KFE', [-2.35, 0.])
+ self.setJointBounds("LF_HAA", [-0.6, 0.6])
+ self.setJointBounds("LF_HFE", [0.25, 1.0])
+ self.setJointBounds("LF_KFE", [-2.35, 0.0])
- self.setJointBounds('RF_HAA', [-0.6, 0.6])
- self.setJointBounds('RF_HFE', [0.25, 1.])
- self.setJointBounds('RF_KFE', [-2.35, 0.])
+ self.setJointBounds("RF_HAA", [-0.6, 0.6])
+ self.setJointBounds("RF_HFE", [0.25, 1.0])
+ self.setJointBounds("RF_KFE", [-2.35, 0.0])
- self.setJointBounds('LH_HAA', [-0.6, 0.6])
- self.setJointBounds('LH_HFE', [-1.05, -0.45])
- self.setJointBounds('LH_KFE', [0., 2.35])
+ self.setJointBounds("LH_HAA", [-0.6, 0.6])
+ self.setJointBounds("LH_HFE", [-1.05, -0.45])
+ self.setJointBounds("LH_KFE", [0.0, 2.35])
- self.setJointBounds('RH_HAA', [-0.6, 0.6])
- self.setJointBounds('RH_HFE', [-1.05, -0.45])
- self.setJointBounds('RH_KFE', [0., 2.35])
+ self.setJointBounds("RH_HAA", [-0.6, 0.6])
+ self.setJointBounds("RH_HFE", [-1.05, -0.45])
+ self.setJointBounds("RH_KFE", [0.0, 2.35])
def setVeryConstrainedJointsBounds(self):
- self.setJointBounds('LF_HAA', [-0.35, 0.05])
- self.setJointBounds('LF_HFE', [0.3, 0.95])
- self.setJointBounds('LF_KFE', [-2.35, 0.])
+ self.setJointBounds("LF_HAA", [-0.35, 0.05])
+ self.setJointBounds("LF_HFE", [0.3, 0.95])
+ self.setJointBounds("LF_KFE", [-2.35, 0.0])
- self.setJointBounds('RF_HAA', [-0.05, 0.35])
- self.setJointBounds('RF_HFE', [0.3, 0.95])
- self.setJointBounds('RF_KFE', [-2.35, 0.])
+ self.setJointBounds("RF_HAA", [-0.05, 0.35])
+ self.setJointBounds("RF_HFE", [0.3, 0.95])
+ self.setJointBounds("RF_KFE", [-2.35, 0.0])
- self.setJointBounds('LH_HAA', [-0.35, 0.05])
- self.setJointBounds('LH_HFE', [-1., -0.5])
- self.setJointBounds('LH_KFE', [0., 2.35])
+ self.setJointBounds("LH_HAA", [-0.35, 0.05])
+ self.setJointBounds("LH_HFE", [-1.0, -0.5])
+ self.setJointBounds("LH_KFE", [0.0, 2.35])
- self.setJointBounds('RH_HAA', [-0.05, 0.35])
- self.setJointBounds('RH_HFE', [-1., -0.5])
- self.setJointBounds('RH_KFE', [0., 2.35])
+ self.setJointBounds("RH_HAA", [-0.05, 0.35])
+ self.setJointBounds("RH_HFE", [-1.0, -0.5])
+ self.setJointBounds("RH_KFE", [0.0, 2.35])
def resetJointsBounds(self):
- self.setJointBounds('LF_HAA', self.LF_HAA_bounds)
- self.setJointBounds('LF_HFE', self.LF_HFE_bounds)
- self.setJointBounds('LF_KFE', self.LF_KFE_bounds)
+ self.setJointBounds("LF_HAA", self.LF_HAA_bounds)
+ self.setJointBounds("LF_HFE", self.LF_HFE_bounds)
+ self.setJointBounds("LF_KFE", self.LF_KFE_bounds)
- self.setJointBounds('RF_HAA', self.RF_HAA_bounds)
- self.setJointBounds('RF_HFE', self.RF_HFE_bounds)
- self.setJointBounds('RF_KFE', self.RF_KFE_bounds)
+ self.setJointBounds("RF_HAA", self.RF_HAA_bounds)
+ self.setJointBounds("RF_HFE", self.RF_HFE_bounds)
+ self.setJointBounds("RF_KFE", self.RF_KFE_bounds)
- self.setJointBounds('LH_HAA', self.LH_HAA_bounds)
- self.setJointBounds('LH_HFE', self.LH_HFE_bounds)
- self.setJointBounds('LH_KFE', self.LH_KFE_bounds)
+ self.setJointBounds("LH_HAA", self.LH_HAA_bounds)
+ self.setJointBounds("LH_HFE", self.LH_HFE_bounds)
+ self.setJointBounds("LH_KFE", self.LH_KFE_bounds)
- self.setJointBounds('RH_HAA', self.RH_HAA_bounds)
- self.setJointBounds('RH_HFE', self.RH_HFE_bounds)
- self.setJointBounds('RH_KFE', self.RH_KFE_bounds)
+ self.setJointBounds("RH_HAA", self.RH_HAA_bounds)
+ self.setJointBounds("RH_HFE", self.RH_HFE_bounds)
+ self.setJointBounds("RH_KFE", self.RH_KFE_bounds)
diff --git a/anymal_rbprm/anymal_abstract.py b/anymal_rbprm/anymal_abstract.py
index b810ce9cbccd37a54e3b2f42e50512340351b5d7..9f518d87830dd5ee6585c02295f16dbb4ed1ae33 100644
--- a/anymal_rbprm/anymal_abstract.py
+++ b/anymal_rbprm/anymal_abstract.py
@@ -21,13 +21,18 @@ from hpp.corbaserver.rbprm.rbprmbuilder import Builder as Parent
class Robot(Parent):
# Information to retrieve urdf and srdf files.
- rootJointType = 'freeflyer'
- packageName = 'anymal-rbprm'
- meshPackageName = 'anymal-rbprm'
+ rootJointType = "freeflyer"
+ packageName = "anymal-rbprm"
+ meshPackageName = "anymal-rbprm"
# URDF file describing the trunk of the robot HyQ
- urdfName = 'anymal_trunk'
+ urdfName = "anymal_trunk"
# URDF files describing the reachable workspace of each limb of HyQ
- urdfNameRom = ['anymal_RFleg_rom', 'anymal_LHleg_rom', 'anymal_LFleg_rom', 'anymal_RHleg_rom']
+ urdfNameRom = [
+ "anymal_RFleg_rom",
+ "anymal_LHleg_rom",
+ "anymal_LFleg_rom",
+ "anymal_RHleg_rom",
+ ]
urdfSuffix = ""
srdfSuffix = ""
name = urdfName
@@ -36,10 +41,10 @@ class Robot(Parent):
# TODO
- rLegId = 'anymal_RFleg_rom'
- lLegId = 'anymal_LFleg_rom'
- rArmId = 'anymal_RHleg_rom'
- lArmId = 'anymal_LHleg_rom'
+ rLegId = "anymal_RFleg_rom"
+ lLegId = "anymal_LFleg_rom"
+ rArmId = "anymal_RHleg_rom"
+ lArmId = "anymal_LHleg_rom"
ref_EE_lLeg = [0.373, 0.264, -0.448]
ref_EE_rLeg = [0.373, -0.264, -0.448]
@@ -50,13 +55,20 @@ class Robot(Parent):
# ref_EE_lArm = [-0.3, 0.165 , -0.44]
# ref_EE_rArm = [-0.3, -0.165 , -0.44]
- dict_ref_effector_from_root = {rLegId: ref_EE_rLeg, lLegId: ref_EE_lLeg, rArmId: ref_EE_rArm, lArmId: ref_EE_lArm}
+ dict_ref_effector_from_root = {
+ rLegId: ref_EE_rLeg,
+ lLegId: ref_EE_lLeg,
+ rArmId: ref_EE_rArm,
+ lArmId: ref_EE_lArm,
+ }
def __init__(self, name=None, load=True, client=None, clientRbprm=None):
if name is not None:
self.name = name
- Parent.__init__(self, self.name, self.rootJointType, load, client, None, clientRbprm)
- self.setReferenceEndEffector('anymal_LFleg_rom', self.ref_EE_lLeg)
- self.setReferenceEndEffector('anymal_RFleg_rom', self.ref_EE_rLeg)
- self.setReferenceEndEffector('anymal_LHleg_rom', self.ref_EE_lArm)
- self.setReferenceEndEffector('anymal_RHleg_rom', self.ref_EE_rArm)
+ Parent.__init__(
+ self, self.name, self.rootJointType, load, client, None, clientRbprm
+ )
+ self.setReferenceEndEffector("anymal_LFleg_rom", self.ref_EE_lLeg)
+ self.setReferenceEndEffector("anymal_RFleg_rom", self.ref_EE_rLeg)
+ self.setReferenceEndEffector("anymal_LHleg_rom", self.ref_EE_lArm)
+ self.setReferenceEndEffector("anymal_RHleg_rom", self.ref_EE_rArm)
diff --git a/anymal_rbprm/anymal_contact6D.py b/anymal_rbprm/anymal_contact6D.py
index 9c8112ec5ba162b4f8dd2a024798fb9f7338f880..068f0610716c80a1a110598c7fce05be7d0ddd8a 100644
--- a/anymal_rbprm/anymal_contact6D.py
+++ b/anymal_rbprm/anymal_contact6D.py
@@ -32,65 +32,65 @@ class Robot(Parent):
srdfSuffix = ""
# Information about the names of thes joints defining the limbs of the robot
- rLegId = 'RFleg'
- rleg = 'RF_HAA'
- rfoot = 'RF_CONTACT_3'
- lLegId = 'LFleg'
- lleg = 'LF_HAA'
- lfoot = 'LF_CONTACT_3'
- lArmId = 'LHleg'
- larm = 'LH_HAA'
- lhand = 'LH_CONTACT_3'
- rArmId = 'RHleg'
- rarm = 'RH_HAA'
- rhand = 'RH_CONTACT_3'
+ rLegId = "RFleg"
+ rleg = "RF_HAA"
+ rfoot = "RF_CONTACT_3"
+ lLegId = "LFleg"
+ lleg = "LF_HAA"
+ lfoot = "LF_CONTACT_3"
+ lArmId = "LHleg"
+ larm = "LH_HAA"
+ lhand = "LH_CONTACT_3"
+ rArmId = "RHleg"
+ rarm = "RH_HAA"
+ rhand = "RH_CONTACT_3"
referenceConfig = [
- 0.,
- 0.,
+ 0.0,
+ 0.0,
0.444,
- 0.,
- 0.,
- 0.,
- 1., # FF
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0, # FF
0.04,
0.74,
-1.08,
0.34,
-0.04,
- 0.,
+ 0.0,
0.04,
-0.74,
1.08,
-0.34,
-0.04,
- 0.,
+ 0.0,
-0.04,
0.74,
-1.08,
0.34,
0.04,
- 0.,
+ 0.0,
-0.04,
-0.74,
1.08,
-0.34,
0.04,
- 0.
+ 0.0,
]
- reference_weights = [100., 1., 1., 0., 0., 0.]
+ reference_weights = [100.0, 1.0, 1.0, 0.0, 0.0, 0.0]
# informations required to generate the limbs databases the limbs :
nbSamples = 50000
octreeSize = 0.01
cType = "_6_DOF"
- offset = [0., 0., 0.]
+ offset = [0.0, 0.0, 0.0]
- rLegLimbOffset = [0.373, 0.264, 0.]
- lLegLimbOffset = [0.373, -0.264, 0.]
- rArmLimbOffset = [-0.373, 0.264, 0.]
- lArmLimbOffset = [-0.373, -0.264, 0.]
+ rLegLimbOffset = [0.373, 0.264, 0.0]
+ lLegLimbOffset = [0.373, -0.264, 0.0]
+ rArmLimbOffset = [-0.373, 0.264, 0.0]
+ lArmLimbOffset = [-0.373, -0.264, 0.0]
normal = [0, 0, 1]
legx = 0.02
legy = 0.02
@@ -100,70 +100,113 @@ class Robot(Parent):
# data used by scripts :,,,
# limbs_names = [rArmId,lLegId,lArmId,rLegId]
- # reverse default order to try to remove contacts at the beginning of the contact plan
+ # reverse default order to try to remove contacts
+ # at the beginning of the contact plan
# limbs_names = [lLegId,rArmId,rLegId,lArmId]
# default order to try to remove contacts at the beginning of the contact plan
limbs_names = [rArmId, lArmId, lLegId, rLegId]
dict_limb_rootJoint = {rLegId: rleg, lLegId: lleg, rArmId: rarm, lArmId: larm}
dict_limb_joint = {rLegId: rfoot, lLegId: lfoot, rArmId: rhand, lArmId: lhand}
- dict_limb_color_traj = {rfoot: [0, 1, 0, 1], lfoot: [1, 0, 0, 1], rhand: [0, 0, 1, 1], lhand: [0.9, 0.5, 0, 1]}
+ dict_limb_color_traj = {
+ rfoot: [0, 1, 0, 1],
+ lfoot: [1, 0, 0, 1],
+ rhand: [0, 0, 1, 1],
+ lhand: [0.9, 0.5, 0, 1],
+ }
FOOT_SAFETY_SIZE = 0.01
# size of the contact surface (x,y)
- dict_size = {rfoot: [0.031, 0.031], lfoot: [0.031, 0.031], rhand: [0.031, 0.031], lhand: [0.031, 0.031]}
- # dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
+ dict_size = {
+ rfoot: [0.031, 0.031],
+ lfoot: [0.031, 0.031],
+ rhand: [0.031, 0.031],
+ lhand: [0.031, 0.031],
+ }
+ # dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],
+ # rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
# various offset used by scripts
MRsole_offset = SE3.Identity()
MRsole_offset.translation = np.matrix(offset).T
MLsole_offset = MRsole_offset.copy()
MRhand_offset = MRsole_offset.copy()
MLhand_offset = MRsole_offset.copy()
- dict_offset = {rfoot: MRsole_offset, lfoot: MLsole_offset, rhand: MRhand_offset, lhand: MLhand_offset}
- dict_limb_offset = {rLegId: rLegLimbOffset, lLegId: lLegLimbOffset, rArmId: rArmLimbOffset, lArmId: lArmLimbOffset}
+ dict_offset = {
+ rfoot: MRsole_offset,
+ lfoot: MLsole_offset,
+ rhand: MRhand_offset,
+ lhand: MLhand_offset,
+ }
+ dict_limb_offset = {
+ rLegId: rLegLimbOffset,
+ lLegId: lLegLimbOffset,
+ rArmId: rArmLimbOffset,
+ lArmId: lArmLimbOffset,
+ }
dict_normal = {rfoot: normal, lfoot: normal, rhand: normal, lhand: normal}
# display transform :
MRsole_display = SE3.Identity()
MLsole_display = SE3.Identity()
MRhand_display = SE3.Identity()
MLhand_display = SE3.Identity()
- dict_display_offset = {rfoot: MRsole_display, lfoot: MLsole_display, rhand: MRhand_display, lhand: MLhand_display}
+ dict_display_offset = {
+ rfoot: MRsole_display,
+ lfoot: MLsole_display,
+ rhand: MRhand_display,
+ lhand: MLhand_display,
+ }
def __init__(self, name=None, load=True):
Parent.__init__(self, load)
if load:
- self.loadFullBodyModel(self.urdfName, self.rootJointType, self.meshPackageName, self.packageName,
- self.urdfSuffix, self.srdfSuffix)
+ self.loadFullBodyModel(
+ self.urdfName,
+ self.rootJointType,
+ self.meshPackageName,
+ self.packageName,
+ self.urdfSuffix,
+ self.srdfSuffix,
+ )
if name is not None:
self.name = name
- def loadAllLimbs(self, heuristic, analysis=None, nbSamples=nbSamples, octreeSize=octreeSize):
- if isinstance(heuristic, str): # only one heuristic name given assign it to all the limbs
+ def loadAllLimbs(
+ self, heuristic, analysis=None, nbSamples=nbSamples, octreeSize=octreeSize
+ ):
+ if isinstance(
+ heuristic, str
+ ): # only one heuristic name given assign it to all the limbs
dict_heuristic = {}
for id in self.limbs_names:
dict_heuristic.update({id: heuristic})
elif isinstance(heuristic, dict):
dict_heuristic = heuristic
else:
- raise Exception("heuristic should be either a string or a map limbId:string")
- # dict_heuristic = {self.rLegId:"static", self.lLegId:"static", self.rArmId:"fixedStep04",
+ raise Exception(
+ "heuristic should be either a string or a map limbId:string"
+ )
+ # dict_heuristic = {self.rLegId:"static", self.lLegId:"static",
+ # self.rArmId:"fixedStep04",
# self.lArmId:"fixedStep04"}
for id in self.limbs_names:
print("add limb : ", id)
eff = self.dict_limb_joint[id]
print("effector name = ", eff)
- self.addLimb(id,
- self.dict_limb_rootJoint[id],
- eff,
- self.dict_offset[eff].translation.tolist(),
- self.dict_normal[eff],
- self.dict_size[eff][0] / 2.,
- self.dict_size[eff][1] / 2.,
- nbSamples,
- dict_heuristic[id],
- octreeSize,
- self.cType,
- kinematicConstraintsPath=self.kinematicConstraintsPath + self.dict_limb_rootJoint[id] +
- "06_com_constraints.obj",
- limbOffset=self.dict_limb_offset[id],
- kinematicConstraintsMin=self.minDist)
+ self.addLimb(
+ id,
+ self.dict_limb_rootJoint[id],
+ eff,
+ self.dict_offset[eff].translation.tolist(),
+ self.dict_normal[eff],
+ self.dict_size[eff][0] / 2.0,
+ self.dict_size[eff][1] / 2.0,
+ nbSamples,
+ dict_heuristic[id],
+ octreeSize,
+ self.cType,
+ kinematicConstraintsPath=self.kinematicConstraintsPath
+ + self.dict_limb_rootJoint[id]
+ + "06_com_constraints.obj",
+ limbOffset=self.dict_limb_offset[id],
+ kinematicConstraintsMin=self.minDist,
+ )
if analysis:
self.runLimbSampleAnalysis(id, analysis, True)
diff --git a/cmake b/cmake
index ee7a773c5c23f83dd21eb0ccfa96277e068b0456..3d6176d439963702d97b82555e3007de05a4e6a4 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit ee7a773c5c23f83dd21eb0ccfa96277e068b0456
+Subproject commit 3d6176d439963702d97b82555e3007de05a4e6a4
diff --git a/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_LFleg_effector_frame_quasi_static.obj b/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_LFleg_effector_frame_quasi_static.obj
index a13eeeb3180343ecd9b74ff07ec7e8026fc6daa2..f93e497966c6b563755c39268313c49ea3607388 100644
--- a/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_LFleg_effector_frame_quasi_static.obj
+++ b/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_LFleg_effector_frame_quasi_static.obj
@@ -1,451 +1,451 @@
-v 0.07126008357447547 -0.20609652639650433 0.4068135635848112
-v -0.5169060644182858 -0.4719651719310997 0.3004475374871134
-v -0.4639459273896392 -0.16667887615462815 0.5105415005846371
-v -0.5828102642190716 -0.05877114205464703 0.31706312052784963
-v -0.39683940113196087 -0.2848911735932782 0.531392209400165
-v -0.34875282145308933 -0.1462958170558764 0.5637605067185196
-v -0.4849068551209793 -0.12243296044143871 0.4953834904548933
-v -0.3090575030100131 0.008817354417762251 0.5597752218910736
-v 0.0023668291809624317 -0.22900357184701137 0.45975142730402146
-v 0.036658635511044924 -0.014334523435427918 0.4153964560791552
-v -0.3732832807054133 0.07582428674922621 0.517022225893347
-v -0.4338327099659134 -0.11979606874688327 0.5324560566259704
-v -0.5646306091238992 -0.35120343354693895 0.34319237304690214
-v -0.3230068650151612 -0.4081697516800907 0.3012726699903249
-v -0.37952621935791525 -0.49486766260163867 0.31038386887093716
-v -0.32830529483092646 -0.3798796078707256 0.5141945772640972
-v -0.12101292545677059 -0.1317628082995782 0.5337765081852127
-v -0.5899157880603623 -0.24935214747861711 0.3026707765905449
-v -0.31882065915077323 0.08585425622585299 0.5345962461371008
-v -0.45432995029388445 -0.11721451077930715 0.5184715769949009
-v -0.2508064219653848 -0.49285407217101146 0.310375941896736
-v -0.1918600487844014 -0.269057214841617 0.5439886893409701
-v 0.018801999892495477 -0.17465159101181796 0.4613894040358236
-v -0.24106777865762777 -0.04340215506535331 0.5713218673160673
-v -0.3711027649674476 -0.42027827079639113 0.47501895222900054
-v -0.4672921508993667 0.04735003044689524 0.47901507498012047
-v 0.022599838846805748 -0.4438570163226297 0.32978455828680414
-v -0.46822161879377433 0.05286507729235576 0.4678984499469448
-v 0.005055780378541976 -0.45525265983831564 0.31266936964214304
-v -0.31352563732147914 -0.5543439324057235 0.39083813520938027
-v -0.0863078115394692 -0.5064294199202495 0.3552300975196999
-v -0.26431018450856403 0.06448775167572485 0.5375822690882391
-v 0.05191616525869401 -0.06643150773028522 0.421723316399481
-v -0.22749354090396165 -0.4234572744279652 0.4905758072648474
-v -0.5239433463659924 -0.4680560777215243 0.3064253793804332
-v -0.512543656701995 -0.34448732797810927 0.42183088656173795
-v -0.5781582179767486 -0.024583158335494064 0.3638116007588562
-v 0.07306075798337451 -0.24343359652528834 0.38061865236247144
-v -0.45247104779618025 -0.4353261781673207 0.42131276088969566
-v -0.2787716259268365 0.043639988889225115 0.5484220174852326
-v -0.04815729975103267 -0.07794927663763578 0.5047740570787348
-v -0.5539872017339906 -0.018301146424282627 0.3494189962733735
-v -0.45143684757807506 -0.42105278980925315 0.4303726209991422
-v 0.002898962460937152 -0.3113886174665866 0.4317400877574484
-v -0.3163676653864284 -0.04554293298890557 0.5679903317147135
-v -0.5867694507509874 -0.04722355385428442 0.3516655975305137
-v -0.4083404956030737 -0.36802945277096194 0.3006046945160396
-v -0.3102773431763005 -0.47264399957147557 0.45513021401664094
-v -0.3116167512989828 0.08915393072347692 0.5253358420967933
-v -0.4343780469844724 -0.03138430372682542 0.5291749830680084
-v -0.3324928771653446 -0.04714727034761457 0.563346623867182
-v -0.009367835661853778 -0.29207115187178906 0.44800081343697645
-v -0.4968800137262488 0.04174111364008366 0.46151514891893686
-v -0.4729528435309781 0.05266683337779513 0.45289448165960744
-v 0.03509649721165961 -0.4260438607278409 0.32945881645397307
-v -0.3378204035330305 0.08513619475864999 0.5138462467840355
-v -0.210323501094933 0.08140905116053626 0.5221957695419653
-v 0.057029467060702985 -0.08906544727149952 0.41889675451907604
-v -0.5344094671787496 -0.39500892414054184 0.30017572554034416
-v -0.598537053209371 -0.15614586125088856 0.3011785609845097
-v 0.04536738198383389 -0.11066077343596968 0.4419280631907304
-v -0.47663576574231176 -0.2287247809565687 0.498139577587069
-v -0.3631123487461492 -0.07607926294912785 0.5586184382317535
-v -0.5515834696777031 -0.41070412812292245 0.31115172304723465
-v -0.05971328664635026 -0.3426378676023408 0.4605634329010616
-v -0.3524117666354008 -0.26395885395406254 0.5584223431773899
-v -0.09352419841499088 -0.03795669913466615 0.522571709933437
-v -0.025346198261077896 -0.4743252700048696 0.3053821465410873
-v -0.23184735141942547 -0.19653944256864245 0.5653217474023681
-v -0.5811038247394771 -0.061773325731205704 0.31555387353740266
-v -0.06238167145713715 -0.4992061450723322 0.3178623588611154
-v -0.16156792613817245 -0.23191746643234012 0.5424444854163469
-v -0.3973148354135192 -0.34971394570827763 0.5056327364783025
-v -0.02560240061901668 -0.48622602516334834 0.317039881496455
-v -0.5449226011205575 -0.36995338282004897 0.3672262833517738
-v -0.16334271171854584 0.08442936117217587 0.49799680037430943
-v -0.18618377784985193 -0.13228613707624054 0.5558540178467287
-v -0.5927965204339779 -0.1711684587864094 0.35351508482671007
-v -0.36773696652269855 -0.10603006004269806 0.5584154476141128
-v 0.051030013824554486 -0.35883834878915216 0.3741911969045968
-v -0.29312696855700404 -0.46868989401461264 0.30116269455343336
-v -0.4441046419868223 0.05937980702425698 0.47800049245037507
-v -0.5801685316183492 -0.2986666372780107 0.32508703918344245
-v -0.5432076237397785 -0.22728234199149627 0.427678272001959
-v -0.610332473345683 -0.10823133619117736 0.31102381683800173
-v 0.06760685328478817 -0.3273572227113141 0.34599894465474634
-v -0.5016676154381987 -0.40448012573438 0.3982501609948002
-v -0.19684555802363773 0.06703468423603398 0.5239649081389145
-v -0.2954149476882511 -0.4796288736642809 0.3020622994405346
-v -0.14087625107790458 -0.05458412356288634 0.5421876309607002
-v -0.4985664246283688 -0.4864625702138711 0.3189085888189619
-v -0.00021736430237647916 -0.4503172760413985 0.30652694553782617
-v -0.5504035000671088 -0.09529047774793714 0.42938244290602406
-v -0.5972481099650129 -0.20887727241235501 0.32321360023844853
-v -0.5372523037979573 -0.09489362398152588 0.4527820975419985
-v -0.5459940178258587 -0.28959838980926594 0.30012287180473335
-v -0.07980088612078434 -0.2555580226988366 0.5004106394621651
-v -0.536059662700914 -0.06297296071375963 0.4453812045473999
-v -0.5142251910586022 -0.26892126385913245 0.45365745765576165
-v -0.3761992712423714 0.044047127862896704 0.531113185973369
-v 0.021396814010689914 -0.023506272675528888 0.44878612271419277
-v -0.4785895892647789 -0.3772991310351158 0.43417649560708643
-v -0.04057371307687037 -0.28472179118277163 0.4725031474014694
-v -0.06379373515924976 0.035641192877822914 0.48501104930628475
-v -0.268552480346635 -0.01147951479006362 0.563930322448866
-v -0.48140519133600373 -0.20596920034000168 0.4963839695996218
-v -0.4676925046277537 -0.40085215202202507 0.4316135062118517
-v -0.09945340657171643 0.023356416620359075 0.5039036037550368
-v -0.2907495866483775 -0.36274823551761187 0.5336687503705929
-v -0.04149962551469581 -0.5000863122817075 0.3241777229620437
-v 0.055784870593456645 -0.20970525084905087 0.41975171852912213
-v -0.3694954938488563 -0.16019083407830673 0.557952867325209
-v -0.32546607979209224 -0.1488984347889514 0.5684618183311246
-v -0.4864234793855924 -0.48239401938540655 0.36379806421152355
-v -0.5546701485744439 -0.1265445746961647 0.42308789131320534
-v -0.5575742514147694 -0.27430544308086047 0.4039746818819943
-v -0.08433836383823479 -0.3455791286002283 0.47199219934492403
-v -0.37183734172977495 -0.5150063281537091 0.41601105318819254
-v -0.4021988410118873 -0.17317239367256465 0.548957562899091
-v -0.0930615403545105 -0.4303561261777834 0.44266573959426403
-v -0.18225400911558487 -0.4551215948132624 0.4603010281335543
-v -0.46560815416105494 -0.3383624702986354 0.46276118920299036
-v -0.19879229181467856 -0.037650812450389315 0.5574652574032906
-v -0.27549673798946506 -0.444700751316397 0.4769310058214301
-v -0.1569658738779502 0.07924042091311725 0.49202131165085655
-v -0.08118867168698839 0.048730218343169006 0.46099135135491487
-v -0.19197427654627053 -0.31432664023095847 0.5281530615682523
-v -0.016236913499099986 -0.4572634404841614 0.35427853418093674
-v -0.5670532773441858 -0.013667255230511191 0.37216677230309103
-v -0.1600702440346251 -0.018218162437105417 0.5395571358378572
-v -0.08508875367359162 -0.1884207475417829 0.5148470357560508
-v -0.48709801336252445 -0.2742352444341565 0.4757797647482181
-v -0.1804870258593016 -0.09486222522204647 0.5543107554492978
-v -0.40293893045744555 -0.13883635986668663 0.5475099735687757
-v -0.15484525774267938 -0.485765021532002 0.4193170102599116
-v -0.5957279576002608 -0.226995108251396 0.30757102000434566
-v -0.13524689896720338 -0.5203807370440556 0.36406142629441485
-v -0.4322738797800809 -0.08828863283119354 0.535344878380172
-v -0.07929114039723988 -0.4912184092734881 0.31194152100979133
-v -0.5393072007103122 -0.24436691161122662 0.4294432944220774
-v -0.33486716432809854 0.06526509221410337 0.4697037968914516
-v -0.598676550628925 -0.1189421132205157 0.34444577152051387
-v -0.05958966622524293 -0.27588260366739276 0.4857656768031185
-v -0.15526796103692886 -0.49779201325519035 0.4081319238472725
-v 0.03268313382800418 -0.39365571182347076 0.3624088350375639
-v 0.05682606888384067 -0.36138116420724786 0.3477239822837412
-v -0.05665627063658426 -0.15191580842532054 0.5055886728103176
-v -0.2653347072890573 -0.12105709715903419 0.5705166043512918
-v -0.013565420907915533 -0.40005736150731536 0.40099520768933283
-v -0.23946974182161074 -0.5147016554010905 0.4167803857859956
-v 0.02891607776427203 -0.32617171497403985 0.40720091623092974
+v 0.07126008357447547 -0.20609652639650433 0.4068135635848112
+v -0.5169060644182858 -0.4719651719310997 0.3004475374871134
+v -0.4639459273896392 -0.16667887615462815 0.5105415005846371
+v -0.5828102642190716 -0.05877114205464703 0.31706312052784963
+v -0.39683940113196087 -0.2848911735932782 0.531392209400165
+v -0.34875282145308933 -0.1462958170558764 0.5637605067185196
+v -0.4849068551209793 -0.12243296044143871 0.4953834904548933
+v -0.3090575030100131 0.008817354417762251 0.5597752218910736
+v 0.0023668291809624317 -0.22900357184701137 0.45975142730402146
+v 0.036658635511044924 -0.014334523435427918 0.4153964560791552
+v -0.3732832807054133 0.07582428674922621 0.517022225893347
+v -0.4338327099659134 -0.11979606874688327 0.5324560566259704
+v -0.5646306091238992 -0.35120343354693895 0.34319237304690214
+v -0.3230068650151612 -0.4081697516800907 0.3012726699903249
+v -0.37952621935791525 -0.49486766260163867 0.31038386887093716
+v -0.32830529483092646 -0.3798796078707256 0.5141945772640972
+v -0.12101292545677059 -0.1317628082995782 0.5337765081852127
+v -0.5899157880603623 -0.24935214747861711 0.3026707765905449
+v -0.31882065915077323 0.08585425622585299 0.5345962461371008
+v -0.45432995029388445 -0.11721451077930715 0.5184715769949009
+v -0.2508064219653848 -0.49285407217101146 0.310375941896736
+v -0.1918600487844014 -0.269057214841617 0.5439886893409701
+v 0.018801999892495477 -0.17465159101181796 0.4613894040358236
+v -0.24106777865762777 -0.04340215506535331 0.5713218673160673
+v -0.3711027649674476 -0.42027827079639113 0.47501895222900054
+v -0.4672921508993667 0.04735003044689524 0.47901507498012047
+v 0.022599838846805748 -0.4438570163226297 0.32978455828680414
+v -0.46822161879377433 0.05286507729235576 0.4678984499469448
+v 0.005055780378541976 -0.45525265983831564 0.31266936964214304
+v -0.31352563732147914 -0.5543439324057235 0.39083813520938027
+v -0.0863078115394692 -0.5064294199202495 0.3552300975196999
+v -0.26431018450856403 0.06448775167572485 0.5375822690882391
+v 0.05191616525869401 -0.06643150773028522 0.421723316399481
+v -0.22749354090396165 -0.4234572744279652 0.4905758072648474
+v -0.5239433463659924 -0.4680560777215243 0.3064253793804332
+v -0.512543656701995 -0.34448732797810927 0.42183088656173795
+v -0.5781582179767486 -0.024583158335494064 0.3638116007588562
+v 0.07306075798337451 -0.24343359652528834 0.38061865236247144
+v -0.45247104779618025 -0.4353261781673207 0.42131276088969566
+v -0.2787716259268365 0.043639988889225115 0.5484220174852326
+v -0.04815729975103267 -0.07794927663763578 0.5047740570787348
+v -0.5539872017339906 -0.018301146424282627 0.3494189962733735
+v -0.45143684757807506 -0.42105278980925315 0.4303726209991422
+v 0.002898962460937152 -0.3113886174665866 0.4317400877574484
+v -0.3163676653864284 -0.04554293298890557 0.5679903317147135
+v -0.5867694507509874 -0.04722355385428442 0.3516655975305137
+v -0.4083404956030737 -0.36802945277096194 0.3006046945160396
+v -0.3102773431763005 -0.47264399957147557 0.45513021401664094
+v -0.3116167512989828 0.08915393072347692 0.5253358420967933
+v -0.4343780469844724 -0.03138430372682542 0.5291749830680084
+v -0.3324928771653446 -0.04714727034761457 0.563346623867182
+v -0.009367835661853778 -0.29207115187178906 0.44800081343697645
+v -0.4968800137262488 0.04174111364008366 0.46151514891893686
+v -0.4729528435309781 0.05266683337779513 0.45289448165960744
+v 0.03509649721165961 -0.4260438607278409 0.32945881645397307
+v -0.3378204035330305 0.08513619475864999 0.5138462467840355
+v -0.210323501094933 0.08140905116053626 0.5221957695419653
+v 0.057029467060702985 -0.08906544727149952 0.41889675451907604
+v -0.5344094671787496 -0.39500892414054184 0.30017572554034416
+v -0.598537053209371 -0.15614586125088856 0.3011785609845097
+v 0.04536738198383389 -0.11066077343596968 0.4419280631907304
+v -0.47663576574231176 -0.2287247809565687 0.498139577587069
+v -0.3631123487461492 -0.07607926294912785 0.5586184382317535
+v -0.5515834696777031 -0.41070412812292245 0.31115172304723465
+v -0.05971328664635026 -0.3426378676023408 0.4605634329010616
+v -0.3524117666354008 -0.26395885395406254 0.5584223431773899
+v -0.09352419841499088 -0.03795669913466615 0.522571709933437
+v -0.025346198261077896 -0.4743252700048696 0.3053821465410873
+v -0.23184735141942547 -0.19653944256864245 0.5653217474023681
+v -0.5811038247394771 -0.061773325731205704 0.31555387353740266
+v -0.06238167145713715 -0.4992061450723322 0.3178623588611154
+v -0.16156792613817245 -0.23191746643234012 0.5424444854163469
+v -0.3973148354135192 -0.34971394570827763 0.5056327364783025
+v -0.02560240061901668 -0.48622602516334834 0.317039881496455
+v -0.5449226011205575 -0.36995338282004897 0.3672262833517738
+v -0.16334271171854584 0.08442936117217587 0.49799680037430943
+v -0.18618377784985193 -0.13228613707624054 0.5558540178467287
+v -0.5927965204339779 -0.1711684587864094 0.35351508482671007
+v -0.36773696652269855 -0.10603006004269806 0.5584154476141128
+v 0.051030013824554486 -0.35883834878915216 0.3741911969045968
+v -0.29312696855700404 -0.46868989401461264 0.30116269455343336
+v -0.4441046419868223 0.05937980702425698 0.47800049245037507
+v -0.5801685316183492 -0.2986666372780107 0.32508703918344245
+v -0.5432076237397785 -0.22728234199149627 0.427678272001959
+v -0.610332473345683 -0.10823133619117736 0.31102381683800173
+v 0.06760685328478817 -0.3273572227113141 0.34599894465474634
+v -0.5016676154381987 -0.40448012573438 0.3982501609948002
+v -0.19684555802363773 0.06703468423603398 0.5239649081389145
+v -0.2954149476882511 -0.4796288736642809 0.3020622994405346
+v -0.14087625107790458 -0.05458412356288634 0.5421876309607002
+v -0.4985664246283688 -0.4864625702138711 0.3189085888189619
+v -0.00021736430237647916 -0.4503172760413985 0.30652694553782617
+v -0.5504035000671088 -0.09529047774793714 0.42938244290602406
+v -0.5972481099650129 -0.20887727241235501 0.32321360023844853
+v -0.5372523037979573 -0.09489362398152588 0.4527820975419985
+v -0.5459940178258587 -0.28959838980926594 0.30012287180473335
+v -0.07980088612078434 -0.2555580226988366 0.5004106394621651
+v -0.536059662700914 -0.06297296071375963 0.4453812045473999
+v -0.5142251910586022 -0.26892126385913245 0.45365745765576165
+v -0.3761992712423714 0.044047127862896704 0.531113185973369
+v 0.021396814010689914 -0.023506272675528888 0.44878612271419277
+v -0.4785895892647789 -0.3772991310351158 0.43417649560708643
+v -0.04057371307687037 -0.28472179118277163 0.4725031474014694
+v -0.06379373515924976 0.035641192877822914 0.48501104930628475
+v -0.268552480346635 -0.01147951479006362 0.563930322448866
+v -0.48140519133600373 -0.20596920034000168 0.4963839695996218
+v -0.4676925046277537 -0.40085215202202507 0.4316135062118517
+v -0.09945340657171643 0.023356416620359075 0.5039036037550368
+v -0.2907495866483775 -0.36274823551761187 0.5336687503705929
+v -0.04149962551469581 -0.5000863122817075 0.3241777229620437
+v 0.055784870593456645 -0.20970525084905087 0.41975171852912213
+v -0.3694954938488563 -0.16019083407830673 0.557952867325209
+v -0.32546607979209224 -0.1488984347889514 0.5684618183311246
+v -0.4864234793855924 -0.48239401938540655 0.36379806421152355
+v -0.5546701485744439 -0.1265445746961647 0.42308789131320534
+v -0.5575742514147694 -0.27430544308086047 0.4039746818819943
+v -0.08433836383823479 -0.3455791286002283 0.47199219934492403
+v -0.37183734172977495 -0.5150063281537091 0.41601105318819254
+v -0.4021988410118873 -0.17317239367256465 0.548957562899091
+v -0.0930615403545105 -0.4303561261777834 0.44266573959426403
+v -0.18225400911558487 -0.4551215948132624 0.4603010281335543
+v -0.46560815416105494 -0.3383624702986354 0.46276118920299036
+v -0.19879229181467856 -0.037650812450389315 0.5574652574032906
+v -0.27549673798946506 -0.444700751316397 0.4769310058214301
+v -0.1569658738779502 0.07924042091311725 0.49202131165085655
+v -0.08118867168698839 0.048730218343169006 0.46099135135491487
+v -0.19197427654627053 -0.31432664023095847 0.5281530615682523
+v -0.016236913499099986 -0.4572634404841614 0.35427853418093674
+v -0.5670532773441858 -0.013667255230511191 0.37216677230309103
+v -0.1600702440346251 -0.018218162437105417 0.5395571358378572
+v -0.08508875367359162 -0.1884207475417829 0.5148470357560508
+v -0.48709801336252445 -0.2742352444341565 0.4757797647482181
+v -0.1804870258593016 -0.09486222522204647 0.5543107554492978
+v -0.40293893045744555 -0.13883635986668663 0.5475099735687757
+v -0.15484525774267938 -0.485765021532002 0.4193170102599116
+v -0.5957279576002608 -0.226995108251396 0.30757102000434566
+v -0.13524689896720338 -0.5203807370440556 0.36406142629441485
+v -0.4322738797800809 -0.08828863283119354 0.535344878380172
+v -0.07929114039723988 -0.4912184092734881 0.31194152100979133
+v -0.5393072007103122 -0.24436691161122662 0.4294432944220774
+v -0.33486716432809854 0.06526509221410337 0.4697037968914516
+v -0.598676550628925 -0.1189421132205157 0.34444577152051387
+v -0.05958966622524293 -0.27588260366739276 0.4857656768031185
+v -0.15526796103692886 -0.49779201325519035 0.4081319238472725
+v 0.03268313382800418 -0.39365571182347076 0.3624088350375639
+v 0.05682606888384067 -0.36138116420724786 0.3477239822837412
+v -0.05665627063658426 -0.15191580842532054 0.5055886728103176
+v -0.2653347072890573 -0.12105709715903419 0.5705166043512918
+v -0.013565420907915533 -0.40005736150731536 0.40099520768933283
+v -0.23946974182161074 -0.5147016554010905 0.4167803857859956
+v 0.02891607776427203 -0.32617171497403985 0.40720091623092974
g foo
-f 10 92 70
-f 104 10 126
-f 60 92 68
-f 60 70 85
-f 60 92 70
-f 34 120 127
-f 58 10 38
-f 86 10 38
-f 86 10 92
-f 57 40 24
-f 101 104 10
-f 101 67 41
-f 101 67 104
-f 69 148 24
-f 89 15 2
-f 89 81 68
-f 89 81 2
-f 89 139 68
-f 144 31 120
-f 109 34 127
-f 109 69 66
-f 5 73 66
-f 25 73 118
-f 119 5 62
-f 119 5 66
-f 76 104 126
-f 76 57 104
-f 108 67 104
-f 33 58 10
-f 33 101 10
-f 117 120 127
-f 147 17 41
-f 14 60 68
-f 14 81 68
-f 71 139 68
-f 71 15 30
-f 21 89 15
-f 21 89 139
-f 21 71 15
-f 21 71 139
-f 137 144 30
-f 137 144 31
-f 150 34 30
-f 150 144 30
-f 121 34 120
-f 121 150 34
-f 121 150 144
-f 29 92 68
-f 128 31 120
-f 124 34 30
-f 124 109 34
-f 22 109 127
-f 22 109 69
-f 22 69 72
-f 22 143 72
-f 22 117 127
-f 22 117 143
-f 16 25 73
-f 16 73 66
-f 16 109 66
-f 16 109 118
-f 16 25 118
-f 4 70 85
-f 4 37 85
-f 4 10 70
-f 28 82 54
-f 28 82 11
-f 90 67 41
-f 90 17 41
-f 90 123 67
-f 90 123 24
-f 77 69 72
-f 77 17 72
-f 77 90 17
-f 77 69 24
-f 19 8 40
-f 19 100 11
-f 19 100 8
-f 105 40 24
-f 105 8 24
-f 105 8 40
-f 125 76 126
-f 141 76 54
-f 131 17 72
-f 131 147 17
-f 75 13 116
-f 75 64 13
-f 59 81 2
-f 59 96 60
-f 110 128 31
-f 110 128 27
-f 110 137 31
-f 110 71 30
-f 110 137 30
-f 110 71 68
-f 78 142 85
-f 135 144 120
-f 135 121 120
-f 135 121 144
-f 55 29 27
-f 55 146 86
-f 55 86 92
-f 55 29 92
-f 80 146 86
-f 80 86 38
-f 80 55 146
-f 80 55 27
-f 48 118 30
-f 48 124 30
-f 48 109 118
-f 48 124 109
-f 91 15 30
-f 91 15 2
-f 114 118 30
-f 114 91 30
-f 114 75 87
-f 36 99 116
-f 36 75 116
-f 36 75 87
-f 84 95 116
-f 84 99 95
-f 46 37 85
-f 46 142 85
-f 46 142 37
-f 93 95 37
-f 93 142 37
-f 93 142 95
-f 42 4 37
-f 42 129 37
-f 42 129 54
-f 42 4 10
-f 42 141 54
-f 42 125 126
-f 42 10 126
-f 42 141 76
-f 42 125 76
-f 3 119 62
-f 106 7 95
-f 106 99 62
-f 106 99 95
-f 106 3 62
-f 106 3 7
-f 138 7 95
-f 98 95 37
-f 88 108 67
-f 88 57 24
-f 88 123 24
-f 88 57 104
-f 88 108 104
-f 133 90 24
-f 133 77 24
-f 133 77 90
-f 112 119 66
-f 32 57 40
-f 32 19 40
-f 32 19 57
-f 49 76 57
-f 49 19 57
-f 61 33 58
-f 61 33 101
-f 61 101 41
-f 65 151 120
-f 65 117 120
-f 65 117 143
-f 65 103 143
-f 97 143 72
-f 97 131 72
-f 18 64 2
-f 18 59 2
-f 18 59 60
-f 47 59 81
-f 47 59 96
-f 47 14 81
-f 47 14 60
-f 47 96 60
-f 74 29 27
-f 74 110 27
-f 74 29 68
-f 74 110 68
-f 115 142 95
-f 115 78 142
-f 115 95 116
-f 115 78 116
-f 1 80 151
-f 1 80 38
-f 1 58 38
-f 1 61 58
-f 149 128 27
-f 149 128 120
-f 149 151 120
-f 149 80 151
-f 107 114 87
-f 107 36 87
-f 107 43 73
-f 35 91 2
-f 35 114 91
-f 35 64 2
-f 35 75 64
-f 35 114 75
-f 140 99 116
-f 140 84 116
-f 140 84 99
-f 134 138 119
-f 12 3 119
-f 12 138 119
-f 12 138 3
-f 20 3 7
-f 20 138 7
-f 20 138 3
-f 50 100 8
-f 50 138 95
-f 130 123 67
-f 130 88 67
-f 130 88 123
-f 56 19 11
-f 56 49 19
-f 56 82 11
-f 56 82 54
-f 56 76 54
-f 56 49 76
-f 44 65 151
-f 44 65 103
-f 136 60 85
-f 136 18 60
-f 9 103 143
-f 145 80 27
-f 145 149 27
-f 145 149 80
-f 39 107 43
-f 39 107 114
-f 39 114 118
-f 39 73 118
-f 39 43 73
-f 132 99 62
-f 132 36 99
-f 132 5 62
-f 132 5 73
-f 63 50 51
-f 63 50 138
-f 53 98 37
-f 53 98 95
-f 53 50 95
-f 53 129 37
-f 53 28 11
-f 53 129 54
-f 53 28 54
-f 94 78 85
-f 94 136 85
-f 94 13 116
-f 94 78 116
-f 83 18 64
-f 83 136 18
-f 83 64 13
-f 83 94 13
-f 83 94 136
-f 52 44 103
-f 52 9 103
-f 52 44 151
-f 52 9 151
-f 23 9 143
-f 23 97 143
-f 23 131 147
-f 23 97 131
-f 23 147 41
-f 23 61 41
-f 23 1 61
-f 111 1 151
-f 111 9 151
-f 111 23 1
-f 111 23 9
-f 122 107 73
-f 122 132 73
-f 102 107 36
-f 102 132 36
-f 102 122 107
-f 102 122 132
-f 6 112 119
-f 6 112 66
-f 26 50 100
-f 26 53 50
-f 26 100 11
-f 26 53 11
-f 79 6 119
-f 79 6 63
-f 79 134 119
-f 79 134 138
-f 79 63 138
-f 45 63 51
-f 45 6 63
-f 45 50 8
-f 45 50 51
-f 45 8 24
-f 113 45 6
-f 113 6 66
-f 113 148 24
-f 113 45 24
-f 113 69 66
-f 113 69 148
-g
+f 10 92 70
+f 104 10 126
+f 60 92 68
+f 60 70 85
+f 60 92 70
+f 34 120 127
+f 58 10 38
+f 86 10 38
+f 86 10 92
+f 57 40 24
+f 101 104 10
+f 101 67 41
+f 101 67 104
+f 69 148 24
+f 89 15 2
+f 89 81 68
+f 89 81 2
+f 89 139 68
+f 144 31 120
+f 109 34 127
+f 109 69 66
+f 5 73 66
+f 25 73 118
+f 119 5 62
+f 119 5 66
+f 76 104 126
+f 76 57 104
+f 108 67 104
+f 33 58 10
+f 33 101 10
+f 117 120 127
+f 147 17 41
+f 14 60 68
+f 14 81 68
+f 71 139 68
+f 71 15 30
+f 21 89 15
+f 21 89 139
+f 21 71 15
+f 21 71 139
+f 137 144 30
+f 137 144 31
+f 150 34 30
+f 150 144 30
+f 121 34 120
+f 121 150 34
+f 121 150 144
+f 29 92 68
+f 128 31 120
+f 124 34 30
+f 124 109 34
+f 22 109 127
+f 22 109 69
+f 22 69 72
+f 22 143 72
+f 22 117 127
+f 22 117 143
+f 16 25 73
+f 16 73 66
+f 16 109 66
+f 16 109 118
+f 16 25 118
+f 4 70 85
+f 4 37 85
+f 4 10 70
+f 28 82 54
+f 28 82 11
+f 90 67 41
+f 90 17 41
+f 90 123 67
+f 90 123 24
+f 77 69 72
+f 77 17 72
+f 77 90 17
+f 77 69 24
+f 19 8 40
+f 19 100 11
+f 19 100 8
+f 105 40 24
+f 105 8 24
+f 105 8 40
+f 125 76 126
+f 141 76 54
+f 131 17 72
+f 131 147 17
+f 75 13 116
+f 75 64 13
+f 59 81 2
+f 59 96 60
+f 110 128 31
+f 110 128 27
+f 110 137 31
+f 110 71 30
+f 110 137 30
+f 110 71 68
+f 78 142 85
+f 135 144 120
+f 135 121 120
+f 135 121 144
+f 55 29 27
+f 55 146 86
+f 55 86 92
+f 55 29 92
+f 80 146 86
+f 80 86 38
+f 80 55 146
+f 80 55 27
+f 48 118 30
+f 48 124 30
+f 48 109 118
+f 48 124 109
+f 91 15 30
+f 91 15 2
+f 114 118 30
+f 114 91 30
+f 114 75 87
+f 36 99 116
+f 36 75 116
+f 36 75 87
+f 84 95 116
+f 84 99 95
+f 46 37 85
+f 46 142 85
+f 46 142 37
+f 93 95 37
+f 93 142 37
+f 93 142 95
+f 42 4 37
+f 42 129 37
+f 42 129 54
+f 42 4 10
+f 42 141 54
+f 42 125 126
+f 42 10 126
+f 42 141 76
+f 42 125 76
+f 3 119 62
+f 106 7 95
+f 106 99 62
+f 106 99 95
+f 106 3 62
+f 106 3 7
+f 138 7 95
+f 98 95 37
+f 88 108 67
+f 88 57 24
+f 88 123 24
+f 88 57 104
+f 88 108 104
+f 133 90 24
+f 133 77 24
+f 133 77 90
+f 112 119 66
+f 32 57 40
+f 32 19 40
+f 32 19 57
+f 49 76 57
+f 49 19 57
+f 61 33 58
+f 61 33 101
+f 61 101 41
+f 65 151 120
+f 65 117 120
+f 65 117 143
+f 65 103 143
+f 97 143 72
+f 97 131 72
+f 18 64 2
+f 18 59 2
+f 18 59 60
+f 47 59 81
+f 47 59 96
+f 47 14 81
+f 47 14 60
+f 47 96 60
+f 74 29 27
+f 74 110 27
+f 74 29 68
+f 74 110 68
+f 115 142 95
+f 115 78 142
+f 115 95 116
+f 115 78 116
+f 1 80 151
+f 1 80 38
+f 1 58 38
+f 1 61 58
+f 149 128 27
+f 149 128 120
+f 149 151 120
+f 149 80 151
+f 107 114 87
+f 107 36 87
+f 107 43 73
+f 35 91 2
+f 35 114 91
+f 35 64 2
+f 35 75 64
+f 35 114 75
+f 140 99 116
+f 140 84 116
+f 140 84 99
+f 134 138 119
+f 12 3 119
+f 12 138 119
+f 12 138 3
+f 20 3 7
+f 20 138 7
+f 20 138 3
+f 50 100 8
+f 50 138 95
+f 130 123 67
+f 130 88 67
+f 130 88 123
+f 56 19 11
+f 56 49 19
+f 56 82 11
+f 56 82 54
+f 56 76 54
+f 56 49 76
+f 44 65 151
+f 44 65 103
+f 136 60 85
+f 136 18 60
+f 9 103 143
+f 145 80 27
+f 145 149 27
+f 145 149 80
+f 39 107 43
+f 39 107 114
+f 39 114 118
+f 39 73 118
+f 39 43 73
+f 132 99 62
+f 132 36 99
+f 132 5 62
+f 132 5 73
+f 63 50 51
+f 63 50 138
+f 53 98 37
+f 53 98 95
+f 53 50 95
+f 53 129 37
+f 53 28 11
+f 53 129 54
+f 53 28 54
+f 94 78 85
+f 94 136 85
+f 94 13 116
+f 94 78 116
+f 83 18 64
+f 83 136 18
+f 83 64 13
+f 83 94 13
+f 83 94 136
+f 52 44 103
+f 52 9 103
+f 52 44 151
+f 52 9 151
+f 23 9 143
+f 23 97 143
+f 23 131 147
+f 23 97 131
+f 23 147 41
+f 23 61 41
+f 23 1 61
+f 111 1 151
+f 111 9 151
+f 111 23 1
+f 111 23 9
+f 122 107 73
+f 122 132 73
+f 102 107 36
+f 102 132 36
+f 102 122 107
+f 102 122 132
+f 6 112 119
+f 6 112 66
+f 26 50 100
+f 26 53 50
+f 26 100 11
+f 26 53 11
+f 79 6 119
+f 79 6 63
+f 79 134 119
+f 79 134 138
+f 79 63 138
+f 45 63 51
+f 45 6 63
+f 45 50 8
+f 45 50 51
+f 45 8 24
+f 113 45 6
+f 113 6 66
+f 113 148 24
+f 113 45 24
+f 113 69 66
+f 113 69 148
+g
diff --git a/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_LHleg_effector_frame_quasi_static.obj b/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_LHleg_effector_frame_quasi_static.obj
index 658cabd21abfe064932b41eb8f1c8b79fdbf2723..5db4c61d2591c336fece6caf21590b8e631f4a32 100644
--- a/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_LHleg_effector_frame_quasi_static.obj
+++ b/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_LHleg_effector_frame_quasi_static.obj
@@ -1,433 +1,433 @@
-v 0.14718504427130963 -0.05289421603794576 0.5381205203333291
-v -0.0325655396713797 -0.40746733133095886 0.34923254571779144
-v 0.3372702582989003 -0.5168395673364624 0.34644868226193776
-v 0.097448969108509 0.04590200199452917 0.4436053332040123
-v 0.2002842898031027 0.082964044129212 0.5075845577250809
-v 0.025791550352176517 -0.1765543731730308 0.4884942947830726
-v -0.021279048194742505 -0.22851868616357207 0.4464173129243503
-v 0.3774195733615292 -0.3225747447196155 0.49005223397835823
-v 0.16663590251578247 -0.1279855724779591 0.5511953055463328
-v 0.1464518723836521 0.03930104156373047 0.5191137904779484
-v 0.5256810288010125 -0.34424513254027034 0.3026136739618094
-v 0.20215705142175375 -0.5237077251839067 0.3815486643323111
-v 0.07723137085320077 -0.3038044550710221 0.4898729258810734
-v 0.5414752882764422 -0.22988308480816316 0.34813967386270084
-v 0.3932698202107994 -0.032841161540851524 0.5074340587880191
-v 0.4862197294199816 -0.43948594968757787 0.31692367516590747
-v 0.5324469002664642 -0.27538886648790123 0.306234920769193
-v -0.03962663652736265 -0.43606064174982895 0.302469722685469
-v 0.03535205333063195 -0.3747966148865813 0.4312589777049594
-v 0.42359840927694353 -0.46313086846430707 0.36626976848584747
-v 0.08813526089700967 -0.1221433325477061 0.5221168118381133
-v 0.35741183040820546 -0.01496213446307269 0.3741253719335921
-v 0.5102554341205823 -0.3454512005653603 0.34995861035454
-v 0.4247477285884385 0.024138104737020563 0.47721227154172896
-v 0.5464194698025543 -0.22538760498617239 0.32587974751545856
-v -0.0785016855424069 -0.058739123088738104 0.3919623549042187
-v 0.178838499226153 -0.37987771130514936 0.49539764655116164
-v 0.3256298598740594 0.003013768091051857 0.5328820173525125
-v 0.5474259900919988 -0.1560826458466168 0.33527225954175716
-v 0.26656172996650235 -0.5294756870698242 0.3726047322821612
-v 0.38657351812774043 -0.43170959779370055 0.4274060498101651
-v 0.5059407717947517 -0.414533661246823 0.3157261887711238
-v 0.19016618854879674 -0.5032105407876963 0.4100935054606614
-v 0.37713692683167455 -0.21448574578250654 0.5179676972335624
-v 0.12016671644194217 -0.2920186397750128 0.5135628205931582
-v 0.3768014445978878 -0.005173400340579648 0.38010025430749356
-v 0.24308923685615108 -0.3808080734006606 0.5017014891541579
-v 0.4050009874982183 -0.19563447351618776 0.5058215856005503
-v 0.10057444435560947 -0.20501654117986 0.5234315250424799
-v 0.29731576629207196 -0.5242114825280705 0.38724276757748344
-v 0.10557392018280096 -0.44289490555486033 0.4315183450986902
-v 0.22142832822362934 0.014734778985374408 0.5407922198005216
-v 0.07594637218987735 0.042302515140973704 0.46627018301142337
-v 0.0880457720147416 -0.21579964691232947 0.5148949211629882
-v 0.16034264566806236 0.07191864283296595 0.5060599237727894
-v 0.2093246429444966 -0.17349579018108766 0.5583640330794547
-v -0.09412756257770427 -0.3025071297814084 0.33067262713911433
-v -0.026763340315977182 -0.2834840555523392 0.4238841834088653
-v 0.38640909876941165 -0.5061760317428099 0.3171623596867242
-v 0.18187956957394125 -0.26677543775949786 0.5403668434254439
-v 0.14201322631340377 -0.15033280617875228 0.5413608694158109
-v 0.11846811298711897 -0.4968706893953498 0.3854181234371374
-v 0.5324496488180664 -0.07014004332632347 0.33577782489750063
-v 0.3434637338454191 -0.3906263011371723 0.47000400195768843
-v 0.43216708336381476 0.037203419134348326 0.4647124100710224
-v -0.09193831130391728 -0.09708028751703733 0.38293172418506943
-v 0.39047803503900935 0.022351235649340384 0.49983280487387766
-v 0.5397446576759479 -0.11152993284984417 0.35731060602504866
-v 0.2565252733797818 -0.06375639230246724 0.5581208407652296
-v -0.0846284299366484 -0.370675074624652 0.31652095486706494
-v 0.5163994499366329 -0.0491097523755632 0.3916391258109787
-v 0.1631467042971894 -0.22200765501805703 0.5434732098081524
-v 0.45060617925270047 -0.43672003086524874 0.3608928434174421
-v 0.05566644777608788 -0.48908389285459486 0.3092047983334409
-v 0.19319061983068694 -0.3511482687302497 0.5088938904494235
-v 0.5293174087080565 -0.28647583199057103 0.34267962436072497
-v 0.05160134547034372 -0.10400752380818873 0.5027428548189901
-v 0.19479119728339722 -0.4697472612272584 0.4437676483790449
-v 0.16275576595159688 -0.417977642874627 0.46838509326541705
-v 0.3978089503712693 -0.5021282646284875 0.334992360230395
-v 0.3656369784122156 -0.49054494500049045 0.37920576270005185
-v 0.41575452813116043 -0.48408034957073537 0.30204069085192203
-v 0.4245528696956878 -0.3385630402246653 0.45069476317552754
-v 0.14851165790882165 -0.5297115336448919 0.36456362625571037
-v 0.45262729211645575 -0.33560784793398185 0.4238314804927496
-v 0.001671317936671115 -0.06364651795664879 0.4677872230807108
-v 0.44063922695083274 -0.42138586444435155 0.301148525036934
-v 0.443837860425937 -0.2987283284577278 0.4502779737946024
-v -0.04400160912600128 -0.4177503381128907 0.30126819090321416
-v 0.32692593908331435 -0.3537486741230297 0.49731302061023214
-v 0.4761820362567233 -0.3993815714735561 0.35982623036477607
-v 0.2989628468782636 -0.1491880729783054 0.5515604899792083
-v 0.5301000933340336 -0.07465301346183284 0.37112412838533204
-v 0.43005390845004166 -0.1069123555970402 0.4922954095957808
-v 0.5051843692634942 -0.20528034993579142 0.4155973135548635
-v 0.16612063369126923 -0.47456266863199087 0.4358328766636434
-v 0.3122930808842397 0.048589543015347625 0.5155263361931058
-v 0.532767675801792 -0.2891451405982435 0.3285829497664973
-v 0.16913118947889866 -0.050079175952601904 0.5435548748125256
-v 0.024251006767442785 -0.33900676697477156 0.44075768351696315
-v 0.46136864679068845 -0.18464814564727608 0.46568708426426325
-v -0.07996357083070699 -0.3816841757783335 0.30513932933656257
-v 0.5419119808747432 -0.05187475611715103 0.3393180603775564
-v 0.07509042964892758 -0.5014704575751466 0.3180766197651611
-v -0.03396159028573677 -0.1172198263574099 0.44698066599197733
-v 0.2960514615420258 -0.30530919779296056 0.525802049783385
-v 0.03450397661680968 -0.4795832987066409 0.30962976983855006
-v 0.041898215714246755 -0.4967110100694547 0.32446988416762085
-v 0.29380562214272765 -0.4734713243578009 0.3003340115807054
-v 0.1591324259528568 -0.4782764834440311 0.3013235258511607
-v 0.5276336621178337 -0.11912021583992749 0.3870324851615829
-v 0.4069343907014679 -0.10187930567015288 0.5047780561977363
-v -0.004668155715866344 -0.43579974592189563 0.35712726529677685
-v 0.27356031765064853 -0.09928022626568726 0.5566759875245015
-v 0.4630286447982832 -0.08766154151438162 0.46653202700588986
-v 0.5068251887822108 -0.3441844520593892 0.301673730014007
-v -0.04006208401295223 -0.32967259475975663 0.3981357406413689
-v 0.12678512343399687 -0.004197076056261535 0.5234007686795238
-v -0.09300333770076895 -0.23896133342696468 0.3786068444957566
-v 0.21718422095151463 -0.451635055365925 0.45624563479670677
-v 0.31078984733124027 -0.4295762481122356 0.4593644724093076
-v 0.4105090189591337 -0.2976076208461078 0.4822600545403294
-v 0.4708379496818156 -0.2499017696997221 0.4451567066605569
-v 0.3250768994836839 0.03306075292975516 0.4251893147467893
-v -0.06400900211010477 -0.02783600325202342 0.4006449740132621
-v 0.19461900632877574 -0.4876821228101697 0.4293882805036068
-v -0.04386069932862607 -0.009241517958156104 0.4081207857010228
-v 0.501782455140549 -0.40199620531846636 0.30185194783022123
-v 0.06913132266168244 -0.4255276295017908 0.4218054494550806
-v 0.011846621297585003 -0.2650893833610031 0.45922441535234887
-v 0.43825177012303473 0.012162199043316634 0.39467418804313564
-v -0.08074642015197207 -0.3870835799269538 0.30752688776618764
-v 0.034235834710230714 -0.15093222643000925 0.49314894621027566
-v 0.04497595239303351 0.027103751459416715 0.48074853382782057
-v 0.053846193932760834 -0.4774540573303553 0.3013941337125082
-v 0.26662993636015053 0.07857742518132713 0.49710702586764166
-v 0.09607561334135585 -0.06138971561486835 0.5224578847869547
-v 0.27000470205987587 -0.2625893318023102 0.5362734418399546
-v 0.51247736237451 -0.00294955441874366 0.3853370426108704
-v 0.10659077459892743 -0.34724827437832273 0.48232173080041435
-v 0.4429984916038468 -0.2042893248705521 0.4760372934589536
-v 0.4179000855422367 -0.38176644296247264 0.3010701086497841
-v 0.34947175591321045 -0.1396392102950793 0.5393766356620201
-v 0.05915937754692272 -0.49132030593568393 0.3581385892007127
-v 0.09917588245440288 0.044706187852750955 0.4996734710470955
-v 0.2116822111651215 0.06082071756677174 0.528144931516333
-v 0.4682432455516526 -0.26058234884295123 0.443566649571339
-v 0.2690061921162706 -0.46632175218819844 0.44082400680182565
-v 0.2966554241940871 -0.5195238760380931 0.34152310673893926
-v 0.04169185647261055 -0.021227013335315608 0.4882935952850909
-v 0.3775920799134929 -0.502528598866461 0.35893321075338847
-v 0.5373113349505223 -0.27720098713541963 0.31342845695344085
-v 0.35718298215978667 -0.07954172257473545 0.529636448264901
-v 0.4549002951610276 -0.4676052784886882 0.31247320057540817
-v 0.34055100301868585 -0.18342262722179759 0.5416724145358219
+v 0.14718504427130963 -0.05289421603794576 0.5381205203333291
+v -0.0325655396713797 -0.40746733133095886 0.34923254571779144
+v 0.3372702582989003 -0.5168395673364624 0.34644868226193776
+v 0.097448969108509 0.04590200199452917 0.4436053332040123
+v 0.2002842898031027 0.082964044129212 0.5075845577250809
+v 0.025791550352176517 -0.1765543731730308 0.4884942947830726
+v -0.021279048194742505 -0.22851868616357207 0.4464173129243503
+v 0.3774195733615292 -0.3225747447196155 0.49005223397835823
+v 0.16663590251578247 -0.1279855724779591 0.5511953055463328
+v 0.1464518723836521 0.03930104156373047 0.5191137904779484
+v 0.5256810288010125 -0.34424513254027034 0.3026136739618094
+v 0.20215705142175375 -0.5237077251839067 0.3815486643323111
+v 0.07723137085320077 -0.3038044550710221 0.4898729258810734
+v 0.5414752882764422 -0.22988308480816316 0.34813967386270084
+v 0.3932698202107994 -0.032841161540851524 0.5074340587880191
+v 0.4862197294199816 -0.43948594968757787 0.31692367516590747
+v 0.5324469002664642 -0.27538886648790123 0.306234920769193
+v -0.03962663652736265 -0.43606064174982895 0.302469722685469
+v 0.03535205333063195 -0.3747966148865813 0.4312589777049594
+v 0.42359840927694353 -0.46313086846430707 0.36626976848584747
+v 0.08813526089700967 -0.1221433325477061 0.5221168118381133
+v 0.35741183040820546 -0.01496213446307269 0.3741253719335921
+v 0.5102554341205823 -0.3454512005653603 0.34995861035454
+v 0.4247477285884385 0.024138104737020563 0.47721227154172896
+v 0.5464194698025543 -0.22538760498617239 0.32587974751545856
+v -0.0785016855424069 -0.058739123088738104 0.3919623549042187
+v 0.178838499226153 -0.37987771130514936 0.49539764655116164
+v 0.3256298598740594 0.003013768091051857 0.5328820173525125
+v 0.5474259900919988 -0.1560826458466168 0.33527225954175716
+v 0.26656172996650235 -0.5294756870698242 0.3726047322821612
+v 0.38657351812774043 -0.43170959779370055 0.4274060498101651
+v 0.5059407717947517 -0.414533661246823 0.3157261887711238
+v 0.19016618854879674 -0.5032105407876963 0.4100935054606614
+v 0.37713692683167455 -0.21448574578250654 0.5179676972335624
+v 0.12016671644194217 -0.2920186397750128 0.5135628205931582
+v 0.3768014445978878 -0.005173400340579648 0.38010025430749356
+v 0.24308923685615108 -0.3808080734006606 0.5017014891541579
+v 0.4050009874982183 -0.19563447351618776 0.5058215856005503
+v 0.10057444435560947 -0.20501654117986 0.5234315250424799
+v 0.29731576629207196 -0.5242114825280705 0.38724276757748344
+v 0.10557392018280096 -0.44289490555486033 0.4315183450986902
+v 0.22142832822362934 0.014734778985374408 0.5407922198005216
+v 0.07594637218987735 0.042302515140973704 0.46627018301142337
+v 0.0880457720147416 -0.21579964691232947 0.5148949211629882
+v 0.16034264566806236 0.07191864283296595 0.5060599237727894
+v 0.2093246429444966 -0.17349579018108766 0.5583640330794547
+v -0.09412756257770427 -0.3025071297814084 0.33067262713911433
+v -0.026763340315977182 -0.2834840555523392 0.4238841834088653
+v 0.38640909876941165 -0.5061760317428099 0.3171623596867242
+v 0.18187956957394125 -0.26677543775949786 0.5403668434254439
+v 0.14201322631340377 -0.15033280617875228 0.5413608694158109
+v 0.11846811298711897 -0.4968706893953498 0.3854181234371374
+v 0.5324496488180664 -0.07014004332632347 0.33577782489750063
+v 0.3434637338454191 -0.3906263011371723 0.47000400195768843
+v 0.43216708336381476 0.037203419134348326 0.4647124100710224
+v -0.09193831130391728 -0.09708028751703733 0.38293172418506943
+v 0.39047803503900935 0.022351235649340384 0.49983280487387766
+v 0.5397446576759479 -0.11152993284984417 0.35731060602504866
+v 0.2565252733797818 -0.06375639230246724 0.5581208407652296
+v -0.0846284299366484 -0.370675074624652 0.31652095486706494
+v 0.5163994499366329 -0.0491097523755632 0.3916391258109787
+v 0.1631467042971894 -0.22200765501805703 0.5434732098081524
+v 0.45060617925270047 -0.43672003086524874 0.3608928434174421
+v 0.05566644777608788 -0.48908389285459486 0.3092047983334409
+v 0.19319061983068694 -0.3511482687302497 0.5088938904494235
+v 0.5293174087080565 -0.28647583199057103 0.34267962436072497
+v 0.05160134547034372 -0.10400752380818873 0.5027428548189901
+v 0.19479119728339722 -0.4697472612272584 0.4437676483790449
+v 0.16275576595159688 -0.417977642874627 0.46838509326541705
+v 0.3978089503712693 -0.5021282646284875 0.334992360230395
+v 0.3656369784122156 -0.49054494500049045 0.37920576270005185
+v 0.41575452813116043 -0.48408034957073537 0.30204069085192203
+v 0.4245528696956878 -0.3385630402246653 0.45069476317552754
+v 0.14851165790882165 -0.5297115336448919 0.36456362625571037
+v 0.45262729211645575 -0.33560784793398185 0.4238314804927496
+v 0.001671317936671115 -0.06364651795664879 0.4677872230807108
+v 0.44063922695083274 -0.42138586444435155 0.301148525036934
+v 0.443837860425937 -0.2987283284577278 0.4502779737946024
+v -0.04400160912600128 -0.4177503381128907 0.30126819090321416
+v 0.32692593908331435 -0.3537486741230297 0.49731302061023214
+v 0.4761820362567233 -0.3993815714735561 0.35982623036477607
+v 0.2989628468782636 -0.1491880729783054 0.5515604899792083
+v 0.5301000933340336 -0.07465301346183284 0.37112412838533204
+v 0.43005390845004166 -0.1069123555970402 0.4922954095957808
+v 0.5051843692634942 -0.20528034993579142 0.4155973135548635
+v 0.16612063369126923 -0.47456266863199087 0.4358328766636434
+v 0.3122930808842397 0.048589543015347625 0.5155263361931058
+v 0.532767675801792 -0.2891451405982435 0.3285829497664973
+v 0.16913118947889866 -0.050079175952601904 0.5435548748125256
+v 0.024251006767442785 -0.33900676697477156 0.44075768351696315
+v 0.46136864679068845 -0.18464814564727608 0.46568708426426325
+v -0.07996357083070699 -0.3816841757783335 0.30513932933656257
+v 0.5419119808747432 -0.05187475611715103 0.3393180603775564
+v 0.07509042964892758 -0.5014704575751466 0.3180766197651611
+v -0.03396159028573677 -0.1172198263574099 0.44698066599197733
+v 0.2960514615420258 -0.30530919779296056 0.525802049783385
+v 0.03450397661680968 -0.4795832987066409 0.30962976983855006
+v 0.041898215714246755 -0.4967110100694547 0.32446988416762085
+v 0.29380562214272765 -0.4734713243578009 0.3003340115807054
+v 0.1591324259528568 -0.4782764834440311 0.3013235258511607
+v 0.5276336621178337 -0.11912021583992749 0.3870324851615829
+v 0.4069343907014679 -0.10187930567015288 0.5047780561977363
+v -0.004668155715866344 -0.43579974592189563 0.35712726529677685
+v 0.27356031765064853 -0.09928022626568726 0.5566759875245015
+v 0.4630286447982832 -0.08766154151438162 0.46653202700588986
+v 0.5068251887822108 -0.3441844520593892 0.301673730014007
+v -0.04006208401295223 -0.32967259475975663 0.3981357406413689
+v 0.12678512343399687 -0.004197076056261535 0.5234007686795238
+v -0.09300333770076895 -0.23896133342696468 0.3786068444957566
+v 0.21718422095151463 -0.451635055365925 0.45624563479670677
+v 0.31078984733124027 -0.4295762481122356 0.4593644724093076
+v 0.4105090189591337 -0.2976076208461078 0.4822600545403294
+v 0.4708379496818156 -0.2499017696997221 0.4451567066605569
+v 0.3250768994836839 0.03306075292975516 0.4251893147467893
+v -0.06400900211010477 -0.02783600325202342 0.4006449740132621
+v 0.19461900632877574 -0.4876821228101697 0.4293882805036068
+v -0.04386069932862607 -0.009241517958156104 0.4081207857010228
+v 0.501782455140549 -0.40199620531846636 0.30185194783022123
+v 0.06913132266168244 -0.4255276295017908 0.4218054494550806
+v 0.011846621297585003 -0.2650893833610031 0.45922441535234887
+v 0.43825177012303473 0.012162199043316634 0.39467418804313564
+v -0.08074642015197207 -0.3870835799269538 0.30752688776618764
+v 0.034235834710230714 -0.15093222643000925 0.49314894621027566
+v 0.04497595239303351 0.027103751459416715 0.48074853382782057
+v 0.053846193932760834 -0.4774540573303553 0.3013941337125082
+v 0.26662993636015053 0.07857742518132713 0.49710702586764166
+v 0.09607561334135585 -0.06138971561486835 0.5224578847869547
+v 0.27000470205987587 -0.2625893318023102 0.5362734418399546
+v 0.51247736237451 -0.00294955441874366 0.3853370426108704
+v 0.10659077459892743 -0.34724827437832273 0.48232173080041435
+v 0.4429984916038468 -0.2042893248705521 0.4760372934589536
+v 0.4179000855422367 -0.38176644296247264 0.3010701086497841
+v 0.34947175591321045 -0.1396392102950793 0.5393766356620201
+v 0.05915937754692272 -0.49132030593568393 0.3581385892007127
+v 0.09917588245440288 0.044706187852750955 0.4996734710470955
+v 0.2116822111651215 0.06082071756677174 0.528144931516333
+v 0.4682432455516526 -0.26058234884295123 0.443566649571339
+v 0.2690061921162706 -0.46632175218819844 0.44082400680182565
+v 0.2966554241940871 -0.5195238760380931 0.34152310673893926
+v 0.04169185647261055 -0.021227013335315608 0.4882935952850909
+v 0.3775920799134929 -0.502528598866461 0.35893321075338847
+v 0.5373113349505223 -0.27720098713541963 0.31342845695344085
+v 0.35718298215978667 -0.07954172257473545 0.529636448264901
+v 0.4549002951610276 -0.4676052784886882 0.31247320057540817
+v 0.34055100301868585 -0.18342262722179759 0.5416724145358219
g foo
-f 56 92 47
-f 36 4 117
-f 53 92 93
-f 26 92 93
-f 26 56 92
-f 43 4 117
-f 9 59 46
-f 9 89 59
-f 42 28 59
-f 42 89 59
-f 83 129 93
-f 49 125 72
-f 49 125 64
-f 126 129 55
-f 126 4 5
-f 126 114 4
-f 22 93 117
-f 22 36 117
-f 22 36 93
-f 87 28 57
-f 87 55 57
-f 87 126 55
-f 18 125 64
-f 122 92 47
-f 122 60 47
-f 122 18 92
-f 115 93 117
-f 115 26 93
-f 1 9 127
-f 1 9 89
-f 1 108 127
-f 1 108 89
-f 21 6 39
-f 21 51 39
-f 21 9 127
-f 21 51 9
-f 95 115 26
-f 95 26 56
-f 95 6 7
-f 62 9 46
-f 62 51 39
-f 62 51 9
-f 104 28 59
-f 104 133 28
-f 104 59 46
-f 15 28 57
-f 10 108 135
-f 10 42 89
-f 10 108 89
-f 17 53 93
-f 17 53 92
-f 101 83 93
-f 94 49 64
-f 94 49 139
-f 94 74 139
-f 3 70 40
-f 3 49 70
-f 30 74 139
-f 30 74 40
-f 30 3 40
-f 30 49 139
-f 30 3 49
-f 86 27 68
-f 86 52 74
-f 86 33 74
-f 121 129 93
-f 121 36 93
-f 121 126 129
-f 121 126 114
-f 121 36 4
-f 121 114 4
-f 61 83 129
-f 61 101 83
-f 24 129 55
-f 24 55 57
-f 24 15 57
-f 79 18 92
-f 79 17 92
-f 79 18 125
-f 79 99 125
-f 109 107 7
-f 109 95 7
-f 109 56 47
-f 109 60 47
-f 109 107 60
-f 109 95 56
-f 124 115 117
-f 124 43 117
-f 124 108 135
-f 124 140 127
-f 124 108 127
-f 45 4 5
-f 45 43 4
-f 45 124 43
-f 45 10 135
-f 45 124 135
-f 67 21 6
-f 67 140 127
-f 67 21 127
-f 82 104 46
-f 82 104 133
-f 143 133 28
-f 143 15 28
-f 143 15 133
-f 102 133 84
-f 102 15 84
-f 102 15 133
-f 141 70 40
-f 136 10 42
-f 136 45 5
-f 136 45 10
-f 136 42 28
-f 136 126 5
-f 136 87 126
-f 136 87 28
-f 25 93 29
-f 25 17 93
-f 14 23 85
-f 14 101 85
-f 14 25 29
-f 100 125 72
-f 100 99 72
-f 100 99 125
-f 77 99 72
-f 105 101 85
-f 105 61 101
-f 105 61 129
-f 105 15 84
-f 105 24 15
-f 105 24 129
-f 58 93 29
-f 58 101 93
-f 58 14 29
-f 58 14 101
-f 98 94 74
-f 98 94 64
-f 116 33 40
-f 116 86 68
-f 116 86 33
-f 12 74 40
-f 12 33 40
-f 12 33 74
-f 37 27 65
-f 37 110 68
-f 37 27 68
-f 69 27 41
-f 69 86 41
-f 69 86 27
-f 35 62 39
-f 44 6 39
-f 44 13 6
-f 44 35 39
-f 44 35 13
-f 130 27 41
-f 130 35 27
-f 130 35 13
-f 120 6 7
-f 120 13 6
-f 19 130 41
-f 19 130 13
-f 103 19 107
-f 103 122 18
-f 103 98 18
-f 76 95 115
-f 76 124 115
-f 76 124 140
-f 76 67 140
-f 76 67 95
-f 123 95 6
-f 123 67 6
-f 123 67 95
-f 32 23 81
-f 32 63 81
-f 32 16 63
-f 142 17 11
-f 142 25 11
-f 142 25 17
-f 113 23 85
-f 113 137 23
-f 113 137 112
-f 75 23 81
-f 75 137 23
-f 106 17 11
-f 106 79 17
-f 134 52 74
-f 134 98 74
-f 134 103 98
-f 134 86 41
-f 134 86 52
-f 97 18 64
-f 97 98 64
-f 97 98 18
-f 50 37 65
-f 50 27 65
-f 50 35 27
-f 50 35 62
-f 50 62 46
-f 48 107 7
-f 48 120 7
-f 48 120 107
-f 90 19 107
-f 90 19 13
-f 90 120 107
-f 90 120 13
-f 2 122 60
-f 2 103 122
-f 2 107 60
-f 2 103 107
-f 119 19 41
-f 119 103 19
-f 119 134 41
-f 119 134 103
-f 88 32 11
-f 88 25 11
-f 88 14 25
-f 66 14 23
-f 66 32 23
-f 66 88 14
-f 66 88 32
-f 118 32 16
-f 118 77 72
-f 118 32 11
-f 118 106 77
-f 118 106 11
-f 20 31 63
-f 20 16 63
-f 20 31 141
-f 20 16 70
-f 20 141 70
-f 71 141 40
-f 71 31 40
-f 71 31 141
-f 138 37 110
-f 138 116 40
-f 138 110 68
-f 138 116 68
-f 145 82 133
-f 145 82 46
-f 38 145 133
-f 38 133 84
-f 73 31 112
-f 73 75 81
-f 73 63 81
-f 73 31 63
-f 132 77 99
-f 132 106 77
-f 132 79 99
-f 132 106 79
-f 144 118 72
-f 144 118 16
-f 144 16 70
-f 144 49 72
-f 144 49 70
-f 8 31 112
-f 8 145 112
-f 111 31 40
-f 111 138 40
-f 111 80 37
-f 111 138 37
-f 128 145 46
-f 128 50 46
-f 131 113 112
-f 131 38 112
-f 34 145 112
-f 34 38 112
-f 34 38 145
-f 78 75 137
-f 78 73 75
-f 78 137 112
-f 78 73 112
-f 96 8 80
-f 96 8 145
-f 96 128 145
-f 96 80 37
-f 96 50 37
-f 96 128 50
-f 54 111 31
-f 54 111 80
-f 54 8 31
-f 54 8 80
-f 91 131 113
-f 91 113 85
-f 91 105 85
-f 91 105 84
-f 91 38 84
-f 91 131 38
-g
+f 56 92 47
+f 36 4 117
+f 53 92 93
+f 26 92 93
+f 26 56 92
+f 43 4 117
+f 9 59 46
+f 9 89 59
+f 42 28 59
+f 42 89 59
+f 83 129 93
+f 49 125 72
+f 49 125 64
+f 126 129 55
+f 126 4 5
+f 126 114 4
+f 22 93 117
+f 22 36 117
+f 22 36 93
+f 87 28 57
+f 87 55 57
+f 87 126 55
+f 18 125 64
+f 122 92 47
+f 122 60 47
+f 122 18 92
+f 115 93 117
+f 115 26 93
+f 1 9 127
+f 1 9 89
+f 1 108 127
+f 1 108 89
+f 21 6 39
+f 21 51 39
+f 21 9 127
+f 21 51 9
+f 95 115 26
+f 95 26 56
+f 95 6 7
+f 62 9 46
+f 62 51 39
+f 62 51 9
+f 104 28 59
+f 104 133 28
+f 104 59 46
+f 15 28 57
+f 10 108 135
+f 10 42 89
+f 10 108 89
+f 17 53 93
+f 17 53 92
+f 101 83 93
+f 94 49 64
+f 94 49 139
+f 94 74 139
+f 3 70 40
+f 3 49 70
+f 30 74 139
+f 30 74 40
+f 30 3 40
+f 30 49 139
+f 30 3 49
+f 86 27 68
+f 86 52 74
+f 86 33 74
+f 121 129 93
+f 121 36 93
+f 121 126 129
+f 121 126 114
+f 121 36 4
+f 121 114 4
+f 61 83 129
+f 61 101 83
+f 24 129 55
+f 24 55 57
+f 24 15 57
+f 79 18 92
+f 79 17 92
+f 79 18 125
+f 79 99 125
+f 109 107 7
+f 109 95 7
+f 109 56 47
+f 109 60 47
+f 109 107 60
+f 109 95 56
+f 124 115 117
+f 124 43 117
+f 124 108 135
+f 124 140 127
+f 124 108 127
+f 45 4 5
+f 45 43 4
+f 45 124 43
+f 45 10 135
+f 45 124 135
+f 67 21 6
+f 67 140 127
+f 67 21 127
+f 82 104 46
+f 82 104 133
+f 143 133 28
+f 143 15 28
+f 143 15 133
+f 102 133 84
+f 102 15 84
+f 102 15 133
+f 141 70 40
+f 136 10 42
+f 136 45 5
+f 136 45 10
+f 136 42 28
+f 136 126 5
+f 136 87 126
+f 136 87 28
+f 25 93 29
+f 25 17 93
+f 14 23 85
+f 14 101 85
+f 14 25 29
+f 100 125 72
+f 100 99 72
+f 100 99 125
+f 77 99 72
+f 105 101 85
+f 105 61 101
+f 105 61 129
+f 105 15 84
+f 105 24 15
+f 105 24 129
+f 58 93 29
+f 58 101 93
+f 58 14 29
+f 58 14 101
+f 98 94 74
+f 98 94 64
+f 116 33 40
+f 116 86 68
+f 116 86 33
+f 12 74 40
+f 12 33 40
+f 12 33 74
+f 37 27 65
+f 37 110 68
+f 37 27 68
+f 69 27 41
+f 69 86 41
+f 69 86 27
+f 35 62 39
+f 44 6 39
+f 44 13 6
+f 44 35 39
+f 44 35 13
+f 130 27 41
+f 130 35 27
+f 130 35 13
+f 120 6 7
+f 120 13 6
+f 19 130 41
+f 19 130 13
+f 103 19 107
+f 103 122 18
+f 103 98 18
+f 76 95 115
+f 76 124 115
+f 76 124 140
+f 76 67 140
+f 76 67 95
+f 123 95 6
+f 123 67 6
+f 123 67 95
+f 32 23 81
+f 32 63 81
+f 32 16 63
+f 142 17 11
+f 142 25 11
+f 142 25 17
+f 113 23 85
+f 113 137 23
+f 113 137 112
+f 75 23 81
+f 75 137 23
+f 106 17 11
+f 106 79 17
+f 134 52 74
+f 134 98 74
+f 134 103 98
+f 134 86 41
+f 134 86 52
+f 97 18 64
+f 97 98 64
+f 97 98 18
+f 50 37 65
+f 50 27 65
+f 50 35 27
+f 50 35 62
+f 50 62 46
+f 48 107 7
+f 48 120 7
+f 48 120 107
+f 90 19 107
+f 90 19 13
+f 90 120 107
+f 90 120 13
+f 2 122 60
+f 2 103 122
+f 2 107 60
+f 2 103 107
+f 119 19 41
+f 119 103 19
+f 119 134 41
+f 119 134 103
+f 88 32 11
+f 88 25 11
+f 88 14 25
+f 66 14 23
+f 66 32 23
+f 66 88 14
+f 66 88 32
+f 118 32 16
+f 118 77 72
+f 118 32 11
+f 118 106 77
+f 118 106 11
+f 20 31 63
+f 20 16 63
+f 20 31 141
+f 20 16 70
+f 20 141 70
+f 71 141 40
+f 71 31 40
+f 71 31 141
+f 138 37 110
+f 138 116 40
+f 138 110 68
+f 138 116 68
+f 145 82 133
+f 145 82 46
+f 38 145 133
+f 38 133 84
+f 73 31 112
+f 73 75 81
+f 73 63 81
+f 73 31 63
+f 132 77 99
+f 132 106 77
+f 132 79 99
+f 132 106 79
+f 144 118 72
+f 144 118 16
+f 144 16 70
+f 144 49 72
+f 144 49 70
+f 8 31 112
+f 8 145 112
+f 111 31 40
+f 111 138 40
+f 111 80 37
+f 111 138 37
+f 128 145 46
+f 128 50 46
+f 131 113 112
+f 131 38 112
+f 34 145 112
+f 34 38 112
+f 34 38 145
+f 78 75 137
+f 78 73 75
+f 78 137 112
+f 78 73 112
+f 96 8 80
+f 96 8 145
+f 96 128 145
+f 96 80 37
+f 96 50 37
+f 96 128 50
+f 54 111 31
+f 54 111 80
+f 54 8 31
+f 54 8 80
+f 91 131 113
+f 91 113 85
+f 91 105 85
+f 91 105 84
+f 91 38 84
+f 91 131 38
+g
diff --git a/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_RFleg_effector_frame_quasi_static.obj b/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_RFleg_effector_frame_quasi_static.obj
index e45cdac4f44982037a14461db7d8a47ab9843d26..14132b4a9ec102670d1eb2b4bf0b2ab7a23e9996 100644
--- a/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_RFleg_effector_frame_quasi_static.obj
+++ b/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_RFleg_effector_frame_quasi_static.obj
@@ -1,433 +1,433 @@
-v -0.033781317859038484 0.40130760853477837 0.4154816714239452
-v -0.26122030818196024 0.18157818980490206 0.5716190594095794
-v -0.26992883804740625 0.1891325888862745 0.570372202612184
-v -0.39602149921177493 0.4917386414992059 0.31755892063860586
-v -0.5851744912050141 0.10016680683496634 0.3682509433522116
-v -0.2238957224188915 0.1386701755465916 0.5750231667600141
-v -0.31904692083636466 0.19657171812415528 0.5698657834057875
-v -0.1538859093285957 0.22752679359528336 0.5427782166190164
-v -0.020212580596457006 0.2700537825418324 0.46851308419025156
-v -0.30040648778433093 0.0004241463001213716 0.5571225316987237
-v -0.3984395685300217 0.2160261635470895 0.5435027079740844
-v -0.40172683259314373 0.2597308993384054 0.5350632102047277
-v -0.577374154628489 0.23379319503458373 0.30129167595377915
-v -0.24582194638959048 0.5186071286326456 0.4171939407852477
-v -0.023108276394261926 0.49169299324417565 0.3047835928841373
-v 0.07097848531934235 0.3039423689626244 0.36740135193680995
-v -0.5839366237355387 0.26043120307407425 0.30075615125577054
-v -0.0879733771932322 -0.03587455335256726 0.5007891780838751
-v 0.01761263236489769 0.4621595867721133 0.30478242082127577
-v -0.3948419322217334 0.5294163062637769 0.3803337938594261
-v -0.028181187726924604 0.4884813969522166 0.32790058588242893
-v -0.22844909795248397 0.2584288474214522 0.5487908112996894
-v -0.48760637672755136 0.4821804169688773 0.3180990006467621
-v -0.2711513226096671 0.273618886921846 0.5501883047450713
-v -0.3336554576875997 -0.03994502928944427 0.5445147736478433
-v -0.08759489324379544 0.3108696051050959 0.49367056328856246
-v -0.3994862639252838 0.2299746865087289 0.5417270809522268
-v -0.3309871727541951 0.31007623517260374 0.5390178946359265
-v -0.597761190535275 0.1293465742508656 0.30778360487737516
-v -0.4694791674850333 0.20282237070023434 0.510343201977993
-v -0.47138609952465477 -0.051209637452609216 0.46396575294438347
-v -0.1958254979779554 0.5379776149633797 0.37741518688390685
-v -0.4052561700411869 0.48033092463496496 0.30833747234550984
-v 0.07736612814863489 0.39956872398319027 0.3044632875369468
-v -0.10805443960978742 0.14135456599965182 0.5350487538235585
-v -0.332429806373103 0.1871109212356554 0.5674742804982271
-v -0.22027390706783798 0.31183785003009457 0.5320368419835447
-v -0.5983271518821642 0.27064581413748534 0.30164097744942653
-v -0.3001343719378102 0.27741635271707166 0.5512990206168228
-v -0.13697061389127466 0.5243491114859729 0.3761504232876936
-v -0.4785162084679789 0.4559662871477316 0.30161176733554396
-v -0.278090965385684 -0.056576164267525486 0.544307749444612
-v -0.43748924648177445 0.3663769474191438 0.4826689599798718
-v -0.34285039397280787 0.01803250809572362 0.560940129446961
-v -0.3616764704216018 -0.03912710872637529 0.5397051821704819
-v -0.03250083203819924 0.14626891221099722 0.49149476076245036
-v -0.03026881189612496 0.18032914913693113 0.48819761149496044
-v -0.21439871091315663 0.23131525700241307 0.5546667552613116
-v -0.4630075898691192 0.003096006551007048 0.5104813081884189
-v -0.5353906415825894 0.23537041834023087 0.4369796611741769
-v -0.06439854202732424 0.4450769047454775 0.40444377562858597
-v -0.49488998403943746 0.27539977474893124 0.4706909662856182
-v -0.604917764442331 0.17042572767467953 0.30840060901749594
-v -0.36588688993503443 0.03845160099792462 0.5586095285408778
-v 0.05654095695105388 0.22957548920292087 0.4117933322025361
-v -0.2625373710651128 -0.08470574898001182 0.5301851549845218
-v -0.05133695009936132 0.25216907086981727 0.48989655867317006
-v -0.2533960153290386 0.5439995699201481 0.3995304105078988
-v -0.33597220313459286 0.3631333534725652 0.5194856145182288
-v -0.12440647759829997 0.3966965997373461 0.47059101848634355
-v -0.4504997046377538 0.11749271591654088 0.5282346051618744
-v -0.548109328433384 0.14841801244497888 0.4277355955777234
-v -0.30253504949581755 0.5478963618467816 0.39544302205849313
-v 0.04370892371845821 0.1959995680277085 0.43491743557280765
-v -0.45715856136835953 0.2016432604668219 0.5196558859578044
-v -0.5374421320339828 0.43875720488287706 0.3205112500497048
-v -0.05938070083187787 0.03480131917107378 0.5063797715588573
-v -0.006191930279689445 -0.04522756646051758 0.44514300280779756
-v -0.02743083275171426 0.058443421428687595 0.4870183849835222
-v -0.578695417515536 0.03889779147623815 0.3496351204228157
-v 0.03025882203700732 -0.026586897929879054 0.41047540831067963
-v -0.5161517770376761 -0.009924833238252292 0.44979263180400864
-v -0.40852590693168206 0.10216602993226256 0.5463824198772008
-v 0.018609478101246715 0.09688614451330058 0.4566278191993324
-v -0.1579674641733427 -0.021196210805302235 0.5366077857043555
-v -0.06943533197784822 0.35306691480198105 0.46610011728020195
-v -0.33147491824894654 -0.08167002289884803 0.5211117471666511
-v 0.026623700292591264 0.04730019698126442 0.4455513823457698
-v -0.5792062571910832 0.09763213317961088 0.38236104074175237
-v -0.18171451029012284 0.3573819608566569 0.5111831913684813
-v -0.3715033518142474 -0.06302832707678513 0.5268159183933534
-v -0.06857579757258787 0.4795246837059511 0.37168991191518846
-v -0.48485963785959524 0.48917635202180754 0.35029547088992546
-v -0.6010226871215913 0.09143981332978034 0.326608389519314
-v -0.27952433584060904 0.35609861741705723 0.5261114793089792
-v 0.06813303991024666 0.14064353622345457 0.41381690002054755
-v -0.5174976529058086 0.055831262616376096 0.4615737445416433
-v -0.3376737873711702 0.044700184593877895 0.5666056353862461
-v -0.031173292892036658 -0.053672645774334365 0.4509897293034927
-v 0.05445319274734333 0.3174221742513215 0.38218360172172705
-v -0.5242927014953698 0.34953470177654505 0.4141706448639366
-v -0.5024976526699059 0.14735951121533866 0.48389348492640843
-v -0.3123519261683843 0.1681797834617138 0.5712205474470073
-v -0.4586659182656778 0.4234456664060631 0.43475536121764663
-v -0.005074102845763751 0.4539092156472814 0.3449902478507794
-v -0.1384589435667647 0.049149720989248066 0.5438963106344396
-v -0.5889696681899163 0.21816933613462283 0.3612914916634434
-v -0.5718231780305036 0.0671174597916259 0.32009051424374846
-v -0.5732266814377932 0.09945529149919852 0.39464755007926283
-v -0.13541141796087253 0.22270208943032893 0.535385892447009
-v -0.04409871919435178 0.20899396932869016 0.49516479117998813
-v -0.4447930685992698 -0.06022321311554486 0.46746193423542787
-v -0.2947599385673379 0.32234331245835773 0.5392209378192288
-v -0.5446667163487982 0.2654770745434011 0.30087399243703494
-v -0.5450742596322874 0.44730387418429984 0.30022212822968636
-v -0.3439542491125273 0.0741696653379742 0.5710926554870503
-v -0.5375900463156392 0.37068271357888577 0.3776518195334018
-v -0.22616360752445525 -0.06799036467316029 0.47304113160667294
-v -0.2803534734452251 0.5152335444522472 0.4239161319467353
-v -0.4795071232436487 0.4905214373093612 0.33025164977930094
-v -0.2956127195269621 0.44462788160953176 0.48130732179977803
-v -0.36368198253800027 0.446270511306141 0.46699300848424713
-v -0.3206377338616592 0.4132241034532992 0.5028476596155308
-v -0.5918040693433165 0.2259668918840301 0.30249846169687084
-v -0.5941368820309284 0.29276390014837045 0.3186175550442184
-v -0.4421700751848822 -0.04481982462103495 0.5069908837435124
-v -0.27926446048434556 0.22713599600155482 0.563171728989051
-v -0.5486331895415514 0.021117509231564018 0.4170183887208556
-v 0.04168451066610792 0.41901282752675095 0.3014507356020942
-v -0.2751941820049736 0.5472319538859967 0.3964535331800077
-v -0.005131079464605534 0.14728130323470776 0.4721696933469555
-v -0.39643404450623176 0.5175255050366494 0.35028663417387684
-v -0.15859800847555955 0.4539850697079341 0.44628052008044344
-v -0.5693146432176401 0.19122645886335943 0.4006484516390678
-v 0.04308470485815255 0.42142913229562773 0.330626430891228
-v -0.07374924733391784 0.47152216172420686 0.3850865115437886
-v -0.3526596820226224 -0.08523793003069545 0.5137386897684328
-v -0.5512745409708739 0.4270165562260182 0.30860866940096826
-v -0.15955880687006993 0.20407328983185966 0.5472278806061864
-v -0.5823803844048459 0.2745662046640092 0.3586060216644692
-v 0.011067745916944077 0.324920830856993 0.4294353709800779
-v -0.35695765958928416 0.14978230650827654 0.5643306537686608
-v -0.50604315515569 -0.035194880678856226 0.4558071072952242
-v -0.3192328505324374 0.5255844626969151 0.41064788535809943
-v -0.22595761456924907 -0.06794167996290817 0.540335600418681
-v 0.08018645782483326 0.3487076182917045 0.3306286425013591
-v -0.21973572548871254 0.07992613735767634 0.5729077260419292
-v -0.3648698606118231 -0.004686798126572677 0.5501861579530598
-v -0.14702226078528463 0.45049059483402626 0.445862496175291
-v -0.558270245724605 0.2212396939708901 0.4133277036084426
-v -0.2331026099071795 0.41975861583230767 0.488011481464437
-v -0.550277609731042 0.040215987106635986 0.32288954385974367
-v -0.5523297194161292 0.0030439968003600658 0.3688015198414387
-v -0.3461778744505543 -0.08305310410606605 0.5066583819115462
-v -0.13275101906415798 -0.07392038428008547 0.4968440978045505
+v -0.033781317859038484 0.40130760853477837 0.4154816714239452
+v -0.26122030818196024 0.18157818980490206 0.5716190594095794
+v -0.26992883804740625 0.1891325888862745 0.570372202612184
+v -0.39602149921177493 0.4917386414992059 0.31755892063860586
+v -0.5851744912050141 0.10016680683496634 0.3682509433522116
+v -0.2238957224188915 0.1386701755465916 0.5750231667600141
+v -0.31904692083636466 0.19657171812415528 0.5698657834057875
+v -0.1538859093285957 0.22752679359528336 0.5427782166190164
+v -0.020212580596457006 0.2700537825418324 0.46851308419025156
+v -0.30040648778433093 0.0004241463001213716 0.5571225316987237
+v -0.3984395685300217 0.2160261635470895 0.5435027079740844
+v -0.40172683259314373 0.2597308993384054 0.5350632102047277
+v -0.577374154628489 0.23379319503458373 0.30129167595377915
+v -0.24582194638959048 0.5186071286326456 0.4171939407852477
+v -0.023108276394261926 0.49169299324417565 0.3047835928841373
+v 0.07097848531934235 0.3039423689626244 0.36740135193680995
+v -0.5839366237355387 0.26043120307407425 0.30075615125577054
+v -0.0879733771932322 -0.03587455335256726 0.5007891780838751
+v 0.01761263236489769 0.4621595867721133 0.30478242082127577
+v -0.3948419322217334 0.5294163062637769 0.3803337938594261
+v -0.028181187726924604 0.4884813969522166 0.32790058588242893
+v -0.22844909795248397 0.2584288474214522 0.5487908112996894
+v -0.48760637672755136 0.4821804169688773 0.3180990006467621
+v -0.2711513226096671 0.273618886921846 0.5501883047450713
+v -0.3336554576875997 -0.03994502928944427 0.5445147736478433
+v -0.08759489324379544 0.3108696051050959 0.49367056328856246
+v -0.3994862639252838 0.2299746865087289 0.5417270809522268
+v -0.3309871727541951 0.31007623517260374 0.5390178946359265
+v -0.597761190535275 0.1293465742508656 0.30778360487737516
+v -0.4694791674850333 0.20282237070023434 0.510343201977993
+v -0.47138609952465477 -0.051209637452609216 0.46396575294438347
+v -0.1958254979779554 0.5379776149633797 0.37741518688390685
+v -0.4052561700411869 0.48033092463496496 0.30833747234550984
+v 0.07736612814863489 0.39956872398319027 0.3044632875369468
+v -0.10805443960978742 0.14135456599965182 0.5350487538235585
+v -0.332429806373103 0.1871109212356554 0.5674742804982271
+v -0.22027390706783798 0.31183785003009457 0.5320368419835447
+v -0.5983271518821642 0.27064581413748534 0.30164097744942653
+v -0.3001343719378102 0.27741635271707166 0.5512990206168228
+v -0.13697061389127466 0.5243491114859729 0.3761504232876936
+v -0.4785162084679789 0.4559662871477316 0.30161176733554396
+v -0.278090965385684 -0.056576164267525486 0.544307749444612
+v -0.43748924648177445 0.3663769474191438 0.4826689599798718
+v -0.34285039397280787 0.01803250809572362 0.560940129446961
+v -0.3616764704216018 -0.03912710872637529 0.5397051821704819
+v -0.03250083203819924 0.14626891221099722 0.49149476076245036
+v -0.03026881189612496 0.18032914913693113 0.48819761149496044
+v -0.21439871091315663 0.23131525700241307 0.5546667552613116
+v -0.4630075898691192 0.003096006551007048 0.5104813081884189
+v -0.5353906415825894 0.23537041834023087 0.4369796611741769
+v -0.06439854202732424 0.4450769047454775 0.40444377562858597
+v -0.49488998403943746 0.27539977474893124 0.4706909662856182
+v -0.604917764442331 0.17042572767467953 0.30840060901749594
+v -0.36588688993503443 0.03845160099792462 0.5586095285408778
+v 0.05654095695105388 0.22957548920292087 0.4117933322025361
+v -0.2625373710651128 -0.08470574898001182 0.5301851549845218
+v -0.05133695009936132 0.25216907086981727 0.48989655867317006
+v -0.2533960153290386 0.5439995699201481 0.3995304105078988
+v -0.33597220313459286 0.3631333534725652 0.5194856145182288
+v -0.12440647759829997 0.3966965997373461 0.47059101848634355
+v -0.4504997046377538 0.11749271591654088 0.5282346051618744
+v -0.548109328433384 0.14841801244497888 0.4277355955777234
+v -0.30253504949581755 0.5478963618467816 0.39544302205849313
+v 0.04370892371845821 0.1959995680277085 0.43491743557280765
+v -0.45715856136835953 0.2016432604668219 0.5196558859578044
+v -0.5374421320339828 0.43875720488287706 0.3205112500497048
+v -0.05938070083187787 0.03480131917107378 0.5063797715588573
+v -0.006191930279689445 -0.04522756646051758 0.44514300280779756
+v -0.02743083275171426 0.058443421428687595 0.4870183849835222
+v -0.578695417515536 0.03889779147623815 0.3496351204228157
+v 0.03025882203700732 -0.026586897929879054 0.41047540831067963
+v -0.5161517770376761 -0.009924833238252292 0.44979263180400864
+v -0.40852590693168206 0.10216602993226256 0.5463824198772008
+v 0.018609478101246715 0.09688614451330058 0.4566278191993324
+v -0.1579674641733427 -0.021196210805302235 0.5366077857043555
+v -0.06943533197784822 0.35306691480198105 0.46610011728020195
+v -0.33147491824894654 -0.08167002289884803 0.5211117471666511
+v 0.026623700292591264 0.04730019698126442 0.4455513823457698
+v -0.5792062571910832 0.09763213317961088 0.38236104074175237
+v -0.18171451029012284 0.3573819608566569 0.5111831913684813
+v -0.3715033518142474 -0.06302832707678513 0.5268159183933534
+v -0.06857579757258787 0.4795246837059511 0.37168991191518846
+v -0.48485963785959524 0.48917635202180754 0.35029547088992546
+v -0.6010226871215913 0.09143981332978034 0.326608389519314
+v -0.27952433584060904 0.35609861741705723 0.5261114793089792
+v 0.06813303991024666 0.14064353622345457 0.41381690002054755
+v -0.5174976529058086 0.055831262616376096 0.4615737445416433
+v -0.3376737873711702 0.044700184593877895 0.5666056353862461
+v -0.031173292892036658 -0.053672645774334365 0.4509897293034927
+v 0.05445319274734333 0.3174221742513215 0.38218360172172705
+v -0.5242927014953698 0.34953470177654505 0.4141706448639366
+v -0.5024976526699059 0.14735951121533866 0.48389348492640843
+v -0.3123519261683843 0.1681797834617138 0.5712205474470073
+v -0.4586659182656778 0.4234456664060631 0.43475536121764663
+v -0.005074102845763751 0.4539092156472814 0.3449902478507794
+v -0.1384589435667647 0.049149720989248066 0.5438963106344396
+v -0.5889696681899163 0.21816933613462283 0.3612914916634434
+v -0.5718231780305036 0.0671174597916259 0.32009051424374846
+v -0.5732266814377932 0.09945529149919852 0.39464755007926283
+v -0.13541141796087253 0.22270208943032893 0.535385892447009
+v -0.04409871919435178 0.20899396932869016 0.49516479117998813
+v -0.4447930685992698 -0.06022321311554486 0.46746193423542787
+v -0.2947599385673379 0.32234331245835773 0.5392209378192288
+v -0.5446667163487982 0.2654770745434011 0.30087399243703494
+v -0.5450742596322874 0.44730387418429984 0.30022212822968636
+v -0.3439542491125273 0.0741696653379742 0.5710926554870503
+v -0.5375900463156392 0.37068271357888577 0.3776518195334018
+v -0.22616360752445525 -0.06799036467316029 0.47304113160667294
+v -0.2803534734452251 0.5152335444522472 0.4239161319467353
+v -0.4795071232436487 0.4905214373093612 0.33025164977930094
+v -0.2956127195269621 0.44462788160953176 0.48130732179977803
+v -0.36368198253800027 0.446270511306141 0.46699300848424713
+v -0.3206377338616592 0.4132241034532992 0.5028476596155308
+v -0.5918040693433165 0.2259668918840301 0.30249846169687084
+v -0.5941368820309284 0.29276390014837045 0.3186175550442184
+v -0.4421700751848822 -0.04481982462103495 0.5069908837435124
+v -0.27926446048434556 0.22713599600155482 0.563171728989051
+v -0.5486331895415514 0.021117509231564018 0.4170183887208556
+v 0.04168451066610792 0.41901282752675095 0.3014507356020942
+v -0.2751941820049736 0.5472319538859967 0.3964535331800077
+v -0.005131079464605534 0.14728130323470776 0.4721696933469555
+v -0.39643404450623176 0.5175255050366494 0.35028663417387684
+v -0.15859800847555955 0.4539850697079341 0.44628052008044344
+v -0.5693146432176401 0.19122645886335943 0.4006484516390678
+v 0.04308470485815255 0.42142913229562773 0.330626430891228
+v -0.07374924733391784 0.47152216172420686 0.3850865115437886
+v -0.3526596820226224 -0.08523793003069545 0.5137386897684328
+v -0.5512745409708739 0.4270165562260182 0.30860866940096826
+v -0.15955880687006993 0.20407328983185966 0.5472278806061864
+v -0.5823803844048459 0.2745662046640092 0.3586060216644692
+v 0.011067745916944077 0.324920830856993 0.4294353709800779
+v -0.35695765958928416 0.14978230650827654 0.5643306537686608
+v -0.50604315515569 -0.035194880678856226 0.4558071072952242
+v -0.3192328505324374 0.5255844626969151 0.41064788535809943
+v -0.22595761456924907 -0.06794167996290817 0.540335600418681
+v 0.08018645782483326 0.3487076182917045 0.3306286425013591
+v -0.21973572548871254 0.07992613735767634 0.5729077260419292
+v -0.3648698606118231 -0.004686798126572677 0.5501861579530598
+v -0.14702226078528463 0.45049059483402626 0.445862496175291
+v -0.558270245724605 0.2212396939708901 0.4133277036084426
+v -0.2331026099071795 0.41975861583230767 0.488011481464437
+v -0.550277609731042 0.040215987106635986 0.32288954385974367
+v -0.5523297194161292 0.0030439968003600658 0.3688015198414387
+v -0.3461778744505543 -0.08305310410606605 0.5066583819115462
+v -0.13275101906415798 -0.07392038428008547 0.4968440978045505
g foo
-f 143 142 71
-f 86 71 136
-f 108 143 71
-f 16 86 136
-f 34 71 136
-f 34 142 71
-f 96 67 75
-f 74 86 64
-f 33 4 15
-f 32 40 15
-f 51 40 139
-f 144 145 127
-f 144 108 145
-f 31 116 127
-f 70 143 142
-f 70 98 142
-f 70 98 84
-f 97 84 53
-f 97 115 53
-f 97 115 130
-f 55 131 64
-f 55 86 64
-f 55 16 86
-f 29 34 142
-f 29 98 142
-f 29 84 53
-f 29 98 84
-f 119 29 34
-f 119 15 105
-f 60 51 139
-f 9 131 64
-f 78 86 71
-f 78 74 86
-f 89 108 71
-f 89 108 145
-f 18 67 75
-f 18 135 75
-f 18 135 145
-f 122 32 63
-f 122 4 15
-f 122 32 15
-f 23 110 105
-f 23 33 105
-f 23 33 4
-f 23 122 4
-f 23 122 110
-f 41 15 105
-f 41 33 105
-f 41 33 15
-f 58 32 40
-f 58 40 139
-f 125 34 131
-f 43 91 94
-f 20 122 63
-f 20 122 110
-f 107 91 130
-f 102 144 127
-f 102 31 127
-f 102 31 143
-f 102 108 143
-f 102 144 108
-f 81 116 127
-f 81 45 116
-f 138 45 116
-f 90 55 131
-f 90 55 16
-f 90 16 136
-f 90 34 136
-f 90 34 131
-f 17 119 105
-f 26 9 57
-f 26 76 131
-f 26 9 131
-f 26 60 76
-f 100 26 8
-f 100 26 57
-f 129 6 8
-f 137 6 106
-f 137 135 75
-f 137 96 75
-f 121 74 64
-f 121 46 74
-f 101 100 57
-f 101 9 57
-f 101 9 64
-f 69 46 67
-f 69 46 74
-f 69 78 67
-f 69 78 74
-f 68 78 67
-f 68 18 67
-f 68 78 71
-f 68 89 71
-f 68 89 145
-f 68 18 145
-f 120 32 63
-f 120 58 32
-f 1 125 131
-f 1 76 131
-f 1 60 76
-f 1 60 51
-f 19 125 34
-f 19 119 15
-f 19 119 34
-f 38 115 105
-f 38 17 105
-f 38 115 53
-f 38 29 53
-f 132 73 106
-f 132 7 106
-f 93 6 106
-f 93 7 106
-f 93 7 6
-f 52 43 65
-f 52 43 91
-f 123 58 139
-f 123 58 14
-f 109 120 63
-f 109 120 58
-f 128 107 130
-f 128 115 105
-f 128 115 130
-f 66 107 91
-f 66 128 105
-f 66 128 107
-f 77 81 127
-f 54 73 106
-f 44 54 106
-f 44 54 138
-f 140 97 130
-f 140 124 97
-f 140 91 130
-f 49 54 73
-f 49 138 116
-f 49 54 138
-f 13 119 29
-f 47 101 64
-f 47 101 46
-f 47 121 64
-f 47 121 46
-f 35 101 46
-f 35 46 67
-f 35 96 67
-f 35 101 100
-f 35 129 6
-f 35 100 8
-f 35 129 8
-f 35 137 96
-f 35 137 6
-f 83 20 110
-f 83 20 94
-f 83 66 105
-f 83 110 105
-f 83 91 94
-f 83 66 91
-f 21 40 15
-f 21 19 15
-f 21 19 125
-f 36 132 7
-f 39 117 103
-f 39 117 7
-f 3 117 7
-f 48 6 8
-f 28 39 103
-f 28 39 7
-f 80 26 60
-f 80 60 139
-f 80 26 8
-f 80 48 8
-f 80 48 37
-f 112 43 94
-f 112 113 43
-f 112 20 94
-f 111 109 63
-f 111 113 63
-f 111 58 14
-f 111 109 58
-f 56 77 81
-f 56 135 145
-f 56 145 127
-f 56 77 127
-f 88 137 106
-f 88 44 106
-f 10 88 137
-f 10 88 44
-f 25 138 45
-f 25 44 138
-f 25 81 45
-f 50 52 91
-f 50 140 91
-f 61 49 73
-f 104 17 119
-f 104 13 119
-f 104 13 17
-f 114 38 29
-f 114 13 29
-f 114 38 17
-f 114 13 17
-f 126 51 40
-f 95 21 125
-f 95 1 51
-f 95 1 125
-f 95 126 51
-f 95 126 21
-f 22 37 103
-f 22 48 37
-f 24 117 103
-f 24 48 117
-f 24 22 103
-f 24 22 48
-f 2 48 6
-f 2 7 6
-f 2 3 7
-f 2 3 117
-f 2 48 117
-f 59 113 43
-f 59 113 103
-f 59 28 103
-f 12 28 7
-f 12 43 65
-f 12 59 28
-f 12 59 43
-f 85 113 103
-f 85 37 103
-f 85 80 37
-f 134 113 63
-f 134 112 113
-f 134 20 63
-f 134 112 20
-f 141 111 113
-f 141 85 113
-f 141 85 80
-f 141 123 139
-f 141 80 139
-f 141 123 14
-f 141 111 14
-f 42 25 81
-f 42 56 81
-f 42 56 135
-f 42 25 44
-f 42 10 44
-f 42 137 135
-f 42 10 137
-f 118 70 84
-f 92 50 140
-f 92 140 124
-f 92 61 49
-f 82 21 40
-f 82 126 40
-f 82 126 21
-f 27 36 7
-f 27 12 7
-f 27 36 132
-f 27 12 65
-f 133 31 143
-f 133 70 143
-f 133 118 70
-f 133 31 116
-f 133 49 116
-f 79 118 84
-f 30 50 52
-f 30 92 50
-f 30 52 65
-f 30 61 65
-f 30 92 61
-f 87 92 49
-f 87 133 49
-f 11 132 73
-f 11 27 132
-f 11 61 73
-f 11 61 65
-f 11 27 65
-f 5 97 84
-f 5 79 84
-f 5 79 97
-f 99 124 97
-f 99 79 97
-f 99 79 118
-f 99 87 118
-f 72 133 118
-f 72 87 118
-f 72 87 133
-f 62 92 124
-f 62 99 124
-f 62 87 92
-f 62 99 87
-g
+f 143 142 71
+f 86 71 136
+f 108 143 71
+f 16 86 136
+f 34 71 136
+f 34 142 71
+f 96 67 75
+f 74 86 64
+f 33 4 15
+f 32 40 15
+f 51 40 139
+f 144 145 127
+f 144 108 145
+f 31 116 127
+f 70 143 142
+f 70 98 142
+f 70 98 84
+f 97 84 53
+f 97 115 53
+f 97 115 130
+f 55 131 64
+f 55 86 64
+f 55 16 86
+f 29 34 142
+f 29 98 142
+f 29 84 53
+f 29 98 84
+f 119 29 34
+f 119 15 105
+f 60 51 139
+f 9 131 64
+f 78 86 71
+f 78 74 86
+f 89 108 71
+f 89 108 145
+f 18 67 75
+f 18 135 75
+f 18 135 145
+f 122 32 63
+f 122 4 15
+f 122 32 15
+f 23 110 105
+f 23 33 105
+f 23 33 4
+f 23 122 4
+f 23 122 110
+f 41 15 105
+f 41 33 105
+f 41 33 15
+f 58 32 40
+f 58 40 139
+f 125 34 131
+f 43 91 94
+f 20 122 63
+f 20 122 110
+f 107 91 130
+f 102 144 127
+f 102 31 127
+f 102 31 143
+f 102 108 143
+f 102 144 108
+f 81 116 127
+f 81 45 116
+f 138 45 116
+f 90 55 131
+f 90 55 16
+f 90 16 136
+f 90 34 136
+f 90 34 131
+f 17 119 105
+f 26 9 57
+f 26 76 131
+f 26 9 131
+f 26 60 76
+f 100 26 8
+f 100 26 57
+f 129 6 8
+f 137 6 106
+f 137 135 75
+f 137 96 75
+f 121 74 64
+f 121 46 74
+f 101 100 57
+f 101 9 57
+f 101 9 64
+f 69 46 67
+f 69 46 74
+f 69 78 67
+f 69 78 74
+f 68 78 67
+f 68 18 67
+f 68 78 71
+f 68 89 71
+f 68 89 145
+f 68 18 145
+f 120 32 63
+f 120 58 32
+f 1 125 131
+f 1 76 131
+f 1 60 76
+f 1 60 51
+f 19 125 34
+f 19 119 15
+f 19 119 34
+f 38 115 105
+f 38 17 105
+f 38 115 53
+f 38 29 53
+f 132 73 106
+f 132 7 106
+f 93 6 106
+f 93 7 106
+f 93 7 6
+f 52 43 65
+f 52 43 91
+f 123 58 139
+f 123 58 14
+f 109 120 63
+f 109 120 58
+f 128 107 130
+f 128 115 105
+f 128 115 130
+f 66 107 91
+f 66 128 105
+f 66 128 107
+f 77 81 127
+f 54 73 106
+f 44 54 106
+f 44 54 138
+f 140 97 130
+f 140 124 97
+f 140 91 130
+f 49 54 73
+f 49 138 116
+f 49 54 138
+f 13 119 29
+f 47 101 64
+f 47 101 46
+f 47 121 64
+f 47 121 46
+f 35 101 46
+f 35 46 67
+f 35 96 67
+f 35 101 100
+f 35 129 6
+f 35 100 8
+f 35 129 8
+f 35 137 96
+f 35 137 6
+f 83 20 110
+f 83 20 94
+f 83 66 105
+f 83 110 105
+f 83 91 94
+f 83 66 91
+f 21 40 15
+f 21 19 15
+f 21 19 125
+f 36 132 7
+f 39 117 103
+f 39 117 7
+f 3 117 7
+f 48 6 8
+f 28 39 103
+f 28 39 7
+f 80 26 60
+f 80 60 139
+f 80 26 8
+f 80 48 8
+f 80 48 37
+f 112 43 94
+f 112 113 43
+f 112 20 94
+f 111 109 63
+f 111 113 63
+f 111 58 14
+f 111 109 58
+f 56 77 81
+f 56 135 145
+f 56 145 127
+f 56 77 127
+f 88 137 106
+f 88 44 106
+f 10 88 137
+f 10 88 44
+f 25 138 45
+f 25 44 138
+f 25 81 45
+f 50 52 91
+f 50 140 91
+f 61 49 73
+f 104 17 119
+f 104 13 119
+f 104 13 17
+f 114 38 29
+f 114 13 29
+f 114 38 17
+f 114 13 17
+f 126 51 40
+f 95 21 125
+f 95 1 51
+f 95 1 125
+f 95 126 51
+f 95 126 21
+f 22 37 103
+f 22 48 37
+f 24 117 103
+f 24 48 117
+f 24 22 103
+f 24 22 48
+f 2 48 6
+f 2 7 6
+f 2 3 7
+f 2 3 117
+f 2 48 117
+f 59 113 43
+f 59 113 103
+f 59 28 103
+f 12 28 7
+f 12 43 65
+f 12 59 28
+f 12 59 43
+f 85 113 103
+f 85 37 103
+f 85 80 37
+f 134 113 63
+f 134 112 113
+f 134 20 63
+f 134 112 20
+f 141 111 113
+f 141 85 113
+f 141 85 80
+f 141 123 139
+f 141 80 139
+f 141 123 14
+f 141 111 14
+f 42 25 81
+f 42 56 81
+f 42 56 135
+f 42 25 44
+f 42 10 44
+f 42 137 135
+f 42 10 137
+f 118 70 84
+f 92 50 140
+f 92 140 124
+f 92 61 49
+f 82 21 40
+f 82 126 40
+f 82 126 21
+f 27 36 7
+f 27 12 7
+f 27 36 132
+f 27 12 65
+f 133 31 143
+f 133 70 143
+f 133 118 70
+f 133 31 116
+f 133 49 116
+f 79 118 84
+f 30 50 52
+f 30 92 50
+f 30 52 65
+f 30 61 65
+f 30 92 61
+f 87 92 49
+f 87 133 49
+f 11 132 73
+f 11 27 132
+f 11 61 73
+f 11 61 65
+f 11 27 65
+f 5 97 84
+f 5 79 84
+f 5 79 97
+f 99 124 97
+f 99 79 97
+f 99 79 118
+f 99 87 118
+f 72 133 118
+f 72 87 118
+f 72 87 133
+f 62 92 124
+f 62 99 124
+f 62 87 92
+f 62 99 87
+g
diff --git a/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_RHleg_effector_frame_quasi_static.obj b/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_RHleg_effector_frame_quasi_static.obj
index 504ceb1077df637f5778e1a57745db8736edfb09..8520b3ef51013edf9a6706c50c5a5108eff6b70a 100644
--- a/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_RHleg_effector_frame_quasi_static.obj
+++ b/data/com_inequalities/feet_quasi_flat/anymal_COM_constraints_in_RHleg_effector_frame_quasi_static.obj
@@ -1,457 +1,457 @@
-v 0.5348188650181489 0.11637958397606382 0.36905079713348576
-v 0.3798580663663941 0.2559523102057514 0.5087593063448219
-v 0.22936398186691737 0.36072216249309724 0.5105552436019702
-v 0.13977587729704657 -0.055188636139963825 0.45881927259569977
-v 0.027750176666316982 -0.03389999706684213 0.4557139132596385
-v 0.005380795280119891 0.41331946147908705 0.37997854216241383
-v -0.08550718498510507 0.33346410221565054 0.3369166526821732
-v 0.312001204003314 0.16258301727025676 0.5461385779427099
-v 0.14890616940817955 0.3083165548811768 0.5230663765102314
-v 0.5142268653780668 0.03904876581506214 0.3521218567175972
-v 0.024144523521990386 0.4842709700848074 0.3292502780060821
-v -0.06532968367833293 0.07276218584860122 0.41107144726436157
-v 0.4279075829220476 -0.041572622298652606 0.44923608945669263
-v -0.07964549707792448 0.07495594454979027 0.3867845912657991
-v 0.029713682499347624 0.0945367921183054 0.49100405539239245
-v 0.33088912664915626 0.4401782183907731 0.44499711450815
-v 0.1972725043412078 0.11327255801997982 0.5575826346243336
-v 0.42126570835681854 0.2901152632219029 0.47960578127586034
-v -0.04169719149342074 0.40890468291196014 0.3403839735682743
-v 0.48319464196965967 0.00022864931506209538 0.4277968026875498
-v 0.4178095203435881 -0.037701293578690315 0.46871245586193966
-v 0.4955588585813527 0.026852765927487306 0.3541192480885312
-v 0.48563946572954486 0.36426419761238643 0.3790285094921448
-v 0.014374094460061188 0.06865577307285244 0.4782576378023352
-v 0.0052240058878829055 0.34697259404836284 0.41981858098195257
-v -0.008187095087698625 0.33998362072711974 0.4133963746737006
-v 0.020027999804426308 0.3110224119846035 0.45193904560793013
-v 0.10253527307256782 0.0502799523868957 0.5265088453532376
-v 0.3931700358950007 0.17598336966071157 0.512498746431124
-v 0.40051306325759223 -0.047829495116439254 0.453415270230963
-v 0.44506223430239394 0.4354799878997898 0.3008107235760579
-v 0.36098171938510754 -0.015251997377530635 0.5124681927293709
-v 0.17452571348482865 -0.06216702332085066 0.5235840224584425
-v 0.024208792063953287 0.20069499778216482 0.4845158794555188
-v -0.07493348978732736 0.13658387884534678 0.41030992236569036
-v 0.17355402081267604 0.46414166452551525 0.4487493550656525
-v 0.05043666534013995 0.384433973957099 0.4308849295622807
-v 0.2769141686535655 0.5183390422354125 0.40199855354850517
-v 0.10899728247765615 0.28322798480726924 0.5132691917962177
-v 0.5144941493148006 0.2776738082556785 0.372325339917312
-v 0.5141946076734203 0.021913945085416484 0.3681072873964556
-v 0.15359572298230073 0.11520419380578319 0.5501147919279143
-v 0.26322403155065976 -0.07060323439409602 0.4812250733556493
-v 0.49503711959311736 0.4296938010988176 0.3224889840383622
-v 0.3060169523823784 0.4248318608789743 0.4648940214974499
-v 0.3020696968893995 0.5019349523416852 0.31631007988066895
-v 0.49424697787395616 0.06875345609871929 0.4253663255850585
-v -0.053468101774198085 0.26258599595981724 0.4056099008253005
-v 0.5344852417046434 0.2873575300801469 0.3017971476458668
-v 0.43272798335861606 0.09150212274623534 0.49217283110639026
-v -0.07208391533451729 0.38385571910404787 0.315398626353534
-v 0.4330428837436079 0.4171804808958735 0.30060205359099706
-v 0.16724364156824653 0.5244609785167771 0.3817115547377107
-v -0.028359984810804865 0.3628942687504186 0.3827418195205361
-v 0.02223307672426829 0.47428609435719554 0.3411077124654146
-v 0.4069580067617863 0.4903602799720347 0.31811776254602986
-v 0.43465535104820097 0.469519562242569 0.30832988413243817
-v 0.10743167504023765 0.5141158882694412 0.3506128416854469
-v 0.4050720238492665 -0.009925885832363012 0.49099825827986526
-v 0.38987384956459925 0.11218057606653217 0.5185835181294818
-v 0.10232131781965915 0.5113155508247945 0.33529737767809403
-v 0.00028833690656163125 0.010051317278276007 0.4534263442283617
-v 0.10110567281679995 0.20398745666423634 0.5236076980459051
-v 0.41169820532229445 0.35542510908568825 0.4553032333068376
-v 0.3057586604636029 0.23344406169542625 0.5456328112971818
-v 0.28553918362959385 0.37765726839937974 0.49686463576301626
-v 0.30082080063629224 0.5209541888366948 0.3823652821606941
-v 0.2632995307981969 0.006346518197501681 0.5443285851304229
-v 0.28655091092293733 -0.07772409501285701 0.5025082223884667
-v 0.09154104318419358 0.4995148682376436 0.3631687097853382
-v 0.4483077932444511 0.48379458874224296 0.3202577525591506
-v 0.41264541039558567 -0.04580563181958642 0.4656761180897857
-v 0.47177190646451406 -0.008698136988356444 0.43329585412616134
-v 0.5124028902296803 0.015742887753994673 0.3897814321296273
-v 0.16017161925227394 0.07884469382934656 0.5491282414714669
-v 0.32186799418915735 0.5129567609514555 0.3870042138569203
-v 0.464866865573899 0.005527975409137189 0.4485373612247556
-v 0.04711822113562206 0.0878092678043489 0.5001186013605561
-v 0.2535840863304995 0.46203015358328403 0.3002429565154321
-v 0.2402938361052236 0.5026279557364394 0.4201544036218224
-v -0.01951699620970303 0.4542686903535187 0.3207204270274548
-v 0.4676047611958796 -0.019461943145957335 0.4170974707818099
-v 0.0033971597792566543 0.10338499416815462 0.47501661217493946
-v 0.4267576264732139 0.43082487802216257 0.38488058322270674
-v 0.3641611303909929 0.5147785435669157 0.34154024240108105
-v 0.29930780258231793 0.3395279994357379 0.5092531396805962
-v 0.16606223782021087 0.23541445375260583 0.5460892854369255
-v 0.05771629649247722 0.49510756217231683 0.3416616454583813
-v 0.4902090937374881 0.2790881651852799 0.4056771471272606
-v 0.08853225019868416 0.36354865854022783 0.4692482809927958
-v 0.17287575231111857 0.28744366097267665 0.5319250435761116
-v -0.02612511370596426 0.23127572835550864 0.43797235339393636
-v 0.1980244920629686 0.03169533552922571 0.5494720742333514
-v 0.3669346320681741 0.49788222078660704 0.38177498835374857
-v 0.32229309333658024 0.2941863677797471 0.5187316951945694
-v -0.08072553259894252 0.3619598944882148 0.3071679951963812
-v 0.4375809466533464 0.18646955761460746 0.4825603219588185
-v 0.3890845366954339 0.009321592602532666 0.5077956174843348
-v 0.1545352962411871 -0.07224560223709225 0.49594872810657664
-v 0.05904270621288995 0.11687620827138598 0.5079454022554466
-v 0.5327207578935678 0.08942567999121016 0.3563141335040033
-v 0.43944649680166387 0.46468580510487767 0.3478661041793704
-v 0.494023064677705 0.17936776376672958 0.43846447057557675
-v 0.014934596115110768 0.4786582480487114 0.30145855998206245
-v 0.4287128169521166 0.10756003533409964 0.49697865553807596
-v 0.3675089567081707 0.092400570569185 0.529283247262643
-v 0.11754335397534874 0.4376701527842047 0.4453889404597973
-v 0.5279773356456784 0.3426140141037255 0.31670033037269907
-v 0.5498763091083105 0.1652295358136102 0.33958982247258007
-v 0.06675961816267394 0.5028228608148075 0.32504876156167434
-v 0.43493036460621803 0.4260845986538472 0.3810054710036676
-v 0.060232333994566366 -0.04571428875437038 0.45220762354093536
-v 0.3411700342967931 0.4874372615861916 0.3031332938789057
-v 0.3032780646360593 0.1496051874146885 0.548499678659311
-v 0.3750275997917404 -0.05553941508762803 0.49429479078782523
-v 0.518512877590543 0.02195291293920747 0.37841685150761484
-v 0.38393895613393775 0.20089962239715814 0.5139637940166795
-v 0.3571461500559406 0.024319615763619004 0.523632179247102
-v 0.2828874409918694 -0.021362282757643878 0.4130389632567569
-v 0.24006335872125736 0.007489144466441673 0.5445899571463495
-v 0.46558263298251834 0.4402412825098601 0.3038137185557639
-v 0.18962881449184538 0.22443703961176306 0.552367216116107
-v 0.18249565989628283 0.527145402901198 0.35778541492075994
-v -0.00908158508202355 -0.011738424538601362 0.4227924128450804
-v 0.07059691871030802 0.4768002341295897 0.30095122556283366
-v 0.09669273272657707 -0.061867204900885986 0.49341624565877557
-v 0.5102164467473932 0.387499651345328 0.31183589640098
-v -0.08500531076352115 0.2962237757834943 0.3636064133271043
-v -0.08556752422674573 0.3005750546794614 0.3578133556921446
-v 0.06224876231021802 0.4182425211275315 0.4193120691338544
-v 0.3860483345157528 -0.05716439689346559 0.47631197034349027
-v 0.2452816981205887 -0.06075402507946215 0.5300367639333541
-v 0.4700986748538043 0.13454981881603256 0.466906886667373
-v 0.48039064729757947 -0.011874040507471602 0.41068810079035784
-v -0.024324003049734547 0.4044330312320143 0.36002056612505745
-v 0.2584904887309416 0.3173629492521654 0.5222564105088725
-v 0.18314638770354286 0.16226569175730257 0.5580089327646384
-v 0.16976551572287918 0.11807558421839824 0.5545376508658464
-v 0.2724321359735391 0.1924739387249029 0.5522873956116781
-v 0.5454744629393667 0.25099629502292997 0.32662760470587593
-v 0.4305166306506609 0.022764744768798938 0.47969654499461767
-v 0.20358331260717552 0.4245192062834874 0.4739303570354129
-v 0.18988997844094982 0.0639493421346288 0.5524416205864936
-v 0.45669686180277796 -0.020003364242854833 0.40753772240055225
-v 0.5110417318927014 0.22629161182972377 0.3950094040405254
-v 0.18623078981666458 0.42413931129297167 0.4742234346627514
-v 0.3647601418920242 0.0025238356096137253 0.38377745737957775
-v 0.328540202829851 0.5106167698784425 0.3875906399912127
-v -0.046408677275028426 0.31921035177842255 0.38813582027971266
-v -0.02658021970337731 0.44196843952359605 0.30615417407349493
-v 0.2603894276423213 0.11086249768660046 0.5602738977697381
-v 0.3726960504919654 0.3172236588189908 0.4924729331535301
-v 0.19027052901101962 0.19793235361747646 0.5573273651917865
+v 0.5348188650181489 0.11637958397606382 0.36905079713348576
+v 0.3798580663663941 0.2559523102057514 0.5087593063448219
+v 0.22936398186691737 0.36072216249309724 0.5105552436019702
+v 0.13977587729704657 -0.055188636139963825 0.45881927259569977
+v 0.027750176666316982 -0.03389999706684213 0.4557139132596385
+v 0.005380795280119891 0.41331946147908705 0.37997854216241383
+v -0.08550718498510507 0.33346410221565054 0.3369166526821732
+v 0.312001204003314 0.16258301727025676 0.5461385779427099
+v 0.14890616940817955 0.3083165548811768 0.5230663765102314
+v 0.5142268653780668 0.03904876581506214 0.3521218567175972
+v 0.024144523521990386 0.4842709700848074 0.3292502780060821
+v -0.06532968367833293 0.07276218584860122 0.41107144726436157
+v 0.4279075829220476 -0.041572622298652606 0.44923608945669263
+v -0.07964549707792448 0.07495594454979027 0.3867845912657991
+v 0.029713682499347624 0.0945367921183054 0.49100405539239245
+v 0.33088912664915626 0.4401782183907731 0.44499711450815
+v 0.1972725043412078 0.11327255801997982 0.5575826346243336
+v 0.42126570835681854 0.2901152632219029 0.47960578127586034
+v -0.04169719149342074 0.40890468291196014 0.3403839735682743
+v 0.48319464196965967 0.00022864931506209538 0.4277968026875498
+v 0.4178095203435881 -0.037701293578690315 0.46871245586193966
+v 0.4955588585813527 0.026852765927487306 0.3541192480885312
+v 0.48563946572954486 0.36426419761238643 0.3790285094921448
+v 0.014374094460061188 0.06865577307285244 0.4782576378023352
+v 0.0052240058878829055 0.34697259404836284 0.41981858098195257
+v -0.008187095087698625 0.33998362072711974 0.4133963746737006
+v 0.020027999804426308 0.3110224119846035 0.45193904560793013
+v 0.10253527307256782 0.0502799523868957 0.5265088453532376
+v 0.3931700358950007 0.17598336966071157 0.512498746431124
+v 0.40051306325759223 -0.047829495116439254 0.453415270230963
+v 0.44506223430239394 0.4354799878997898 0.3008107235760579
+v 0.36098171938510754 -0.015251997377530635 0.5124681927293709
+v 0.17452571348482865 -0.06216702332085066 0.5235840224584425
+v 0.024208792063953287 0.20069499778216482 0.4845158794555188
+v -0.07493348978732736 0.13658387884534678 0.41030992236569036
+v 0.17355402081267604 0.46414166452551525 0.4487493550656525
+v 0.05043666534013995 0.384433973957099 0.4308849295622807
+v 0.2769141686535655 0.5183390422354125 0.40199855354850517
+v 0.10899728247765615 0.28322798480726924 0.5132691917962177
+v 0.5144941493148006 0.2776738082556785 0.372325339917312
+v 0.5141946076734203 0.021913945085416484 0.3681072873964556
+v 0.15359572298230073 0.11520419380578319 0.5501147919279143
+v 0.26322403155065976 -0.07060323439409602 0.4812250733556493
+v 0.49503711959311736 0.4296938010988176 0.3224889840383622
+v 0.3060169523823784 0.4248318608789743 0.4648940214974499
+v 0.3020696968893995 0.5019349523416852 0.31631007988066895
+v 0.49424697787395616 0.06875345609871929 0.4253663255850585
+v -0.053468101774198085 0.26258599595981724 0.4056099008253005
+v 0.5344852417046434 0.2873575300801469 0.3017971476458668
+v 0.43272798335861606 0.09150212274623534 0.49217283110639026
+v -0.07208391533451729 0.38385571910404787 0.315398626353534
+v 0.4330428837436079 0.4171804808958735 0.30060205359099706
+v 0.16724364156824653 0.5244609785167771 0.3817115547377107
+v -0.028359984810804865 0.3628942687504186 0.3827418195205361
+v 0.02223307672426829 0.47428609435719554 0.3411077124654146
+v 0.4069580067617863 0.4903602799720347 0.31811776254602986
+v 0.43465535104820097 0.469519562242569 0.30832988413243817
+v 0.10743167504023765 0.5141158882694412 0.3506128416854469
+v 0.4050720238492665 -0.009925885832363012 0.49099825827986526
+v 0.38987384956459925 0.11218057606653217 0.5185835181294818
+v 0.10232131781965915 0.5113155508247945 0.33529737767809403
+v 0.00028833690656163125 0.010051317278276007 0.4534263442283617
+v 0.10110567281679995 0.20398745666423634 0.5236076980459051
+v 0.41169820532229445 0.35542510908568825 0.4553032333068376
+v 0.3057586604636029 0.23344406169542625 0.5456328112971818
+v 0.28553918362959385 0.37765726839937974 0.49686463576301626
+v 0.30082080063629224 0.5209541888366948 0.3823652821606941
+v 0.2632995307981969 0.006346518197501681 0.5443285851304229
+v 0.28655091092293733 -0.07772409501285701 0.5025082223884667
+v 0.09154104318419358 0.4995148682376436 0.3631687097853382
+v 0.4483077932444511 0.48379458874224296 0.3202577525591506
+v 0.41264541039558567 -0.04580563181958642 0.4656761180897857
+v 0.47177190646451406 -0.008698136988356444 0.43329585412616134
+v 0.5124028902296803 0.015742887753994673 0.3897814321296273
+v 0.16017161925227394 0.07884469382934656 0.5491282414714669
+v 0.32186799418915735 0.5129567609514555 0.3870042138569203
+v 0.464866865573899 0.005527975409137189 0.4485373612247556
+v 0.04711822113562206 0.0878092678043489 0.5001186013605561
+v 0.2535840863304995 0.46203015358328403 0.3002429565154321
+v 0.2402938361052236 0.5026279557364394 0.4201544036218224
+v -0.01951699620970303 0.4542686903535187 0.3207204270274548
+v 0.4676047611958796 -0.019461943145957335 0.4170974707818099
+v 0.0033971597792566543 0.10338499416815462 0.47501661217493946
+v 0.4267576264732139 0.43082487802216257 0.38488058322270674
+v 0.3641611303909929 0.5147785435669157 0.34154024240108105
+v 0.29930780258231793 0.3395279994357379 0.5092531396805962
+v 0.16606223782021087 0.23541445375260583 0.5460892854369255
+v 0.05771629649247722 0.49510756217231683 0.3416616454583813
+v 0.4902090937374881 0.2790881651852799 0.4056771471272606
+v 0.08853225019868416 0.36354865854022783 0.4692482809927958
+v 0.17287575231111857 0.28744366097267665 0.5319250435761116
+v -0.02612511370596426 0.23127572835550864 0.43797235339393636
+v 0.1980244920629686 0.03169533552922571 0.5494720742333514
+v 0.3669346320681741 0.49788222078660704 0.38177498835374857
+v 0.32229309333658024 0.2941863677797471 0.5187316951945694
+v -0.08072553259894252 0.3619598944882148 0.3071679951963812
+v 0.4375809466533464 0.18646955761460746 0.4825603219588185
+v 0.3890845366954339 0.009321592602532666 0.5077956174843348
+v 0.1545352962411871 -0.07224560223709225 0.49594872810657664
+v 0.05904270621288995 0.11687620827138598 0.5079454022554466
+v 0.5327207578935678 0.08942567999121016 0.3563141335040033
+v 0.43944649680166387 0.46468580510487767 0.3478661041793704
+v 0.494023064677705 0.17936776376672958 0.43846447057557675
+v 0.014934596115110768 0.4786582480487114 0.30145855998206245
+v 0.4287128169521166 0.10756003533409964 0.49697865553807596
+v 0.3675089567081707 0.092400570569185 0.529283247262643
+v 0.11754335397534874 0.4376701527842047 0.4453889404597973
+v 0.5279773356456784 0.3426140141037255 0.31670033037269907
+v 0.5498763091083105 0.1652295358136102 0.33958982247258007
+v 0.06675961816267394 0.5028228608148075 0.32504876156167434
+v 0.43493036460621803 0.4260845986538472 0.3810054710036676
+v 0.060232333994566366 -0.04571428875437038 0.45220762354093536
+v 0.3411700342967931 0.4874372615861916 0.3031332938789057
+v 0.3032780646360593 0.1496051874146885 0.548499678659311
+v 0.3750275997917404 -0.05553941508762803 0.49429479078782523
+v 0.518512877590543 0.02195291293920747 0.37841685150761484
+v 0.38393895613393775 0.20089962239715814 0.5139637940166795
+v 0.3571461500559406 0.024319615763619004 0.523632179247102
+v 0.2828874409918694 -0.021362282757643878 0.4130389632567569
+v 0.24006335872125736 0.007489144466441673 0.5445899571463495
+v 0.46558263298251834 0.4402412825098601 0.3038137185557639
+v 0.18962881449184538 0.22443703961176306 0.552367216116107
+v 0.18249565989628283 0.527145402901198 0.35778541492075994
+v -0.00908158508202355 -0.011738424538601362 0.4227924128450804
+v 0.07059691871030802 0.4768002341295897 0.30095122556283366
+v 0.09669273272657707 -0.061867204900885986 0.49341624565877557
+v 0.5102164467473932 0.387499651345328 0.31183589640098
+v -0.08500531076352115 0.2962237757834943 0.3636064133271043
+v -0.08556752422674573 0.3005750546794614 0.3578133556921446
+v 0.06224876231021802 0.4182425211275315 0.4193120691338544
+v 0.3860483345157528 -0.05716439689346559 0.47631197034349027
+v 0.2452816981205887 -0.06075402507946215 0.5300367639333541
+v 0.4700986748538043 0.13454981881603256 0.466906886667373
+v 0.48039064729757947 -0.011874040507471602 0.41068810079035784
+v -0.024324003049734547 0.4044330312320143 0.36002056612505745
+v 0.2584904887309416 0.3173629492521654 0.5222564105088725
+v 0.18314638770354286 0.16226569175730257 0.5580089327646384
+v 0.16976551572287918 0.11807558421839824 0.5545376508658464
+v 0.2724321359735391 0.1924739387249029 0.5522873956116781
+v 0.5454744629393667 0.25099629502292997 0.32662760470587593
+v 0.4305166306506609 0.022764744768798938 0.47969654499461767
+v 0.20358331260717552 0.4245192062834874 0.4739303570354129
+v 0.18988997844094982 0.0639493421346288 0.5524416205864936
+v 0.45669686180277796 -0.020003364242854833 0.40753772240055225
+v 0.5110417318927014 0.22629161182972377 0.3950094040405254
+v 0.18623078981666458 0.42413931129297167 0.4742234346627514
+v 0.3647601418920242 0.0025238356096137253 0.38377745737957775
+v 0.328540202829851 0.5106167698784425 0.3875906399912127
+v -0.046408677275028426 0.31921035177842255 0.38813582027971266
+v -0.02658021970337731 0.44196843952359605 0.30615417407349493
+v 0.2603894276423213 0.11086249768660046 0.5602738977697381
+v 0.3726960504919654 0.3172236588189908 0.4924729331535301
+v 0.19027052901101962 0.19793235361747646 0.5573273651917865
g foo
-f 124 22 14
-f 7 14 129
-f 46 113 104
-f 71 44 121
-f 110 46 104
-f 53 70 107
-f 8 65 151
-f 99 43 69
-f 10 49 109
-f 10 49 22
-f 30 43 69
-f 30 144 43
-f 4 144 43
-f 4 99 43
-f 33 99 126
-f 33 28 126
-f 33 28 75
-f 33 132 69
-f 33 99 69
-f 93 33 132
-f 93 33 75
-f 127 44 121
-f 127 49 121
-f 96 49 22
-f 96 22 14
-f 96 49 104
-f 96 150 104
-f 96 7 14
-f 96 7 51
-f 96 150 51
-f 57 113 121
-f 57 71 121
-f 57 71 113
-f 85 46 113
-f 85 46 123
-f 81 150 51
-f 81 150 104
-f 61 46 123
-f 61 110 46
-f 9 39 87
-f 63 39 87
-f 90 27 39
-f 90 9 107
-f 90 9 39
-f 139 65 151
-f 139 153 151
-f 139 153 65
-f 137 153 151
-f 137 153 87
-f 137 63 87
-f 122 3 65
-f 122 153 65
-f 122 153 87
-f 122 9 87
-f 102 71 44
-f 133 47 20
-f 133 47 103
-f 145 40 103
-f 23 64 44
-f 101 116 109
-f 101 10 109
-f 101 10 116
-f 41 10 22
-f 41 10 116
-f 41 144 22
-f 1 116 109
-f 1 47 103
-f 1 103 109
-f 119 4 144
-f 112 99 126
-f 112 4 99
-f 112 119 124
-f 112 119 4
-f 134 41 116
-f 134 82 144
-f 134 41 144
-f 15 83 100
-f 120 93 132
-f 12 35 83
-f 12 35 14
-f 12 124 14
-f 12 62 124
-f 5 62 126
-f 5 62 124
-f 5 112 126
-f 5 112 124
-f 108 127 44
-f 108 23 44
-f 108 23 40
-f 108 127 49
-f 56 71 113
-f 56 85 113
-f 56 85 71
-f 67 53 123
-f 67 85 123
-f 11 110 104
-f 11 81 104
-f 58 61 110
-f 58 11 110
-f 58 53 123
-f 58 61 123
-f 58 53 70
-f 146 9 107
-f 146 9 3
-f 36 53 107
-f 36 80 53
-f 36 146 107
-f 34 27 39
-f 34 63 39
-f 34 63 100
-f 34 83 100
-f 34 35 83
-f 19 7 51
-f 19 6 81
-f 19 81 51
-f 91 9 3
-f 91 122 3
-f 91 122 9
-f 111 64 44
-f 111 102 44
-f 111 102 64
-f 94 102 71
-f 94 85 71
-f 136 3 65
-f 86 152 66
-f 86 136 65
-f 86 66 3
-f 86 136 3
-f 2 152 65
-f 131 30 69
-f 77 133 20
-f 106 118 151
-f 106 8 65
-f 89 40 103
-f 89 23 40
-f 74 1 116
-f 74 1 47
-f 74 47 20
-f 74 134 20
-f 74 134 116
-f 92 48 27
-f 92 34 27
-f 92 48 35
-f 92 34 35
-f 128 26 27
-f 128 48 27
-f 128 19 7
-f 128 48 35
-f 128 7 129
-f 128 14 129
-f 128 35 14
-f 147 144 22
-f 147 119 144
-f 147 124 22
-f 147 119 124
-f 78 28 100
-f 78 15 100
-f 24 12 83
-f 24 12 62
-f 24 15 83
-f 24 28 126
-f 24 62 126
-f 24 78 28
-f 24 78 15
-f 140 108 40
-f 140 145 40
-f 140 49 109
-f 140 108 49
-f 140 103 109
-f 140 145 103
-f 125 49 104
-f 125 79 49
-f 125 113 104
-f 125 79 113
-f 52 79 49
-f 55 6 81
-f 55 11 81
-f 55 6 107
-f 55 70 107
-f 55 11 70
-f 130 6 107
-f 130 90 107
-f 25 26 27
-f 25 6 26
-f 25 130 6
-f 88 11 70
-f 88 58 70
-f 88 58 11
-f 142 66 3
-f 142 146 3
-f 142 36 80
-f 142 36 146
-f 54 6 26
-f 54 128 19
-f 17 137 151
-f 17 138 137
-f 143 93 75
-f 143 138 75
-f 143 17 138
-f 143 93 151
-f 143 17 151
-f 42 28 75
-f 42 138 75
-f 42 28 100
-f 42 63 100
-f 42 137 63
-f 42 138 137
-f 84 102 64
-f 84 94 64
-f 84 94 102
-f 38 80 53
-f 38 67 53
-f 16 94 64
-f 95 152 65
-f 95 86 65
-f 95 86 152
-f 18 23 64
-f 18 89 103
-f 18 89 23
-f 18 133 103
-f 18 152 64
-f 18 2 152
-f 18 2 29
-f 117 65 29
-f 117 2 29
-f 117 2 65
-f 13 131 30
-f 13 30 144
-f 13 82 144
-f 13 134 82
-f 115 131 69
-f 115 132 69
-f 114 8 151
-f 114 106 151
-f 114 106 8
-f 60 65 29
-f 60 106 65
-f 68 120 132
-f 68 93 151
-f 68 120 93
-f 68 118 151
-f 31 52 79
-f 31 79 113
-f 31 113 121
-f 31 49 121
-f 31 52 49
-f 37 25 27
-f 37 25 130
-f 37 90 27
-f 37 130 90
-f 135 19 6
-f 135 54 6
-f 135 54 19
-f 149 128 26
-f 149 54 26
-f 149 54 128
-f 148 94 85
-f 148 16 94
-f 148 16 38
-f 45 152 64
-f 45 16 64
-f 45 152 66
-f 45 142 80
-f 45 142 66
-f 45 38 80
-f 45 16 38
-f 97 18 29
-f 97 18 133
-f 73 134 20
-f 73 13 134
-f 32 115 132
-f 32 68 132
-f 32 68 118
-f 72 13 131
-f 72 115 131
-f 72 73 13
-f 105 60 29
-f 105 97 29
-f 105 97 133
-f 105 60 106
-f 76 38 67
-f 76 148 38
-f 76 67 85
-f 76 148 85
-f 21 72 73
-f 21 77 20
-f 21 73 20
-f 21 72 115
-f 21 77 59
-f 21 115 59
-f 141 77 59
-f 98 105 106
-f 98 106 118
-f 98 141 59
-f 98 32 118
-f 98 115 59
-f 98 32 115
-f 50 98 105
-f 50 98 141
-f 50 141 77
-f 50 77 133
-f 50 105 133
-g
+f 124 22 14
+f 7 14 129
+f 46 113 104
+f 71 44 121
+f 110 46 104
+f 53 70 107
+f 8 65 151
+f 99 43 69
+f 10 49 109
+f 10 49 22
+f 30 43 69
+f 30 144 43
+f 4 144 43
+f 4 99 43
+f 33 99 126
+f 33 28 126
+f 33 28 75
+f 33 132 69
+f 33 99 69
+f 93 33 132
+f 93 33 75
+f 127 44 121
+f 127 49 121
+f 96 49 22
+f 96 22 14
+f 96 49 104
+f 96 150 104
+f 96 7 14
+f 96 7 51
+f 96 150 51
+f 57 113 121
+f 57 71 121
+f 57 71 113
+f 85 46 113
+f 85 46 123
+f 81 150 51
+f 81 150 104
+f 61 46 123
+f 61 110 46
+f 9 39 87
+f 63 39 87
+f 90 27 39
+f 90 9 107
+f 90 9 39
+f 139 65 151
+f 139 153 151
+f 139 153 65
+f 137 153 151
+f 137 153 87
+f 137 63 87
+f 122 3 65
+f 122 153 65
+f 122 153 87
+f 122 9 87
+f 102 71 44
+f 133 47 20
+f 133 47 103
+f 145 40 103
+f 23 64 44
+f 101 116 109
+f 101 10 109
+f 101 10 116
+f 41 10 22
+f 41 10 116
+f 41 144 22
+f 1 116 109
+f 1 47 103
+f 1 103 109
+f 119 4 144
+f 112 99 126
+f 112 4 99
+f 112 119 124
+f 112 119 4
+f 134 41 116
+f 134 82 144
+f 134 41 144
+f 15 83 100
+f 120 93 132
+f 12 35 83
+f 12 35 14
+f 12 124 14
+f 12 62 124
+f 5 62 126
+f 5 62 124
+f 5 112 126
+f 5 112 124
+f 108 127 44
+f 108 23 44
+f 108 23 40
+f 108 127 49
+f 56 71 113
+f 56 85 113
+f 56 85 71
+f 67 53 123
+f 67 85 123
+f 11 110 104
+f 11 81 104
+f 58 61 110
+f 58 11 110
+f 58 53 123
+f 58 61 123
+f 58 53 70
+f 146 9 107
+f 146 9 3
+f 36 53 107
+f 36 80 53
+f 36 146 107
+f 34 27 39
+f 34 63 39
+f 34 63 100
+f 34 83 100
+f 34 35 83
+f 19 7 51
+f 19 6 81
+f 19 81 51
+f 91 9 3
+f 91 122 3
+f 91 122 9
+f 111 64 44
+f 111 102 44
+f 111 102 64
+f 94 102 71
+f 94 85 71
+f 136 3 65
+f 86 152 66
+f 86 136 65
+f 86 66 3
+f 86 136 3
+f 2 152 65
+f 131 30 69
+f 77 133 20
+f 106 118 151
+f 106 8 65
+f 89 40 103
+f 89 23 40
+f 74 1 116
+f 74 1 47
+f 74 47 20
+f 74 134 20
+f 74 134 116
+f 92 48 27
+f 92 34 27
+f 92 48 35
+f 92 34 35
+f 128 26 27
+f 128 48 27
+f 128 19 7
+f 128 48 35
+f 128 7 129
+f 128 14 129
+f 128 35 14
+f 147 144 22
+f 147 119 144
+f 147 124 22
+f 147 119 124
+f 78 28 100
+f 78 15 100
+f 24 12 83
+f 24 12 62
+f 24 15 83
+f 24 28 126
+f 24 62 126
+f 24 78 28
+f 24 78 15
+f 140 108 40
+f 140 145 40
+f 140 49 109
+f 140 108 49
+f 140 103 109
+f 140 145 103
+f 125 49 104
+f 125 79 49
+f 125 113 104
+f 125 79 113
+f 52 79 49
+f 55 6 81
+f 55 11 81
+f 55 6 107
+f 55 70 107
+f 55 11 70
+f 130 6 107
+f 130 90 107
+f 25 26 27
+f 25 6 26
+f 25 130 6
+f 88 11 70
+f 88 58 70
+f 88 58 11
+f 142 66 3
+f 142 146 3
+f 142 36 80
+f 142 36 146
+f 54 6 26
+f 54 128 19
+f 17 137 151
+f 17 138 137
+f 143 93 75
+f 143 138 75
+f 143 17 138
+f 143 93 151
+f 143 17 151
+f 42 28 75
+f 42 138 75
+f 42 28 100
+f 42 63 100
+f 42 137 63
+f 42 138 137
+f 84 102 64
+f 84 94 64
+f 84 94 102
+f 38 80 53
+f 38 67 53
+f 16 94 64
+f 95 152 65
+f 95 86 65
+f 95 86 152
+f 18 23 64
+f 18 89 103
+f 18 89 23
+f 18 133 103
+f 18 152 64
+f 18 2 152
+f 18 2 29
+f 117 65 29
+f 117 2 29
+f 117 2 65
+f 13 131 30
+f 13 30 144
+f 13 82 144
+f 13 134 82
+f 115 131 69
+f 115 132 69
+f 114 8 151
+f 114 106 151
+f 114 106 8
+f 60 65 29
+f 60 106 65
+f 68 120 132
+f 68 93 151
+f 68 120 93
+f 68 118 151
+f 31 52 79
+f 31 79 113
+f 31 113 121
+f 31 49 121
+f 31 52 49
+f 37 25 27
+f 37 25 130
+f 37 90 27
+f 37 130 90
+f 135 19 6
+f 135 54 6
+f 135 54 19
+f 149 128 26
+f 149 54 26
+f 149 54 128
+f 148 94 85
+f 148 16 94
+f 148 16 38
+f 45 152 64
+f 45 16 64
+f 45 152 66
+f 45 142 80
+f 45 142 66
+f 45 38 80
+f 45 16 38
+f 97 18 29
+f 97 18 133
+f 73 134 20
+f 73 13 134
+f 32 115 132
+f 32 68 132
+f 32 68 118
+f 72 13 131
+f 72 115 131
+f 72 73 13
+f 105 60 29
+f 105 97 29
+f 105 97 133
+f 105 60 106
+f 76 38 67
+f 76 148 38
+f 76 67 85
+f 76 148 85
+f 21 72 73
+f 21 77 20
+f 21 73 20
+f 21 72 115
+f 21 77 59
+f 21 115 59
+f 141 77 59
+f 98 105 106
+f 98 106 118
+f 98 141 59
+f 98 32 118
+f 98 115 59
+f 98 32 115
+f 50 98 105
+f 50 98 141
+f 50 141 77
+f 50 77 133
+f 50 105 133
+g
diff --git a/data/com_inequalities/visual/LF_HAA_com_constraints.dae b/data/com_inequalities/visual/LF_HAA_com_constraints.dae
index ee47e729acb4f8e1c118401eac12a776d8d3b816..72abae1ecb75e188d96fef2f373514de159aaded 100644
--- a/data/com_inequalities/visual/LF_HAA_com_constraints.dae
+++ b/data/com_inequalities/visual/LF_HAA_com_constraints.dae
@@ -100,4 +100,4 @@
<scene>
<instance_visual_scene url="#Scene"/>
</scene>
-</COLLADA>
\ No newline at end of file
+</COLLADA>
diff --git a/data/com_inequalities/visual/LH_HAA_com_constraints.dae b/data/com_inequalities/visual/LH_HAA_com_constraints.dae
index 838cca31827b7bc071fa610aaeb093844a471764..4e90dbdb8724ca385f5a2c49d57655bedb5149fb 100644
--- a/data/com_inequalities/visual/LH_HAA_com_constraints.dae
+++ b/data/com_inequalities/visual/LH_HAA_com_constraints.dae
@@ -100,4 +100,4 @@
<scene>
<instance_visual_scene url="#Scene"/>
</scene>
-</COLLADA>
\ No newline at end of file
+</COLLADA>
diff --git a/data/com_inequalities/visual/RF_HAA_com_constraints.dae b/data/com_inequalities/visual/RF_HAA_com_constraints.dae
index 87da34a67c34d15124b00d764d08127e219d6a49..96a41486c1530883d2a2896a848766d630780dfd 100644
--- a/data/com_inequalities/visual/RF_HAA_com_constraints.dae
+++ b/data/com_inequalities/visual/RF_HAA_com_constraints.dae
@@ -100,4 +100,4 @@
<scene>
<instance_visual_scene url="#Scene"/>
</scene>
-</COLLADA>
\ No newline at end of file
+</COLLADA>
diff --git a/data/com_inequalities/visual/RH_HAA_com_constraints.dae b/data/com_inequalities/visual/RH_HAA_com_constraints.dae
index 045836c46a9092de2f872cf5a7ba57ee64e309d3..f19919ad60fc97b8d01693c4c59f4c4d9c10cdf0 100644
--- a/data/com_inequalities/visual/RH_HAA_com_constraints.dae
+++ b/data/com_inequalities/visual/RH_HAA_com_constraints.dae
@@ -100,4 +100,4 @@
<scene>
<instance_visual_scene url="#Scene"/>
</scene>
-</COLLADA>
\ No newline at end of file
+</COLLADA>
diff --git a/data/meshes/anymal_all_large_visual.dae b/data/meshes/anymal_all_large_visual.dae
index e3055fc44c0a6634d989edbbe4904665f24256d9..eed9304dcbef103375e7d00fb220a18c224d4dff 100644
--- a/data/meshes/anymal_all_large_visual.dae
+++ b/data/meshes/anymal_all_large_visual.dae
@@ -301,4 +301,4 @@
<scene>
<instance_visual_scene url="#Scene"/>
</scene>
-</COLLADA>
\ No newline at end of file
+</COLLADA>
diff --git a/data/meshes/anymal_all_visual.dae b/data/meshes/anymal_all_visual.dae
index 6d74f962ee94035adbbb08bedd68d00308ebf591..2aa3fccab7975b86a5917208dbd17bc970d3d703 100644
--- a/data/meshes/anymal_all_visual.dae
+++ b/data/meshes/anymal_all_visual.dae
@@ -296,4 +296,4 @@
<scene>
<instance_visual_scene url="#Scene"/>
</scene>
-</COLLADA>
\ No newline at end of file
+</COLLADA>
diff --git a/data/relative_effector_positions/anymal_LF_ADAPTER_TO_FOOT_constraints_in_LHleg.obj b/data/relative_effector_positions/anymal_LF_ADAPTER_TO_FOOT_constraints_in_LHleg.obj
index 8780fa64d8b9cc3e95d73ee0cc5d0c2fc1666f43..3ebd1f59a0d940d0c2511b58ad46be6c41cc608d 100644
--- a/data/relative_effector_positions/anymal_LF_ADAPTER_TO_FOOT_constraints_in_LHleg.obj
+++ b/data/relative_effector_positions/anymal_LF_ADAPTER_TO_FOOT_constraints_in_LHleg.obj
@@ -1,196 +1,196 @@
-v 1.1016156456174515 -0.18716037026434723 -0.10491811898192216
-v 0.9483476051382198 -0.40551556487632273 0.028744151817720387
-v -0.10301695114753623 0.05946302152310194 -0.04433007650084542
-v 0.9675813320779578 -0.11009740482113023 -0.22442920404146566
-v 0.13082318632316392 -0.2142191687052919 0.08283293104784528
-v 0.1454796437740883 -0.36449828074146284 -0.2683236356176397
-v 0.8770808101830576 0.3611980037955306 -0.0541445064828015
-v 1.0008899762066488 -0.34702340226975986 -0.21884967817469614
-v 0.8181316875827684 -0.3595975638346992 0.11293535556139267
-v 1.0513914602045085 -0.04989232089426465 0.1675542411380365
-v 0.16574275234368932 0.43358712296135354 0.03596616269131475
-v 0.9985469168184148 -0.3282806987615391 -0.22021321451325399
-v 0.13250472598354443 0.38348959572213154 0.20289932512977166
-v 0.07777466659525026 -0.44502695409735976 -0.024122196316354705
-v 0.6712402787811517 -0.47799052828723915 0.035631518289514996
-v 0.3425088246411209 -0.5365210826932507 -0.04506345297335429
-v 0.12622369534448383 0.21413048926666667 0.23249963782275723
-v 0.23649580132743966 0.5444888945408589 0.1675813063167798
-v 0.6507224158162519 0.5345221244132509 0.22458977848284467
-v 0.6832270874934577 -0.18690941614891868 0.17797578146405935
-v 0.7637048493529556 -0.4932664934188713 -0.2625542806659487
-v 0.905586306729775 -0.5100482073014392 -0.07644761077276124
-v 0.8232100744328823 0.31567835695077723 -0.14516008072531628
-v 0.35781150920993976 -0.5791349275530329 -0.13475127469522385
-v -0.08362108480241309 -0.2985316830254039 -0.18151995288722567
-v 0.7100381937433868 -0.3884462131146426 0.09225319579849167
-v 0.17678027427351561 0.2979362439742722 -0.1019045943255204
-v 0.6183089216401132 0.5246264838277366 0.009282714854610408
-v -0.09097991048453513 0.1357449324656998 0.033677509794578675
-v 0.7813554966875866 -0.465171249447521 -0.26985175916242254
-v 0.36990208209458547 0.5692732712741655 0.13195398397068892
-v 0.8877273161038031 -0.26154543276455366 0.17448196911055291
-v 0.12408880351915416 0.32834544073781613 0.24788986580028383
-v 0.5062148923539761 -0.6039636536170416 -0.1980051538884065
-v 0.46895668735517476 0.5784691536561541 0.050189199634081194
-v 0.519118126105557 -0.3951591579735598 0.0678965261265651
-v 1.0842341947131373 0.20046956959658446 -0.015784419015480167
-v 0.305789476043127 0.5184060930787473 0.21994329441834315
-v 0.5062938539534614 -0.6077308233628076 -0.13110619459121903
-v 1.1228215086701279 0.3166888565492295 0.10749424890237158
-v 1.0801938534524556 -0.08953799841332316 -0.11905822683949335
-v 0.7460736315401675 0.45881673386070143 0.29533785775624843
-v 0.8628409720544576 -0.15868143629418943 -0.28594247501612524
-v 0.581235615219669 0.5684783560454431 0.07708417552219515
-v 1.059064779073533 0.22550316818584323 -0.04014376953473464
-v 0.6100348971314509 0.5710252960272635 0.18386986327042043
-v 0.29599674738931947 -0.38402812929973457 -0.2791030777621292
-v 0.13108649972968078 0.4099074326800608 0.06600538587529958
-v 1.1145765508001686 0.029267545085410573 0.04270911910775799
-v 0.762039071746003 0.44843388537342965 -0.025174143372533486
-v 0.6765319311638041 -0.5499259920982886 -0.059719234301817525
-v 0.6594241772010206 0.3210964660612202 0.2929017696401899
-v 0.0442546482201097 -0.4044571292548988 -0.01686928001851301
-v 0.7346045584637135 -0.5573269083875003 -0.08570262578271098
-v 0.784178910379753 0.20120735990879318 0.2881962351849568
-v 1.023699283659865 0.4226477134644746 0.276698806855761
-v 0.9057835926662687 -0.3610935314665127 0.10653999021285826
-v 0.8126525983890377 0.5207363214620788 0.1467996848800035
-v 0.8099678208284703 0.01297467010194625 0.26748323577493116
-v 0.6645742289611063 0.26722123284487337 0.28664870300177353
-v 0.17526625980224628 -0.03720660182128155 -0.20430844393851927
-v 0.7738827709013244 -0.4426493527070797 0.0591490050276669
-v 1.049690078778225 -0.37397053437655525 -0.027815522515540092
-v 0.7324987491648285 -0.2207421292943082 -0.32426009409954654
-v -0.07870655511397151 0.14127144871409958 0.014476751128909393
-v -0.06758838836482653 -0.2688871132209691 -0.1071370637564345
+v 1.1016156456174515 -0.18716037026434723 -0.10491811898192216
+v 0.9483476051382198 -0.40551556487632273 0.028744151817720387
+v -0.10301695114753623 0.05946302152310194 -0.04433007650084542
+v 0.9675813320779578 -0.11009740482113023 -0.22442920404146566
+v 0.13082318632316392 -0.2142191687052919 0.08283293104784528
+v 0.1454796437740883 -0.36449828074146284 -0.2683236356176397
+v 0.8770808101830576 0.3611980037955306 -0.0541445064828015
+v 1.0008899762066488 -0.34702340226975986 -0.21884967817469614
+v 0.8181316875827684 -0.3595975638346992 0.11293535556139267
+v 1.0513914602045085 -0.04989232089426465 0.1675542411380365
+v 0.16574275234368932 0.43358712296135354 0.03596616269131475
+v 0.9985469168184148 -0.3282806987615391 -0.22021321451325399
+v 0.13250472598354443 0.38348959572213154 0.20289932512977166
+v 0.07777466659525026 -0.44502695409735976 -0.024122196316354705
+v 0.6712402787811517 -0.47799052828723915 0.035631518289514996
+v 0.3425088246411209 -0.5365210826932507 -0.04506345297335429
+v 0.12622369534448383 0.21413048926666667 0.23249963782275723
+v 0.23649580132743966 0.5444888945408589 0.1675813063167798
+v 0.6507224158162519 0.5345221244132509 0.22458977848284467
+v 0.6832270874934577 -0.18690941614891868 0.17797578146405935
+v 0.7637048493529556 -0.4932664934188713 -0.2625542806659487
+v 0.905586306729775 -0.5100482073014392 -0.07644761077276124
+v 0.8232100744328823 0.31567835695077723 -0.14516008072531628
+v 0.35781150920993976 -0.5791349275530329 -0.13475127469522385
+v -0.08362108480241309 -0.2985316830254039 -0.18151995288722567
+v 0.7100381937433868 -0.3884462131146426 0.09225319579849167
+v 0.17678027427351561 0.2979362439742722 -0.1019045943255204
+v 0.6183089216401132 0.5246264838277366 0.009282714854610408
+v -0.09097991048453513 0.1357449324656998 0.033677509794578675
+v 0.7813554966875866 -0.465171249447521 -0.26985175916242254
+v 0.36990208209458547 0.5692732712741655 0.13195398397068892
+v 0.8877273161038031 -0.26154543276455366 0.17448196911055291
+v 0.12408880351915416 0.32834544073781613 0.24788986580028383
+v 0.5062148923539761 -0.6039636536170416 -0.1980051538884065
+v 0.46895668735517476 0.5784691536561541 0.050189199634081194
+v 0.519118126105557 -0.3951591579735598 0.0678965261265651
+v 1.0842341947131373 0.20046956959658446 -0.015784419015480167
+v 0.305789476043127 0.5184060930787473 0.21994329441834315
+v 0.5062938539534614 -0.6077308233628076 -0.13110619459121903
+v 1.1228215086701279 0.3166888565492295 0.10749424890237158
+v 1.0801938534524556 -0.08953799841332316 -0.11905822683949335
+v 0.7460736315401675 0.45881673386070143 0.29533785775624843
+v 0.8628409720544576 -0.15868143629418943 -0.28594247501612524
+v 0.581235615219669 0.5684783560454431 0.07708417552219515
+v 1.059064779073533 0.22550316818584323 -0.04014376953473464
+v 0.6100348971314509 0.5710252960272635 0.18386986327042043
+v 0.29599674738931947 -0.38402812929973457 -0.2791030777621292
+v 0.13108649972968078 0.4099074326800608 0.06600538587529958
+v 1.1145765508001686 0.029267545085410573 0.04270911910775799
+v 0.762039071746003 0.44843388537342965 -0.025174143372533486
+v 0.6765319311638041 -0.5499259920982886 -0.059719234301817525
+v 0.6594241772010206 0.3210964660612202 0.2929017696401899
+v 0.0442546482201097 -0.4044571292548988 -0.01686928001851301
+v 0.7346045584637135 -0.5573269083875003 -0.08570262578271098
+v 0.784178910379753 0.20120735990879318 0.2881962351849568
+v 1.023699283659865 0.4226477134644746 0.276698806855761
+v 0.9057835926662687 -0.3610935314665127 0.10653999021285826
+v 0.8126525983890377 0.5207363214620788 0.1467996848800035
+v 0.8099678208284703 0.01297467010194625 0.26748323577493116
+v 0.6645742289611063 0.26722123284487337 0.28664870300177353
+v 0.17526625980224628 -0.03720660182128155 -0.20430844393851927
+v 0.7738827709013244 -0.4426493527070797 0.0591490050276669
+v 1.049690078778225 -0.37397053437655525 -0.027815522515540092
+v 0.7324987491648285 -0.2207421292943082 -0.32426009409954654
+v -0.07870655511397151 0.14127144871409958 0.014476751128909393
+v -0.06758838836482653 -0.2688871132209691 -0.1071370637564345
g foo
-f 47 6 64
-f 49 10 40
-f 43 23 64
-f 43 8 64
-f 27 23 64
-f 27 23 35
-f 58 50 40
-f 34 47 21
-f 34 47 6
-f 30 47 64
-f 30 47 21
-f 30 8 64
-f 30 8 21
-f 1 49 40
-f 16 15 39
-f 36 16 15
-f 51 15 39
-f 7 50 40
-f 7 50 23
-f 7 45 40
-f 7 45 23
-f 28 23 35
-f 28 50 23
-f 28 58 50
-f 37 45 40
-f 37 45 41
-f 37 1 40
-f 37 1 41
-f 4 45 41
-f 4 45 23
-f 4 43 23
-f 61 6 64
-f 61 27 64
-f 31 18 35
-f 11 27 35
-f 11 18 35
-f 11 27 3
-f 29 17 53
-f 29 66 3
-f 29 66 53
-f 24 34 6
-f 24 34 39
-f 24 16 39
-f 63 1 8
-f 63 57 10
-f 63 49 10
-f 63 1 49
-f 5 17 53
-f 54 51 39
-f 54 34 39
-f 20 5 36
-f 20 17 59
-f 20 5 17
-f 32 20 36
-f 32 20 59
-f 32 10 59
-f 32 57 10
-f 44 28 35
-f 44 28 58
-f 12 43 8
-f 12 4 43
-f 12 4 41
-f 12 1 41
-f 12 1 8
-f 60 52 59
-f 33 52 42
-f 33 38 42
-f 33 60 52
-f 33 38 18
-f 33 17 59
-f 33 60 59
-f 33 29 17
-f 56 10 59
-f 56 10 40
-f 56 58 40
-f 46 31 18
-f 46 38 18
-f 46 56 58
-f 46 44 58
-f 46 31 35
-f 46 44 35
-f 65 11 3
-f 65 29 3
-f 48 11 18
-f 48 29 18
-f 48 65 11
-f 48 65 29
-f 25 24 6
-f 25 66 53
-f 25 66 3
-f 25 27 3
-f 25 61 6
-f 25 61 27
-f 14 5 53
-f 14 5 36
-f 14 36 16
-f 14 24 16
-f 14 25 53
-f 14 25 24
-f 22 54 34
-f 22 63 8
-f 22 34 21
-f 22 8 21
-f 26 36 15
-f 26 32 36
-f 9 32 57
-f 9 26 15
-f 9 26 32
-f 13 29 18
-f 13 33 18
-f 13 33 29
-f 55 56 59
-f 55 56 42
-f 55 52 59
-f 55 52 42
-f 19 38 42
-f 19 46 38
-f 19 56 42
-f 19 46 56
-f 2 63 57
-f 2 22 57
-f 2 22 63
-f 62 22 54
-f 62 51 15
-f 62 54 51
-f 62 22 57
-f 62 9 15
-f 62 9 57
-g
+f 47 6 64
+f 49 10 40
+f 43 23 64
+f 43 8 64
+f 27 23 64
+f 27 23 35
+f 58 50 40
+f 34 47 21
+f 34 47 6
+f 30 47 64
+f 30 47 21
+f 30 8 64
+f 30 8 21
+f 1 49 40
+f 16 15 39
+f 36 16 15
+f 51 15 39
+f 7 50 40
+f 7 50 23
+f 7 45 40
+f 7 45 23
+f 28 23 35
+f 28 50 23
+f 28 58 50
+f 37 45 40
+f 37 45 41
+f 37 1 40
+f 37 1 41
+f 4 45 41
+f 4 45 23
+f 4 43 23
+f 61 6 64
+f 61 27 64
+f 31 18 35
+f 11 27 35
+f 11 18 35
+f 11 27 3
+f 29 17 53
+f 29 66 3
+f 29 66 53
+f 24 34 6
+f 24 34 39
+f 24 16 39
+f 63 1 8
+f 63 57 10
+f 63 49 10
+f 63 1 49
+f 5 17 53
+f 54 51 39
+f 54 34 39
+f 20 5 36
+f 20 17 59
+f 20 5 17
+f 32 20 36
+f 32 20 59
+f 32 10 59
+f 32 57 10
+f 44 28 35
+f 44 28 58
+f 12 43 8
+f 12 4 43
+f 12 4 41
+f 12 1 41
+f 12 1 8
+f 60 52 59
+f 33 52 42
+f 33 38 42
+f 33 60 52
+f 33 38 18
+f 33 17 59
+f 33 60 59
+f 33 29 17
+f 56 10 59
+f 56 10 40
+f 56 58 40
+f 46 31 18
+f 46 38 18
+f 46 56 58
+f 46 44 58
+f 46 31 35
+f 46 44 35
+f 65 11 3
+f 65 29 3
+f 48 11 18
+f 48 29 18
+f 48 65 11
+f 48 65 29
+f 25 24 6
+f 25 66 53
+f 25 66 3
+f 25 27 3
+f 25 61 6
+f 25 61 27
+f 14 5 53
+f 14 5 36
+f 14 36 16
+f 14 24 16
+f 14 25 53
+f 14 25 24
+f 22 54 34
+f 22 63 8
+f 22 34 21
+f 22 8 21
+f 26 36 15
+f 26 32 36
+f 9 32 57
+f 9 26 15
+f 9 26 32
+f 13 29 18
+f 13 33 18
+f 13 33 29
+f 55 56 59
+f 55 56 42
+f 55 52 59
+f 55 52 42
+f 19 38 42
+f 19 46 38
+f 19 56 42
+f 19 46 56
+f 2 63 57
+f 2 22 57
+f 2 22 63
+f 62 22 54
+f 62 51 15
+f 62 54 51
+f 62 22 57
+f 62 9 15
+f 62 9 57
+g
diff --git a/data/relative_effector_positions/anymal_LF_ADAPTER_TO_FOOT_constraints_in_RFleg.obj b/data/relative_effector_positions/anymal_LF_ADAPTER_TO_FOOT_constraints_in_RFleg.obj
index a8dbd49f5205d54d20e82eb25c1beea93afbf812..a67951fede707492ef54346591e8af7985a54356 100644
--- a/data/relative_effector_positions/anymal_LF_ADAPTER_TO_FOOT_constraints_in_RFleg.obj
+++ b/data/relative_effector_positions/anymal_LF_ADAPTER_TO_FOOT_constraints_in_RFleg.obj
@@ -1,223 +1,223 @@
-v -0.25371647695714983 0.6799748787759159 -0.3428533269427781
-v 0.29020573392093846 0.4545565255836158 -0.24521987513475973
-v -0.4423121034011809 0.23719421623788245 -0.23489987589073458
-v -0.4215051155125492 0.9306048887493676 0.0456912479311572
-v 0.3126383655419672 0.6403380339063044 -0.23865624924444484
-v 0.61316825014685 0.3664100700552299 0.11019183893255691
-v -0.22089366768258656 0.9994067753315382 -0.07643021005041595
-v 0.5989754733635821 0.41801385159116095 -0.09416438794709481
-v -0.6174217129315323 0.7320992050784687 -0.11660061080704831
-v 0.34053836469221843 0.9344305148683328 0.12672019136402968
-v 0.3600430314368503 0.6192025213711285 0.3155360997830766
-v 0.5241878224481961 0.23645301588769227 0.18999445004477328
-v 0.20540463359376865 0.9862352993580824 0.06860222345051709
-v -0.48375904489711824 0.8999870151081437 -0.10776015398792232
-v -0.5371930152583232 0.22269417285423693 0.02045675873758812
-v -0.3918878231502397 0.027465961810078113 -0.0016181923535628084
-v 0.40915831185998935 0.03297366281328307 0.07700244997159622
-v -0.336897812780938 0.9751425242103723 0.05317926933888001
-v -0.274278084009981 0.9440397354068651 0.11561985961273624
-v 0.41755813145918597 0.05460615743586433 -0.009793124373953577
-v 0.36250803429999845 0.8383621762129823 -0.20284968499796352
-v 0.41051462662911037 0.3882140508065027 0.30819988896512646
-v 0.03020023981258907 0.9629316886563344 0.13987835188864456
-v 0.5743109629768014 0.22407914471146836 0.09092520636830537
-v -0.1778384644806908 -0.10239833199086085 -0.0004956392695580902
-v -0.21912888125563373 1.0239733929908685 0.06587135229089303
-v 0.5684770249856574 0.6564159916089021 -0.12680307392519152
-v -0.27096335587156894 0.3911301733105495 0.22948223792472489
-v 0.04961377547430673 1.0419146556852357 -0.0478136448525559
-v 0.43601024145618517 0.8896948788135565 -0.12294641199285317
-v 0.0545881602160318 -0.10646731198268644 -0.10017866000678238
-v -0.4438834989797132 0.6263888465317633 -0.32678270919827485
-v -0.1099633438699708 1.0262462673115818 0.054165787698413626
-v -0.3254668360846248 0.2521348306949759 -0.27523566281656087
-v -0.27134384933032885 1.0119464194995222 0.010109746363874461
-v -0.25009797893305613 0.37595849056572495 -0.3001850012727912
-v -0.43089001372482577 0.15333849070567118 -0.20243695388536953
-v 0.47040947945888256 0.6777023673102476 -0.18331117352485116
-v -0.23624564782887847 -0.07473138531298552 -0.12715804180322737
-v 0.22833332887557367 -0.08424620890667525 0.07158777056452631
-v -0.5278412382939461 0.8312467418851792 0.09364010559580405
-v -0.5960052675990208 0.2118193120460683 -0.11503062446742218
-v 0.6466914797378791 0.6294322388813847 0.1611989959856323
-v -0.3912613945253691 0.36093161706826954 0.20032652343654378
-v -0.2768972416012521 -0.06025881977240541 0.0341237682960609
-v -0.1449270179947676 0.8862963948777918 -0.218664024706125
-v 0.574987346047534 0.7017131290595637 -0.11551276667772015
-v -0.3320173501822831 0.981528759477555 0.006191029385312352
-v 0.04414446859499627 0.018904634462761902 0.13811550145868756
-v -0.3126434917175691 0.5888975611161336 0.25852067323745165
-v 0.25804744863885476 0.7737451994017154 0.31617049760513327
-v 0.07725048665876016 -0.11849335600540556 -0.0708652813862698
-v 0.23068828433193012 0.586377342568983 0.3405151127150782
-v 0.49007430073053226 0.3539033888496617 -0.1537118419221717
-v -0.6350878496850334 0.4320894586568175 0.08972668120887006
-v 0.24765359116593877 0.4764808254687489 0.34069430688615054
-v 0.30795028791982876 0.33744288676172074 0.30671608945810586
-v 0.3445481865768727 0.8420167053412856 0.24112741895288564
-v 0.561763644248093 0.8550269594908949 0.12920156583351156
-v -0.5641509660218031 0.5266078005221415 0.1299628977194905
-v -0.6232446643217986 0.4058485800957602 -0.2315105211889302
-v -0.3502974868603396 0.30664724303348445 -0.2968670621138706
-v 0.30232762042498135 0.2562038647208614 0.2781981130672706
-v -0.42692518656199974 0.7693250195607457 -0.3041170661249278
-v 0.520102320485851 0.8267905675090668 -0.05405224826619465
-v 0.4096745712315625 0.15958176808256141 0.2334377255697942
-v -0.5478130716873577 0.8116993878824705 -0.1540750217752953
-v 0.42525819492000216 0.4222404686243919 0.3097750783690063
-v -0.45803672040374976 0.7791665371004801 0.14174636000730362
-v 0.141431925481868 -0.1229584587579747 0.06541965113978288
-v 0.3383782430551372 0.9914309840347261 -0.09236619146460462
-v 0.016657211453283527 1.023590186759229 0.024038672388621174
-v 0.373846434625112 0.04809248987858458 -0.11223272750799007
-v -0.37973898640408277 0.9249358587269481 -0.15998527975194204
-v -0.3927156168080518 0.6175336795473476 0.24137912033908682
+v -0.25371647695714983 0.6799748787759159 -0.3428533269427781
+v 0.29020573392093846 0.4545565255836158 -0.24521987513475973
+v -0.4423121034011809 0.23719421623788245 -0.23489987589073458
+v -0.4215051155125492 0.9306048887493676 0.0456912479311572
+v 0.3126383655419672 0.6403380339063044 -0.23865624924444484
+v 0.61316825014685 0.3664100700552299 0.11019183893255691
+v -0.22089366768258656 0.9994067753315382 -0.07643021005041595
+v 0.5989754733635821 0.41801385159116095 -0.09416438794709481
+v -0.6174217129315323 0.7320992050784687 -0.11660061080704831
+v 0.34053836469221843 0.9344305148683328 0.12672019136402968
+v 0.3600430314368503 0.6192025213711285 0.3155360997830766
+v 0.5241878224481961 0.23645301588769227 0.18999445004477328
+v 0.20540463359376865 0.9862352993580824 0.06860222345051709
+v -0.48375904489711824 0.8999870151081437 -0.10776015398792232
+v -0.5371930152583232 0.22269417285423693 0.02045675873758812
+v -0.3918878231502397 0.027465961810078113 -0.0016181923535628084
+v 0.40915831185998935 0.03297366281328307 0.07700244997159622
+v -0.336897812780938 0.9751425242103723 0.05317926933888001
+v -0.274278084009981 0.9440397354068651 0.11561985961273624
+v 0.41755813145918597 0.05460615743586433 -0.009793124373953577
+v 0.36250803429999845 0.8383621762129823 -0.20284968499796352
+v 0.41051462662911037 0.3882140508065027 0.30819988896512646
+v 0.03020023981258907 0.9629316886563344 0.13987835188864456
+v 0.5743109629768014 0.22407914471146836 0.09092520636830537
+v -0.1778384644806908 -0.10239833199086085 -0.0004956392695580902
+v -0.21912888125563373 1.0239733929908685 0.06587135229089303
+v 0.5684770249856574 0.6564159916089021 -0.12680307392519152
+v -0.27096335587156894 0.3911301733105495 0.22948223792472489
+v 0.04961377547430673 1.0419146556852357 -0.0478136448525559
+v 0.43601024145618517 0.8896948788135565 -0.12294641199285317
+v 0.0545881602160318 -0.10646731198268644 -0.10017866000678238
+v -0.4438834989797132 0.6263888465317633 -0.32678270919827485
+v -0.1099633438699708 1.0262462673115818 0.054165787698413626
+v -0.3254668360846248 0.2521348306949759 -0.27523566281656087
+v -0.27134384933032885 1.0119464194995222 0.010109746363874461
+v -0.25009797893305613 0.37595849056572495 -0.3001850012727912
+v -0.43089001372482577 0.15333849070567118 -0.20243695388536953
+v 0.47040947945888256 0.6777023673102476 -0.18331117352485116
+v -0.23624564782887847 -0.07473138531298552 -0.12715804180322737
+v 0.22833332887557367 -0.08424620890667525 0.07158777056452631
+v -0.5278412382939461 0.8312467418851792 0.09364010559580405
+v -0.5960052675990208 0.2118193120460683 -0.11503062446742218
+v 0.6466914797378791 0.6294322388813847 0.1611989959856323
+v -0.3912613945253691 0.36093161706826954 0.20032652343654378
+v -0.2768972416012521 -0.06025881977240541 0.0341237682960609
+v -0.1449270179947676 0.8862963948777918 -0.218664024706125
+v 0.574987346047534 0.7017131290595637 -0.11551276667772015
+v -0.3320173501822831 0.981528759477555 0.006191029385312352
+v 0.04414446859499627 0.018904634462761902 0.13811550145868756
+v -0.3126434917175691 0.5888975611161336 0.25852067323745165
+v 0.25804744863885476 0.7737451994017154 0.31617049760513327
+v 0.07725048665876016 -0.11849335600540556 -0.0708652813862698
+v 0.23068828433193012 0.586377342568983 0.3405151127150782
+v 0.49007430073053226 0.3539033888496617 -0.1537118419221717
+v -0.6350878496850334 0.4320894586568175 0.08972668120887006
+v 0.24765359116593877 0.4764808254687489 0.34069430688615054
+v 0.30795028791982876 0.33744288676172074 0.30671608945810586
+v 0.3445481865768727 0.8420167053412856 0.24112741895288564
+v 0.561763644248093 0.8550269594908949 0.12920156583351156
+v -0.5641509660218031 0.5266078005221415 0.1299628977194905
+v -0.6232446643217986 0.4058485800957602 -0.2315105211889302
+v -0.3502974868603396 0.30664724303348445 -0.2968670621138706
+v 0.30232762042498135 0.2562038647208614 0.2781981130672706
+v -0.42692518656199974 0.7693250195607457 -0.3041170661249278
+v 0.520102320485851 0.8267905675090668 -0.05405224826619465
+v 0.4096745712315625 0.15958176808256141 0.2334377255697942
+v -0.5478130716873577 0.8116993878824705 -0.1540750217752953
+v 0.42525819492000216 0.4222404686243919 0.3097750783690063
+v -0.45803672040374976 0.7791665371004801 0.14174636000730362
+v 0.141431925481868 -0.1229584587579747 0.06541965113978288
+v 0.3383782430551372 0.9914309840347261 -0.09236619146460462
+v 0.016657211453283527 1.023590186759229 0.024038672388621174
+v 0.373846434625112 0.04809248987858458 -0.11223272750799007
+v -0.37973898640408277 0.9249358587269481 -0.15998527975194204
+v -0.3927156168080518 0.6175336795473476 0.24137912033908682
g foo
-f 66 49 70
-f 45 49 70
-f 63 66 49
-f 51 19 75
-f 51 19 26
-f 25 45 70
-f 9 61 55
-f 9 41 55
-f 42 61 37
-f 42 61 55
-f 2 38 54
-f 2 36 1
-f 2 73 54
-f 8 73 54
-f 29 13 71
-f 29 46 71
-f 29 46 74
-f 12 68 43
-f 12 17 66
-f 22 12 66
-f 22 12 68
-f 22 68 56
-f 60 75 55
-f 60 41 55
-f 60 41 75
-f 44 45 49
-f 44 75 55
-f 44 45 55
-f 44 63 49
-f 23 51 26
-f 23 51 58
-f 69 41 75
-f 69 19 75
-f 69 19 41
-f 53 68 56
-f 40 66 70
-f 40 17 66
-f 3 61 37
-f 64 9 67
-f 64 32 1
-f 64 32 61
-f 64 9 61
-f 64 46 1
-f 64 46 74
-f 15 45 55
-f 15 42 55
-f 59 51 58
-f 59 30 71
-f 59 30 65
-f 5 2 1
-f 5 2 38
-f 47 30 65
-f 47 8 43
-f 47 59 43
-f 47 59 65
-f 21 5 38
-f 21 47 38
-f 21 47 30
-f 21 30 71
-f 21 5 1
-f 21 46 71
-f 21 46 1
-f 20 40 73
-f 20 40 17
-f 7 29 74
-f 33 29 26
-f 33 23 26
-f 33 23 13
-f 24 8 73
-f 24 20 73
-f 24 12 17
-f 24 20 17
-f 57 63 66
-f 57 22 66
-f 57 22 56
-f 57 44 63
-f 35 7 74
-f 35 29 26
-f 35 7 29
-f 18 4 41
-f 18 19 26
-f 18 19 41
-f 18 35 26
-f 14 4 41
-f 14 9 67
-f 14 9 41
-f 14 64 67
-f 14 64 74
-f 14 35 74
-f 50 44 75
-f 50 53 56
-f 50 51 75
-f 50 53 51
-f 11 53 68
-f 11 53 51
-f 11 68 43
-f 11 59 43
-f 11 59 51
-f 62 2 73
-f 62 2 36
-f 62 3 37
-f 62 36 1
-f 62 32 1
-f 62 32 61
-f 62 3 61
-f 39 25 45
-f 39 42 37
-f 52 25 70
-f 52 40 70
-f 52 39 25
-f 52 39 31
-f 52 40 73
-f 52 31 73
-f 16 15 45
-f 16 15 42
-f 16 39 45
-f 16 39 42
-f 10 59 58
-f 10 23 58
-f 10 23 13
-f 10 13 71
-f 10 59 71
-f 27 47 38
-f 27 47 8
-f 27 38 54
-f 27 8 54
-f 72 29 13
-f 72 33 13
-f 72 33 29
-f 6 8 43
-f 6 24 8
-f 6 12 43
-f 6 24 12
-f 48 14 4
-f 48 14 35
-f 48 18 4
-f 48 18 35
-f 28 50 56
-f 28 50 44
-f 28 57 56
-f 28 57 44
-f 34 39 31
-f 34 31 73
-f 34 62 73
-f 34 62 37
-f 34 39 37
-g
+f 66 49 70
+f 45 49 70
+f 63 66 49
+f 51 19 75
+f 51 19 26
+f 25 45 70
+f 9 61 55
+f 9 41 55
+f 42 61 37
+f 42 61 55
+f 2 38 54
+f 2 36 1
+f 2 73 54
+f 8 73 54
+f 29 13 71
+f 29 46 71
+f 29 46 74
+f 12 68 43
+f 12 17 66
+f 22 12 66
+f 22 12 68
+f 22 68 56
+f 60 75 55
+f 60 41 55
+f 60 41 75
+f 44 45 49
+f 44 75 55
+f 44 45 55
+f 44 63 49
+f 23 51 26
+f 23 51 58
+f 69 41 75
+f 69 19 75
+f 69 19 41
+f 53 68 56
+f 40 66 70
+f 40 17 66
+f 3 61 37
+f 64 9 67
+f 64 32 1
+f 64 32 61
+f 64 9 61
+f 64 46 1
+f 64 46 74
+f 15 45 55
+f 15 42 55
+f 59 51 58
+f 59 30 71
+f 59 30 65
+f 5 2 1
+f 5 2 38
+f 47 30 65
+f 47 8 43
+f 47 59 43
+f 47 59 65
+f 21 5 38
+f 21 47 38
+f 21 47 30
+f 21 30 71
+f 21 5 1
+f 21 46 71
+f 21 46 1
+f 20 40 73
+f 20 40 17
+f 7 29 74
+f 33 29 26
+f 33 23 26
+f 33 23 13
+f 24 8 73
+f 24 20 73
+f 24 12 17
+f 24 20 17
+f 57 63 66
+f 57 22 66
+f 57 22 56
+f 57 44 63
+f 35 7 74
+f 35 29 26
+f 35 7 29
+f 18 4 41
+f 18 19 26
+f 18 19 41
+f 18 35 26
+f 14 4 41
+f 14 9 67
+f 14 9 41
+f 14 64 67
+f 14 64 74
+f 14 35 74
+f 50 44 75
+f 50 53 56
+f 50 51 75
+f 50 53 51
+f 11 53 68
+f 11 53 51
+f 11 68 43
+f 11 59 43
+f 11 59 51
+f 62 2 73
+f 62 2 36
+f 62 3 37
+f 62 36 1
+f 62 32 1
+f 62 32 61
+f 62 3 61
+f 39 25 45
+f 39 42 37
+f 52 25 70
+f 52 40 70
+f 52 39 25
+f 52 39 31
+f 52 40 73
+f 52 31 73
+f 16 15 45
+f 16 15 42
+f 16 39 45
+f 16 39 42
+f 10 59 58
+f 10 23 58
+f 10 23 13
+f 10 13 71
+f 10 59 71
+f 27 47 38
+f 27 47 8
+f 27 38 54
+f 27 8 54
+f 72 29 13
+f 72 33 13
+f 72 33 29
+f 6 8 43
+f 6 24 8
+f 6 12 43
+f 6 24 12
+f 48 14 4
+f 48 14 35
+f 48 18 4
+f 48 18 35
+f 28 50 56
+f 28 50 44
+f 28 57 56
+f 28 57 44
+f 34 39 31
+f 34 31 73
+f 34 62 73
+f 34 62 37
+f 34 39 37
+g
diff --git a/data/relative_effector_positions/anymal_LF_ADAPTER_TO_FOOT_constraints_in_RHleg.obj b/data/relative_effector_positions/anymal_LF_ADAPTER_TO_FOOT_constraints_in_RHleg.obj
index 1eee4646f82981c13f6b49248742b34bd18027ca..fa483c53a299f043f5dea17f1dc4f5898ed94c4f 100644
--- a/data/relative_effector_positions/anymal_LF_ADAPTER_TO_FOOT_constraints_in_RHleg.obj
+++ b/data/relative_effector_positions/anymal_LF_ADAPTER_TO_FOOT_constraints_in_RHleg.obj
@@ -1,229 +1,229 @@
-v -0.052186058738587476 0.6715354267181292 0.04146096289394907
-v 0.6006569890441394 1.0174113872167452 -0.04781523554606598
-v 0.08657547209554617 0.21622480246035297 -0.15177681086674588
-v 0.14696110946763008 0.883261733043069 0.11547685036682531
-v 0.4793769786576849 -0.09524519730759502 -0.04759682856964276
-v -0.054745809892117026 0.7866903898995912 -0.0661358827018198
-v 1.1076522382609182 0.5956914766788222 -0.06540724741611711
-v 0.012250292888808578 0.8959270108712803 -0.02023259946858863
-v 1.1033295559388543 0.4421053253896936 -0.08713479119623119
-v 0.8814999756273814 0.7584183788889511 0.283491384324239
-v 0.8797213095418484 0.9777070875342824 -0.08089397431765177
-v 0.7080328949749688 0.6808029378883678 -0.29499516620088606
-v 0.03784467925055483 -0.05689051843397952 0.06930311781158971
-v 1.057620212431891 0.055223461804426624 0.010680785624305855
-v 0.8490707050420219 0.4387164176103174 0.3157548738660349
-v 0.9510951544228079 0.7877949024842967 -0.1624984368157601
-v 0.2549390284751417 0.9834619955822634 0.0948385183443024
-v 1.0772869195099077 0.6906440110294891 -0.04461464186407205
-v 0.2594457596969465 0.11354787678827069 -0.1402528316430397
-v 0.66733217283874 0.9654866934019485 0.100081889111727
-v 1.141387125392382 0.4268531261482441 0.15808471360596116
-v 0.6633944703595794 1.013766557759125 -0.016568880096893168
-v 0.41676682342997373 1.0064140285759904 -0.11651875199185818
-v 1.0397619277740482 0.42056929497750295 0.22477857871637863
-v 0.7949290025411306 -0.06697468890968604 -0.1261807016385631
-v 0.8436392683520442 0.369839093555039 -0.2633822911886875
-v 0.22472064101720873 0.616200182084464 0.2524745013685493
-v 0.9176990754633856 0.42628332076368813 -0.24870390372211387
-v 1.0309170190233656 0.5073761650063267 -0.16932365049530507
-v 0.8507781532039418 0.1879800586374831 0.21328736642953516
-v 0.3090766954834622 0.8001386363593801 -0.22141574013936965
-v -0.0014103055057584496 0.29080686276796447 0.09293266318790205
-v 1.0959610138634153 0.6327658369706911 0.1096331243740582
-v 1.0451034405685347 0.7823085156173795 0.06449226330854962
-v 0.23132525736514206 0.7226364402876801 -0.2404773988263515
-v 0.5205018154938208 -0.09494204187239988 0.09318562654040702
-v 0.8766411295924498 0.9400285161065368 0.09353237413725479
-v 0.9401852152365353 0.8584116878298973 0.15422226855187393
-v 0.5553316189654864 1.0128035212349558 0.048846194236874974
-v 0.5696783381323665 -0.11455256053067364 0.045835757652572084
-v 0.12038297555648408 0.44450809327969865 -0.2730842199224136
-v 0.270915513071742 -0.06436773339745047 -0.022021891796854653
-v 0.8196913598720024 0.8340568004536715 0.2431444809807837
-v 0.13520060676929163 0.7485861952639247 0.20838697392652494
-v 0.14388104422057885 0.6182119272019748 -0.24361459155251958
-v 0.7662953995464215 0.6281042057119652 -0.3018902524153172
-v 0.7384722364924339 0.8685961547305201 -0.2316251441802929
-v 0.9331109314548167 0.632590491951534 -0.211117048863943
-v 0.7501239019792867 0.35510572571080884 -0.2645005930270417
-v 0.8618430000106856 0.8804060375983322 0.18101625768941731
-v 0.7565060814483878 0.8205190296265314 0.2681242154880791
-v 1.1020221651815858 0.18796898994581263 0.049333389898626345
-v -0.05475479237363495 0.1106410927757966 0.024125409761160288
-v 0.452243934667251 -0.11140593005588145 -0.006629172364510494
-v 0.14034732411469725 0.6709909895799657 0.22204642689107895
-v -0.09187059949551221 0.6773256703071341 -0.16542086710986476
-v 1.027761514275682 0.19230141903193215 -0.11590759915466187
-v 0.9489051065769997 0.7569046566357206 0.23219396553569283
-v -0.024091571044383725 0.6935778127599832 0.1363722317891768
-v 1.0272791251984015 0.7766771705789324 -0.12190827206447286
-v 1.0613117575537845 0.06208516926256423 0.1148192527181065
-v 0.3696867115823105 1.0089913098634167 0.07039448838131557
-v 0.10573807738718366 0.9582558385287894 -0.03679889094355765
-v 0.10128673406694527 0.9441022156794496 0.0290454112670524
-v 0.39900143272857647 1.031673049688779 0.026667210877306224
-v 0.8815558957052275 -0.054699156569096574 -0.05042943830157365
-v -0.019497343059730388 0.7770183442803242 -0.1696617413182928
-v 0.9687114618726644 0.07445403539800795 -0.14968986261994854
-v 1.0206489116057722 0.8194063419321018 0.05333748500210095
-v 0.8554592814889905 0.34556515248731057 0.29127073575355455
-v 0.8394520423159423 0.9496887112783993 -0.13010608361112852
-v 0.17034746085503719 0.9633191364930949 -0.11647580522762713
-v 1.0466934234441458 9.486403272819521e-05 0.03373576847878018
-v 0.681701109248365 0.018751379757344325 0.14273172055320815
-v 1.0683278426779181 0.6868443353587095 0.16630615283798655
-v 0.6815207369323745 0.6191727876618469 0.2947564137553465
-v 0.16255218932724552 0.3881222096544618 0.20199869472483867
+v -0.052186058738587476 0.6715354267181292 0.04146096289394907
+v 0.6006569890441394 1.0174113872167452 -0.04781523554606598
+v 0.08657547209554617 0.21622480246035297 -0.15177681086674588
+v 0.14696110946763008 0.883261733043069 0.11547685036682531
+v 0.4793769786576849 -0.09524519730759502 -0.04759682856964276
+v -0.054745809892117026 0.7866903898995912 -0.0661358827018198
+v 1.1076522382609182 0.5956914766788222 -0.06540724741611711
+v 0.012250292888808578 0.8959270108712803 -0.02023259946858863
+v 1.1033295559388543 0.4421053253896936 -0.08713479119623119
+v 0.8814999756273814 0.7584183788889511 0.283491384324239
+v 0.8797213095418484 0.9777070875342824 -0.08089397431765177
+v 0.7080328949749688 0.6808029378883678 -0.29499516620088606
+v 0.03784467925055483 -0.05689051843397952 0.06930311781158971
+v 1.057620212431891 0.055223461804426624 0.010680785624305855
+v 0.8490707050420219 0.4387164176103174 0.3157548738660349
+v 0.9510951544228079 0.7877949024842967 -0.1624984368157601
+v 0.2549390284751417 0.9834619955822634 0.0948385183443024
+v 1.0772869195099077 0.6906440110294891 -0.04461464186407205
+v 0.2594457596969465 0.11354787678827069 -0.1402528316430397
+v 0.66733217283874 0.9654866934019485 0.100081889111727
+v 1.141387125392382 0.4268531261482441 0.15808471360596116
+v 0.6633944703595794 1.013766557759125 -0.016568880096893168
+v 0.41676682342997373 1.0064140285759904 -0.11651875199185818
+v 1.0397619277740482 0.42056929497750295 0.22477857871637863
+v 0.7949290025411306 -0.06697468890968604 -0.1261807016385631
+v 0.8436392683520442 0.369839093555039 -0.2633822911886875
+v 0.22472064101720873 0.616200182084464 0.2524745013685493
+v 0.9176990754633856 0.42628332076368813 -0.24870390372211387
+v 1.0309170190233656 0.5073761650063267 -0.16932365049530507
+v 0.8507781532039418 0.1879800586374831 0.21328736642953516
+v 0.3090766954834622 0.8001386363593801 -0.22141574013936965
+v -0.0014103055057584496 0.29080686276796447 0.09293266318790205
+v 1.0959610138634153 0.6327658369706911 0.1096331243740582
+v 1.0451034405685347 0.7823085156173795 0.06449226330854962
+v 0.23132525736514206 0.7226364402876801 -0.2404773988263515
+v 0.5205018154938208 -0.09494204187239988 0.09318562654040702
+v 0.8766411295924498 0.9400285161065368 0.09353237413725479
+v 0.9401852152365353 0.8584116878298973 0.15422226855187393
+v 0.5553316189654864 1.0128035212349558 0.048846194236874974
+v 0.5696783381323665 -0.11455256053067364 0.045835757652572084
+v 0.12038297555648408 0.44450809327969865 -0.2730842199224136
+v 0.270915513071742 -0.06436773339745047 -0.022021891796854653
+v 0.8196913598720024 0.8340568004536715 0.2431444809807837
+v 0.13520060676929163 0.7485861952639247 0.20838697392652494
+v 0.14388104422057885 0.6182119272019748 -0.24361459155251958
+v 0.7662953995464215 0.6281042057119652 -0.3018902524153172
+v 0.7384722364924339 0.8685961547305201 -0.2316251441802929
+v 0.9331109314548167 0.632590491951534 -0.211117048863943
+v 0.7501239019792867 0.35510572571080884 -0.2645005930270417
+v 0.8618430000106856 0.8804060375983322 0.18101625768941731
+v 0.7565060814483878 0.8205190296265314 0.2681242154880791
+v 1.1020221651815858 0.18796898994581263 0.049333389898626345
+v -0.05475479237363495 0.1106410927757966 0.024125409761160288
+v 0.452243934667251 -0.11140593005588145 -0.006629172364510494
+v 0.14034732411469725 0.6709909895799657 0.22204642689107895
+v -0.09187059949551221 0.6773256703071341 -0.16542086710986476
+v 1.027761514275682 0.19230141903193215 -0.11590759915466187
+v 0.9489051065769997 0.7569046566357206 0.23219396553569283
+v -0.024091571044383725 0.6935778127599832 0.1363722317891768
+v 1.0272791251984015 0.7766771705789324 -0.12190827206447286
+v 1.0613117575537845 0.06208516926256423 0.1148192527181065
+v 0.3696867115823105 1.0089913098634167 0.07039448838131557
+v 0.10573807738718366 0.9582558385287894 -0.03679889094355765
+v 0.10128673406694527 0.9441022156794496 0.0290454112670524
+v 0.39900143272857647 1.031673049688779 0.026667210877306224
+v 0.8815558957052275 -0.054699156569096574 -0.05042943830157365
+v -0.019497343059730388 0.7770183442803242 -0.1696617413182928
+v 0.9687114618726644 0.07445403539800795 -0.14968986261994854
+v 1.0206489116057722 0.8194063419321018 0.05333748500210095
+v 0.8554592814889905 0.34556515248731057 0.29127073575355455
+v 0.8394520423159423 0.9496887112783993 -0.13010608361112852
+v 0.17034746085503719 0.9633191364930949 -0.11647580522762713
+v 1.0466934234441458 9.486403272819521e-05 0.03373576847878018
+v 0.681701109248365 0.018751379757344325 0.14273172055320815
+v 1.0683278426779181 0.6868443353587095 0.16630615283798655
+v 0.6815207369323745 0.6191727876618469 0.2947564137553465
+v 0.16255218932724552 0.3881222096544618 0.20199869472483867
g foo
-f 53 1 56
-f 36 13 40
-f 36 70 13
-f 54 13 40
-f 54 25 40
-f 54 25 5
-f 3 53 13
-f 3 41 56
-f 3 53 56
-f 66 25 40
-f 59 53 1
-f 59 77 13
-f 63 65 17
-f 11 69 60
-f 52 61 21
-f 18 7 60
-f 29 7 60
-f 16 47 46
-f 12 47 46
-f 12 41 46
-f 31 12 47
-f 74 36 70
-f 74 36 61
-f 42 3 13
-f 42 54 13
-f 42 54 5
-f 19 25 5
-f 19 42 5
-f 19 42 3
-f 19 25 41
-f 19 3 41
-f 49 41 46
-f 49 25 41
-f 73 68 25
-f 73 66 25
-f 73 14 68
-f 73 52 61
-f 73 14 52
-f 73 66 40
-f 73 36 40
-f 73 36 61
-f 15 77 27
-f 15 70 13
-f 15 77 13
-f 44 51 27
-f 44 51 17
-f 62 65 17
-f 62 51 17
-f 32 53 13
-f 32 59 13
-f 32 59 53
-f 72 8 63
-f 72 65 23
-f 72 63 65
-f 72 47 23
-f 72 31 47
-f 20 62 51
-f 2 11 22
-f 2 11 23
-f 2 65 22
-f 2 65 23
-f 71 47 23
-f 71 11 23
-f 71 16 47
-f 71 11 60
-f 71 16 60
-f 48 29 60
-f 48 16 46
-f 48 16 60
-f 28 29 68
-f 28 48 46
-f 28 48 29
-f 35 31 12
-f 35 45 41
-f 35 12 41
-f 30 61 70
-f 30 74 70
-f 30 74 61
-f 76 51 27
-f 76 15 27
-f 76 10 51
-f 76 10 15
-f 55 44 27
-f 55 44 59
-f 55 77 27
-f 55 59 77
-f 4 44 17
-f 4 44 59
-f 75 10 21
-f 24 10 21
-f 24 10 15
-f 24 15 70
-f 24 61 21
-f 24 61 70
-f 50 20 51
-f 6 1 56
-f 6 59 1
-f 6 8 59
-f 64 63 17
-f 64 8 63
-f 64 4 17
-f 64 8 59
-f 64 4 59
-f 39 62 65
-f 39 20 62
-f 39 65 22
-f 9 29 7
-f 9 7 21
-f 9 52 21
-f 9 14 52
-f 26 49 46
-f 26 28 46
-f 26 28 68
-f 26 68 25
-f 26 49 25
-f 67 72 31
-f 67 35 31
-f 67 35 45
-f 67 6 56
-f 67 72 8
-f 67 41 56
-f 67 45 41
-f 67 6 8
-f 34 69 60
-f 34 18 60
-f 37 11 69
-f 37 11 22
-f 37 50 20
-f 37 39 22
-f 37 39 20
-f 38 50 10
-f 38 37 50
-f 38 37 69
-f 38 34 69
-f 38 34 75
-f 43 10 51
-f 43 50 51
-f 43 50 10
-f 57 29 68
-f 57 9 29
-f 57 14 68
-f 57 9 14
-f 33 34 75
-f 33 7 21
-f 33 75 21
-f 33 18 7
-f 33 34 18
-f 58 75 10
-f 58 38 10
-f 58 38 75
-g
+f 53 1 56
+f 36 13 40
+f 36 70 13
+f 54 13 40
+f 54 25 40
+f 54 25 5
+f 3 53 13
+f 3 41 56
+f 3 53 56
+f 66 25 40
+f 59 53 1
+f 59 77 13
+f 63 65 17
+f 11 69 60
+f 52 61 21
+f 18 7 60
+f 29 7 60
+f 16 47 46
+f 12 47 46
+f 12 41 46
+f 31 12 47
+f 74 36 70
+f 74 36 61
+f 42 3 13
+f 42 54 13
+f 42 54 5
+f 19 25 5
+f 19 42 5
+f 19 42 3
+f 19 25 41
+f 19 3 41
+f 49 41 46
+f 49 25 41
+f 73 68 25
+f 73 66 25
+f 73 14 68
+f 73 52 61
+f 73 14 52
+f 73 66 40
+f 73 36 40
+f 73 36 61
+f 15 77 27
+f 15 70 13
+f 15 77 13
+f 44 51 27
+f 44 51 17
+f 62 65 17
+f 62 51 17
+f 32 53 13
+f 32 59 13
+f 32 59 53
+f 72 8 63
+f 72 65 23
+f 72 63 65
+f 72 47 23
+f 72 31 47
+f 20 62 51
+f 2 11 22
+f 2 11 23
+f 2 65 22
+f 2 65 23
+f 71 47 23
+f 71 11 23
+f 71 16 47
+f 71 11 60
+f 71 16 60
+f 48 29 60
+f 48 16 46
+f 48 16 60
+f 28 29 68
+f 28 48 46
+f 28 48 29
+f 35 31 12
+f 35 45 41
+f 35 12 41
+f 30 61 70
+f 30 74 70
+f 30 74 61
+f 76 51 27
+f 76 15 27
+f 76 10 51
+f 76 10 15
+f 55 44 27
+f 55 44 59
+f 55 77 27
+f 55 59 77
+f 4 44 17
+f 4 44 59
+f 75 10 21
+f 24 10 21
+f 24 10 15
+f 24 15 70
+f 24 61 21
+f 24 61 70
+f 50 20 51
+f 6 1 56
+f 6 59 1
+f 6 8 59
+f 64 63 17
+f 64 8 63
+f 64 4 17
+f 64 8 59
+f 64 4 59
+f 39 62 65
+f 39 20 62
+f 39 65 22
+f 9 29 7
+f 9 7 21
+f 9 52 21
+f 9 14 52
+f 26 49 46
+f 26 28 46
+f 26 28 68
+f 26 68 25
+f 26 49 25
+f 67 72 31
+f 67 35 31
+f 67 35 45
+f 67 6 56
+f 67 72 8
+f 67 41 56
+f 67 45 41
+f 67 6 8
+f 34 69 60
+f 34 18 60
+f 37 11 69
+f 37 11 22
+f 37 50 20
+f 37 39 22
+f 37 39 20
+f 38 50 10
+f 38 37 50
+f 38 37 69
+f 38 34 69
+f 38 34 75
+f 43 10 51
+f 43 50 51
+f 43 50 10
+f 57 29 68
+f 57 9 29
+f 57 14 68
+f 57 9 14
+f 33 34 75
+f 33 7 21
+f 33 75 21
+f 33 18 7
+f 33 34 18
+f 58 75 10
+f 58 38 10
+f 58 38 75
+g
diff --git a/data/relative_effector_positions/anymal_LH_ADAPTER_TO_FOOT_constraints_in_LFleg.obj b/data/relative_effector_positions/anymal_LH_ADAPTER_TO_FOOT_constraints_in_LFleg.obj
index 222592ed6bb2eedf6b31277fe9efbe983965e39f..b4bdafb75186885db19893ac2d079ddb41dfc924 100644
--- a/data/relative_effector_positions/anymal_LH_ADAPTER_TO_FOOT_constraints_in_LFleg.obj
+++ b/data/relative_effector_positions/anymal_LH_ADAPTER_TO_FOOT_constraints_in_LFleg.obj
@@ -1,196 +1,196 @@
-v -1.1016156456174515 0.18716037026434723 0.10491811898192216
-v -0.9483476051382198 0.40551556487632273 -0.028744151817720387
-v 0.10301695114753623 -0.05946302152310194 0.04433007650084542
-v -0.9675813320779578 0.11009740482113023 0.22442920404146566
-v -0.13082318632316392 0.2142191687052919 -0.08283293104784528
-v -0.1454796437740883 0.36449828074146284 0.2683236356176397
-v -0.8770808101830576 -0.3611980037955306 0.0541445064828015
-v -1.0008899762066488 0.34702340226975986 0.21884967817469614
-v -0.8181316875827684 0.3595975638346992 -0.11293535556139267
-v -1.0513914602045085 0.04989232089426465 -0.1675542411380365
-v -0.16574275234368932 -0.43358712296135354 -0.03596616269131475
-v -0.9985469168184148 0.3282806987615391 0.22021321451325399
-v -0.13250472598354443 -0.38348959572213154 -0.20289932512977166
-v -0.07777466659525026 0.44502695409735976 0.024122196316354705
-v -0.6712402787811517 0.47799052828723915 -0.035631518289514996
-v -0.3425088246411209 0.5365210826932507 0.04506345297335429
-v -0.12622369534448383 -0.21413048926666667 -0.23249963782275723
-v -0.23649580132743966 -0.5444888945408589 -0.1675813063167798
-v -0.6507224158162519 -0.5345221244132509 -0.22458977848284467
-v -0.6832270874934577 0.18690941614891868 -0.17797578146405935
-v -0.7637048493529556 0.4932664934188713 0.2625542806659487
-v -0.905586306729775 0.5100482073014392 0.07644761077276124
-v -0.8232100744328823 -0.31567835695077723 0.14516008072531628
-v -0.35781150920993976 0.5791349275530329 0.13475127469522385
-v 0.08362108480241309 0.2985316830254039 0.18151995288722567
-v -0.7100381937433868 0.3884462131146426 -0.09225319579849167
-v -0.17678027427351561 -0.2979362439742722 0.1019045943255204
-v -0.6183089216401132 -0.5246264838277366 -0.009282714854610408
-v 0.09097991048453513 -0.1357449324656998 -0.033677509794578675
-v -0.7813554966875866 0.465171249447521 0.26985175916242254
-v -0.36990208209458547 -0.5692732712741655 -0.13195398397068892
-v -0.8877273161038031 0.26154543276455366 -0.17448196911055291
-v -0.12408880351915416 -0.32834544073781613 -0.24788986580028383
-v -0.5062148923539761 0.6039636536170416 0.1980051538884065
-v -0.46895668735517476 -0.5784691536561541 -0.050189199634081194
-v -0.519118126105557 0.3951591579735598 -0.0678965261265651
-v -1.0842341947131373 -0.20046956959658446 0.015784419015480167
-v -0.305789476043127 -0.5184060930787473 -0.21994329441834315
-v -0.5062938539534614 0.6077308233628076 0.13110619459121903
-v -1.1228215086701279 -0.3166888565492295 -0.10749424890237158
-v -1.0801938534524556 0.08953799841332316 0.11905822683949335
-v -0.7460736315401675 -0.45881673386070143 -0.29533785775624843
-v -0.8628409720544576 0.15868143629418943 0.28594247501612524
-v -0.581235615219669 -0.5684783560454431 -0.07708417552219515
-v -1.059064779073533 -0.22550316818584323 0.04014376953473464
-v -0.6100348971314509 -0.5710252960272635 -0.18386986327042043
-v -0.29599674738931947 0.38402812929973457 0.2791030777621292
-v -0.13108649972968078 -0.4099074326800608 -0.06600538587529958
-v -1.1145765508001686 -0.029267545085410573 -0.04270911910775799
-v -0.762039071746003 -0.44843388537342965 0.025174143372533486
-v -0.6765319311638041 0.5499259920982886 0.059719234301817525
-v -0.6594241772010206 -0.3210964660612202 -0.2929017696401899
-v -0.0442546482201097 0.4044571292548988 0.01686928001851301
-v -0.7346045584637135 0.5573269083875003 0.08570262578271098
-v -0.784178910379753 -0.20120735990879318 -0.2881962351849568
-v -1.023699283659865 -0.4226477134644746 -0.276698806855761
-v -0.9057835926662687 0.3610935314665127 -0.10653999021285826
-v -0.8126525983890377 -0.5207363214620788 -0.1467996848800035
-v -0.8099678208284703 -0.01297467010194625 -0.26748323577493116
-v -0.6645742289611063 -0.26722123284487337 -0.28664870300177353
-v -0.17526625980224628 0.03720660182128155 0.20430844393851927
-v -0.7738827709013244 0.4426493527070797 -0.0591490050276669
-v -1.049690078778225 0.37397053437655525 0.027815522515540092
-v -0.7324987491648285 0.2207421292943082 0.32426009409954654
-v 0.07870655511397151 -0.14127144871409958 -0.014476751128909393
-v 0.06758838836482653 0.2688871132209691 0.1071370637564345
+v -1.1016156456174515 0.18716037026434723 0.10491811898192216
+v -0.9483476051382198 0.40551556487632273 -0.028744151817720387
+v 0.10301695114753623 -0.05946302152310194 0.04433007650084542
+v -0.9675813320779578 0.11009740482113023 0.22442920404146566
+v -0.13082318632316392 0.2142191687052919 -0.08283293104784528
+v -0.1454796437740883 0.36449828074146284 0.2683236356176397
+v -0.8770808101830576 -0.3611980037955306 0.0541445064828015
+v -1.0008899762066488 0.34702340226975986 0.21884967817469614
+v -0.8181316875827684 0.3595975638346992 -0.11293535556139267
+v -1.0513914602045085 0.04989232089426465 -0.1675542411380365
+v -0.16574275234368932 -0.43358712296135354 -0.03596616269131475
+v -0.9985469168184148 0.3282806987615391 0.22021321451325399
+v -0.13250472598354443 -0.38348959572213154 -0.20289932512977166
+v -0.07777466659525026 0.44502695409735976 0.024122196316354705
+v -0.6712402787811517 0.47799052828723915 -0.035631518289514996
+v -0.3425088246411209 0.5365210826932507 0.04506345297335429
+v -0.12622369534448383 -0.21413048926666667 -0.23249963782275723
+v -0.23649580132743966 -0.5444888945408589 -0.1675813063167798
+v -0.6507224158162519 -0.5345221244132509 -0.22458977848284467
+v -0.6832270874934577 0.18690941614891868 -0.17797578146405935
+v -0.7637048493529556 0.4932664934188713 0.2625542806659487
+v -0.905586306729775 0.5100482073014392 0.07644761077276124
+v -0.8232100744328823 -0.31567835695077723 0.14516008072531628
+v -0.35781150920993976 0.5791349275530329 0.13475127469522385
+v 0.08362108480241309 0.2985316830254039 0.18151995288722567
+v -0.7100381937433868 0.3884462131146426 -0.09225319579849167
+v -0.17678027427351561 -0.2979362439742722 0.1019045943255204
+v -0.6183089216401132 -0.5246264838277366 -0.009282714854610408
+v 0.09097991048453513 -0.1357449324656998 -0.033677509794578675
+v -0.7813554966875866 0.465171249447521 0.26985175916242254
+v -0.36990208209458547 -0.5692732712741655 -0.13195398397068892
+v -0.8877273161038031 0.26154543276455366 -0.17448196911055291
+v -0.12408880351915416 -0.32834544073781613 -0.24788986580028383
+v -0.5062148923539761 0.6039636536170416 0.1980051538884065
+v -0.46895668735517476 -0.5784691536561541 -0.050189199634081194
+v -0.519118126105557 0.3951591579735598 -0.0678965261265651
+v -1.0842341947131373 -0.20046956959658446 0.015784419015480167
+v -0.305789476043127 -0.5184060930787473 -0.21994329441834315
+v -0.5062938539534614 0.6077308233628076 0.13110619459121903
+v -1.1228215086701279 -0.3166888565492295 -0.10749424890237158
+v -1.0801938534524556 0.08953799841332316 0.11905822683949335
+v -0.7460736315401675 -0.45881673386070143 -0.29533785775624843
+v -0.8628409720544576 0.15868143629418943 0.28594247501612524
+v -0.581235615219669 -0.5684783560454431 -0.07708417552219515
+v -1.059064779073533 -0.22550316818584323 0.04014376953473464
+v -0.6100348971314509 -0.5710252960272635 -0.18386986327042043
+v -0.29599674738931947 0.38402812929973457 0.2791030777621292
+v -0.13108649972968078 -0.4099074326800608 -0.06600538587529958
+v -1.1145765508001686 -0.029267545085410573 -0.04270911910775799
+v -0.762039071746003 -0.44843388537342965 0.025174143372533486
+v -0.6765319311638041 0.5499259920982886 0.059719234301817525
+v -0.6594241772010206 -0.3210964660612202 -0.2929017696401899
+v -0.0442546482201097 0.4044571292548988 0.01686928001851301
+v -0.7346045584637135 0.5573269083875003 0.08570262578271098
+v -0.784178910379753 -0.20120735990879318 -0.2881962351849568
+v -1.023699283659865 -0.4226477134644746 -0.276698806855761
+v -0.9057835926662687 0.3610935314665127 -0.10653999021285826
+v -0.8126525983890377 -0.5207363214620788 -0.1467996848800035
+v -0.8099678208284703 -0.01297467010194625 -0.26748323577493116
+v -0.6645742289611063 -0.26722123284487337 -0.28664870300177353
+v -0.17526625980224628 0.03720660182128155 0.20430844393851927
+v -0.7738827709013244 0.4426493527070797 -0.0591490050276669
+v -1.049690078778225 0.37397053437655525 0.027815522515540092
+v -0.7324987491648285 0.2207421292943082 0.32426009409954654
+v 0.07870655511397151 -0.14127144871409958 -0.014476751128909393
+v 0.06758838836482653 0.2688871132209691 0.1071370637564345
g foo
-f 47 6 64
-f 56 10 40
-f 56 10 59
-f 49 10 40
-f 34 47 21
-f 34 47 6
-f 30 47 64
-f 30 47 21
-f 30 8 64
-f 30 8 21
-f 4 45 41
-f 1 49 40
-f 16 15 39
-f 36 16 15
-f 51 15 39
-f 7 50 40
-f 7 45 40
-f 37 45 40
-f 37 45 41
-f 37 1 40
-f 37 1 41
-f 31 18 35
-f 61 6 64
-f 61 27 64
-f 23 27 64
-f 23 4 45
-f 23 28 50
-f 23 27 35
-f 23 28 35
-f 23 7 45
-f 23 7 50
-f 11 18 35
-f 11 27 35
-f 11 27 3
-f 11 65 3
-f 24 34 6
-f 24 34 39
-f 24 16 39
-f 12 4 41
-f 12 1 41
-f 12 1 8
-f 63 1 8
-f 63 57 10
-f 63 49 10
-f 63 1 49
-f 5 17 53
-f 54 51 39
-f 54 34 39
-f 20 5 36
-f 20 17 59
-f 20 5 17
-f 32 20 36
-f 32 20 59
-f 32 10 59
-f 32 57 10
-f 55 56 59
-f 55 52 59
-f 60 52 59
-f 33 60 52
-f 33 13 18
-f 33 17 59
-f 33 60 59
-f 58 28 50
-f 58 56 40
-f 58 50 40
-f 42 55 56
-f 42 55 52
-f 42 33 52
-f 43 23 64
-f 43 23 4
-f 43 12 4
-f 43 8 64
-f 43 12 8
-f 48 11 18
-f 48 11 65
-f 25 24 6
-f 25 66 53
-f 25 66 3
-f 25 27 3
-f 25 61 6
-f 25 61 27
-f 14 5 53
-f 14 5 36
-f 14 36 16
-f 14 24 16
-f 14 25 53
-f 14 25 24
-f 22 54 34
-f 22 63 8
-f 22 34 21
-f 22 8 21
-f 26 36 15
-f 26 32 36
-f 9 32 57
-f 9 26 15
-f 9 26 32
-f 29 33 13
-f 29 13 18
-f 29 33 17
-f 29 48 18
-f 29 48 65
-f 29 17 53
-f 29 66 53
-f 29 66 3
-f 29 65 3
-f 44 28 35
-f 44 58 28
-f 46 31 18
-f 46 44 58
-f 46 58 56
-f 46 31 35
-f 46 44 35
-f 2 63 57
-f 2 22 57
-f 2 22 63
-f 62 22 54
-f 62 51 15
-f 62 54 51
-f 62 22 57
-f 62 9 15
-f 62 9 57
-f 38 46 18
-f 38 33 18
-f 38 42 33
-f 19 38 42
-f 19 38 46
-f 19 42 56
-f 19 46 56
-g
+f 47 6 64
+f 56 10 40
+f 56 10 59
+f 49 10 40
+f 34 47 21
+f 34 47 6
+f 30 47 64
+f 30 47 21
+f 30 8 64
+f 30 8 21
+f 4 45 41
+f 1 49 40
+f 16 15 39
+f 36 16 15
+f 51 15 39
+f 7 50 40
+f 7 45 40
+f 37 45 40
+f 37 45 41
+f 37 1 40
+f 37 1 41
+f 31 18 35
+f 61 6 64
+f 61 27 64
+f 23 27 64
+f 23 4 45
+f 23 28 50
+f 23 27 35
+f 23 28 35
+f 23 7 45
+f 23 7 50
+f 11 18 35
+f 11 27 35
+f 11 27 3
+f 11 65 3
+f 24 34 6
+f 24 34 39
+f 24 16 39
+f 12 4 41
+f 12 1 41
+f 12 1 8
+f 63 1 8
+f 63 57 10
+f 63 49 10
+f 63 1 49
+f 5 17 53
+f 54 51 39
+f 54 34 39
+f 20 5 36
+f 20 17 59
+f 20 5 17
+f 32 20 36
+f 32 20 59
+f 32 10 59
+f 32 57 10
+f 55 56 59
+f 55 52 59
+f 60 52 59
+f 33 60 52
+f 33 13 18
+f 33 17 59
+f 33 60 59
+f 58 28 50
+f 58 56 40
+f 58 50 40
+f 42 55 56
+f 42 55 52
+f 42 33 52
+f 43 23 64
+f 43 23 4
+f 43 12 4
+f 43 8 64
+f 43 12 8
+f 48 11 18
+f 48 11 65
+f 25 24 6
+f 25 66 53
+f 25 66 3
+f 25 27 3
+f 25 61 6
+f 25 61 27
+f 14 5 53
+f 14 5 36
+f 14 36 16
+f 14 24 16
+f 14 25 53
+f 14 25 24
+f 22 54 34
+f 22 63 8
+f 22 34 21
+f 22 8 21
+f 26 36 15
+f 26 32 36
+f 9 32 57
+f 9 26 15
+f 9 26 32
+f 29 33 13
+f 29 13 18
+f 29 33 17
+f 29 48 18
+f 29 48 65
+f 29 17 53
+f 29 66 53
+f 29 66 3
+f 29 65 3
+f 44 28 35
+f 44 58 28
+f 46 31 18
+f 46 44 58
+f 46 58 56
+f 46 31 35
+f 46 44 35
+f 2 63 57
+f 2 22 57
+f 2 22 63
+f 62 22 54
+f 62 51 15
+f 62 54 51
+f 62 22 57
+f 62 9 15
+f 62 9 57
+f 38 46 18
+f 38 33 18
+f 38 42 33
+f 19 38 42
+f 19 38 46
+f 19 42 56
+f 19 46 56
+g
diff --git a/data/relative_effector_positions/anymal_LH_ADAPTER_TO_FOOT_constraints_in_RFleg.obj b/data/relative_effector_positions/anymal_LH_ADAPTER_TO_FOOT_constraints_in_RFleg.obj
index 7d87c97b6e3c219497dce5a58b1371428734b79f..01d4e2b28f284b80ca966399a44e7a1f5e6b0520 100644
--- a/data/relative_effector_positions/anymal_LH_ADAPTER_TO_FOOT_constraints_in_RFleg.obj
+++ b/data/relative_effector_positions/anymal_LH_ADAPTER_TO_FOOT_constraints_in_RFleg.obj
@@ -1,250 +1,250 @@
-v -0.9405757361672906 0.8616709792727616 -0.12767764753906743
-v -1.1240514042041592 0.14112301160674312 -0.04216637712125565
-v 0.0421115032636957 0.47786623497445113 -0.04286973445164116
-v -0.06429912844013033 0.6405177883092998 0.2589738425172482
-v -0.034782600518695514 0.4615542232932315 -0.18863347347633774
-v -0.7278064932288637 0.7669784142229005 0.27356999736946325
-v -0.23842506095580374 0.8801725928826065 -0.16108289236873197
-v 0.06697442644417703 0.34780705738001494 0.06853315485595646
-v -0.8143268884886785 0.9773742454590657 0.041876964290914254
-v -1.091873926733319 0.6365353392464423 0.05574685199000878
-v -1.080228461477513 0.7926153260301343 0.026967577869018988
-v -0.8106468583148366 0.374066961428389 -0.32684506616040543
-v -1.0894906211517597 0.07826072620517467 0.002214919197831733
-v -0.6100043381507293 -0.07075142495213126 0.07210387414650965
-v -0.3947835146937222 1.0199177761172908 -0.04194748398418657
-v -1.1100386019325565 0.14501922006841017 -0.008082807132256953
-v -1.0588015736763814 0.8309529422572521 -0.10000290017317137
-v 0.08252526014887376 0.5307512907657159 0.1865755878823704
-v -0.9461727107226728 0.8926844628427666 0.06779045476446466
-v -0.8696675959436577 0.6146032831947359 0.30808479662435523
-v -0.17550185013634886 0.6071575543183397 -0.2820633172400618
-v -0.8145679302832763 0.9487502754268866 0.10713984858903991
-v 0.012402136297563732 0.7648849998394194 -0.05112919109904346
-v -0.7936182618448755 0.5646763264932246 0.319730891896664
-v -1.064620934930934 0.038921249848529654 0.04274125538366125
-v -0.8210560601953649 0.5114797226693114 -0.3499785265463613
-v -0.6057788370351336 -0.006782098637873241 0.13805315496163262
-v -1.000336386460533 0.6703049848416837 -0.2371294588209189
-v -0.9094123909267458 0.706137826046562 0.23260128386883316
-v 0.039825285740750616 0.7120347479852323 0.025245083171578087
-v -0.24021693908235867 -0.002979059748146995 0.11513803764678077
-v -0.8539043935243397 0.040660062816138404 -0.18595094308930182
-v -0.8644674674667996 0.8227313976097552 -0.2024601564878642
-v -0.9183786087937096 0.10213151077674762 0.1542784418486095
-v -0.12380887966989113 0.944281767664171 -0.063013063792205
-v -0.5279172199683972 0.9140843508198795 0.16613271274882613
-v -0.5757273286663042 0.9141649884213497 -0.17434029616975277
-v -0.09944023038027339 0.9747734604881828 0.017981121342632356
-v -1.1011671114305914 0.36678125244599996 0.07991646496763588
-v -0.6073976341169258 0.9597650482836105 0.12043026671171819
-v -0.9356203957448052 0.43578128109988556 0.23760431966499274
-v -0.2265821371971911 0.9451948161848855 -0.09329939010631977
-v -1.0415982137637454 0.14056254785672886 -0.11063575619445715
-v -0.6309673232578825 0.5233429585504008 0.3154324448276982
-v -1.1084771244357725 0.5884126062022549 -0.1487300274151031
-v -0.8717572608392643 0.7961376346283748 0.21461994431182724
-v -0.7935503679930702 0.10381562175747021 0.18064150493670278
-v -0.1930045861227373 -0.016472567669853083 0.055588103122916455
-v -0.2993876552253006 -0.068558130299813 -0.05000292959714747
-v -0.6924979778477462 0.9668359505626445 -0.09270230189174977
-v -0.6058417937103333 -0.02638670731363664 -0.13330177816314193
-v -0.8799156176905546 0.05316924253091149 0.14030335805749
-v 0.03079873151815908 0.0802799299948706 -0.021596095982981622
-v -1.0641264046687584 0.5831225665734691 0.13240524374201096
-v -0.8975839327885125 -0.03171601038986818 0.03175023217440753
-v -0.7485534984930229 0.6251246158792332 -0.3277257829104002
-v -0.00044654153001363883 0.5196138546850763 -0.1291549211950357
-v -0.33772493966100053 -0.1132855735710503 -0.023160301600170685
-v -0.8608413160501167 0.9056034514194153 0.13558463504892249
-v -0.5605073386486454 0.8097792079852102 0.23967071303923726
-v -1.0822732334889182 0.5308882222561354 -0.19977503297159924
-v -0.7099585314967243 0.7597139791843466 -0.2813809762769149
-v -0.12525657743891688 0.4504680405145489 -0.23678194793554713
-v -0.02220599896631595 0.833916447723732 0.15362716581839286
-v -0.8027646816452443 0.9234334483485782 -0.11700518232667789
-v -0.4585150943223921 -0.13886381502531764 0.01077830594725282
-v -0.6444362119717503 -0.10480736805227411 -0.03115727318618422
-v -0.35715072635957984 1.0147167589258765 0.043519543542239525
-v -0.44655818297057626 -0.1278474340611117 0.03982371044300115
-v -0.12918631114482187 0.7174134161851835 -0.18865575303773596
-v -0.6564281019615854 0.9971038952658463 -0.054806541006044984
-v -0.7799593668382123 0.8282157236495682 -0.2272305331051926
-v -0.46496795109363115 1.0229147298990597 0.05495869788773361
-v -0.42250500513503464 -0.12738031165599628 -0.02216266906471931
-v -0.37388741816348675 -0.1155213358708857 -0.031046522038815194
-v -0.23330502706074513 -0.07836863229235402 -0.004294196279049811
-v -0.18514171450456227 0.9108464599628172 0.14969176329083134
-v -0.21508494550247922 0.33639531608209455 0.24700349005588523
-v -0.7370612161626663 0.9777753370574719 -0.03579474750828032
-v -0.9378244560335888 0.053457606680938075 -0.13553624092047073
-v -0.8428810322893587 0.41346073701448866 0.27404067758492584
-v -0.9907265435532993 0.399146286546847 -0.24757822471673163
-v -0.8503141543566188 0.38781613084587124 0.26586269707141263
-v -0.7556473978696131 0.417790726206639 0.28918464416774486
+v -0.9405757361672906 0.8616709792727616 -0.12767764753906743
+v -1.1240514042041592 0.14112301160674312 -0.04216637712125565
+v 0.0421115032636957 0.47786623497445113 -0.04286973445164116
+v -0.06429912844013033 0.6405177883092998 0.2589738425172482
+v -0.034782600518695514 0.4615542232932315 -0.18863347347633774
+v -0.7278064932288637 0.7669784142229005 0.27356999736946325
+v -0.23842506095580374 0.8801725928826065 -0.16108289236873197
+v 0.06697442644417703 0.34780705738001494 0.06853315485595646
+v -0.8143268884886785 0.9773742454590657 0.041876964290914254
+v -1.091873926733319 0.6365353392464423 0.05574685199000878
+v -1.080228461477513 0.7926153260301343 0.026967577869018988
+v -0.8106468583148366 0.374066961428389 -0.32684506616040543
+v -1.0894906211517597 0.07826072620517467 0.002214919197831733
+v -0.6100043381507293 -0.07075142495213126 0.07210387414650965
+v -0.3947835146937222 1.0199177761172908 -0.04194748398418657
+v -1.1100386019325565 0.14501922006841017 -0.008082807132256953
+v -1.0588015736763814 0.8309529422572521 -0.10000290017317137
+v 0.08252526014887376 0.5307512907657159 0.1865755878823704
+v -0.9461727107226728 0.8926844628427666 0.06779045476446466
+v -0.8696675959436577 0.6146032831947359 0.30808479662435523
+v -0.17550185013634886 0.6071575543183397 -0.2820633172400618
+v -0.8145679302832763 0.9487502754268866 0.10713984858903991
+v 0.012402136297563732 0.7648849998394194 -0.05112919109904346
+v -0.7936182618448755 0.5646763264932246 0.319730891896664
+v -1.064620934930934 0.038921249848529654 0.04274125538366125
+v -0.8210560601953649 0.5114797226693114 -0.3499785265463613
+v -0.6057788370351336 -0.006782098637873241 0.13805315496163262
+v -1.000336386460533 0.6703049848416837 -0.2371294588209189
+v -0.9094123909267458 0.706137826046562 0.23260128386883316
+v 0.039825285740750616 0.7120347479852323 0.025245083171578087
+v -0.24021693908235867 -0.002979059748146995 0.11513803764678077
+v -0.8539043935243397 0.040660062816138404 -0.18595094308930182
+v -0.8644674674667996 0.8227313976097552 -0.2024601564878642
+v -0.9183786087937096 0.10213151077674762 0.1542784418486095
+v -0.12380887966989113 0.944281767664171 -0.063013063792205
+v -0.5279172199683972 0.9140843508198795 0.16613271274882613
+v -0.5757273286663042 0.9141649884213497 -0.17434029616975277
+v -0.09944023038027339 0.9747734604881828 0.017981121342632356
+v -1.1011671114305914 0.36678125244599996 0.07991646496763588
+v -0.6073976341169258 0.9597650482836105 0.12043026671171819
+v -0.9356203957448052 0.43578128109988556 0.23760431966499274
+v -0.2265821371971911 0.9451948161848855 -0.09329939010631977
+v -1.0415982137637454 0.14056254785672886 -0.11063575619445715
+v -0.6309673232578825 0.5233429585504008 0.3154324448276982
+v -1.1084771244357725 0.5884126062022549 -0.1487300274151031
+v -0.8717572608392643 0.7961376346283748 0.21461994431182724
+v -0.7935503679930702 0.10381562175747021 0.18064150493670278
+v -0.1930045861227373 -0.016472567669853083 0.055588103122916455
+v -0.2993876552253006 -0.068558130299813 -0.05000292959714747
+v -0.6924979778477462 0.9668359505626445 -0.09270230189174977
+v -0.6058417937103333 -0.02638670731363664 -0.13330177816314193
+v -0.8799156176905546 0.05316924253091149 0.14030335805749
+v 0.03079873151815908 0.0802799299948706 -0.021596095982981622
+v -1.0641264046687584 0.5831225665734691 0.13240524374201096
+v -0.8975839327885125 -0.03171601038986818 0.03175023217440753
+v -0.7485534984930229 0.6251246158792332 -0.3277257829104002
+v -0.00044654153001363883 0.5196138546850763 -0.1291549211950357
+v -0.33772493966100053 -0.1132855735710503 -0.023160301600170685
+v -0.8608413160501167 0.9056034514194153 0.13558463504892249
+v -0.5605073386486454 0.8097792079852102 0.23967071303923726
+v -1.0822732334889182 0.5308882222561354 -0.19977503297159924
+v -0.7099585314967243 0.7597139791843466 -0.2813809762769149
+v -0.12525657743891688 0.4504680405145489 -0.23678194793554713
+v -0.02220599896631595 0.833916447723732 0.15362716581839286
+v -0.8027646816452443 0.9234334483485782 -0.11700518232667789
+v -0.4585150943223921 -0.13886381502531764 0.01077830594725282
+v -0.6444362119717503 -0.10480736805227411 -0.03115727318618422
+v -0.35715072635957984 1.0147167589258765 0.043519543542239525
+v -0.44655818297057626 -0.1278474340611117 0.03982371044300115
+v -0.12918631114482187 0.7174134161851835 -0.18865575303773596
+v -0.6564281019615854 0.9971038952658463 -0.054806541006044984
+v -0.7799593668382123 0.8282157236495682 -0.2272305331051926
+v -0.46496795109363115 1.0229147298990597 0.05495869788773361
+v -0.42250500513503464 -0.12738031165599628 -0.02216266906471931
+v -0.37388741816348675 -0.1155213358708857 -0.031046522038815194
+v -0.23330502706074513 -0.07836863229235402 -0.004294196279049811
+v -0.18514171450456227 0.9108464599628172 0.14969176329083134
+v -0.21508494550247922 0.33639531608209455 0.24700349005588523
+v -0.7370612161626663 0.9777753370574719 -0.03579474750828032
+v -0.9378244560335888 0.053457606680938075 -0.13553624092047073
+v -0.8428810322893587 0.41346073701448866 0.27404067758492584
+v -0.9907265435532993 0.399146286546847 -0.24757822471673163
+v -0.8503141543566188 0.38781613084587124 0.26586269707141263
+v -0.7556473978696131 0.417790726206639 0.28918464416774486
g foo
-f 78 4 18
-f 8 3 18
-f 21 23 70
-f 12 21 26
-f 12 21 63
-f 51 12 32
-f 51 12 63
-f 34 25 52
-f 16 25 2
-f 80 43 2
-f 80 43 32
-f 67 51 32
-f 44 78 4
-f 82 43 2
-f 82 61 2
-f 82 43 32
-f 82 12 32
-f 82 61 26
-f 82 12 26
-f 62 21 7
-f 53 8 3
-f 53 8 18
-f 35 23 38
-f 35 23 70
-f 35 21 70
-f 35 21 7
-f 30 3 18
-f 30 23 3
-f 57 23 3
-f 57 53 3
-f 46 6 20
-f 46 59 6
-f 46 29 20
-f 46 59 11
-f 46 29 11
-f 41 34 25
-f 13 25 2
-f 13 80 2
-f 13 80 25
-f 74 67 51
-f 84 44 78
-f 84 27 78
-f 55 80 25
-f 55 80 32
-f 55 67 32
-f 55 25 52
-f 60 6 4
-f 60 77 4
-f 64 77 4
-f 64 77 38
-f 64 4 18
-f 64 30 18
-f 64 23 38
-f 64 30 23
-f 56 21 26
-f 56 62 21
-f 45 17 11
-f 45 61 2
-f 19 59 11
-f 19 17 11
-f 28 61 26
-f 28 17 33
-f 28 45 61
-f 28 45 17
-f 28 56 26
-f 28 56 62
-f 76 69 58
-f 76 53 48
-f 49 51 63
-f 49 53 63
-f 49 76 58
-f 49 76 53
-f 42 35 7
-f 5 53 63
-f 5 57 53
-f 5 21 63
-f 5 21 23
-f 5 57 23
-f 22 59 6
-f 22 40 6
-f 22 40 73
-f 22 19 59
-f 39 16 25
-f 39 41 25
-f 39 16 2
-f 75 74 58
-f 75 74 51
-f 75 49 58
-f 75 49 51
-f 66 74 67
-f 66 55 67
-f 66 55 69
-f 66 69 58
-f 66 74 58
-f 31 27 69
-f 31 76 69
-f 31 76 48
-f 31 27 78
-f 31 78 18
-f 31 53 18
-f 31 53 48
-f 47 34 52
-f 47 27 52
-f 47 84 27
-f 14 55 52
-f 14 55 69
-f 14 27 52
-f 14 27 69
-f 36 60 77
-f 36 77 73
-f 36 40 73
-f 36 40 6
-f 36 60 6
-f 68 77 73
-f 68 77 38
-f 83 47 34
-f 83 47 84
-f 83 41 34
-f 83 41 20
-f 54 29 20
-f 54 29 11
-f 54 41 20
-f 54 39 41
-f 10 45 2
-f 10 39 2
-f 10 45 11
-f 10 54 11
-f 10 54 39
-f 79 65 71
-f 15 42 7
-f 15 68 38
-f 15 71 73
-f 15 68 73
-f 15 35 38
-f 15 42 35
-f 24 84 44
-f 24 6 20
-f 24 6 4
-f 24 44 4
-f 1 17 33
-f 1 65 33
-f 1 65 17
-f 37 15 71
-f 37 62 7
-f 37 15 7
-f 9 79 71
-f 9 22 19
-f 9 22 73
-f 9 71 73
-f 9 19 17
-f 9 65 17
-f 9 79 65
-f 81 83 84
-f 81 24 84
-f 81 83 20
-f 81 24 20
-f 50 65 71
-f 50 37 71
-f 72 37 62
-f 72 28 33
-f 72 28 62
-f 72 65 33
-f 72 50 65
-f 72 50 37
-g
+f 78 4 18
+f 8 3 18
+f 21 23 70
+f 12 21 26
+f 12 21 63
+f 51 12 32
+f 51 12 63
+f 34 25 52
+f 16 25 2
+f 80 43 2
+f 80 43 32
+f 67 51 32
+f 44 78 4
+f 82 43 2
+f 82 61 2
+f 82 43 32
+f 82 12 32
+f 82 61 26
+f 82 12 26
+f 62 21 7
+f 53 8 3
+f 53 8 18
+f 35 23 38
+f 35 23 70
+f 35 21 70
+f 35 21 7
+f 30 3 18
+f 30 23 3
+f 57 23 3
+f 57 53 3
+f 46 6 20
+f 46 59 6
+f 46 29 20
+f 46 59 11
+f 46 29 11
+f 41 34 25
+f 13 25 2
+f 13 80 2
+f 13 80 25
+f 74 67 51
+f 84 44 78
+f 84 27 78
+f 55 80 25
+f 55 80 32
+f 55 67 32
+f 55 25 52
+f 60 6 4
+f 60 77 4
+f 64 77 4
+f 64 77 38
+f 64 4 18
+f 64 30 18
+f 64 23 38
+f 64 30 23
+f 56 21 26
+f 56 62 21
+f 45 17 11
+f 45 61 2
+f 19 59 11
+f 19 17 11
+f 28 61 26
+f 28 17 33
+f 28 45 61
+f 28 45 17
+f 28 56 26
+f 28 56 62
+f 76 69 58
+f 76 53 48
+f 49 51 63
+f 49 53 63
+f 49 76 58
+f 49 76 53
+f 42 35 7
+f 5 53 63
+f 5 57 53
+f 5 21 63
+f 5 21 23
+f 5 57 23
+f 22 59 6
+f 22 40 6
+f 22 40 73
+f 22 19 59
+f 39 16 25
+f 39 41 25
+f 39 16 2
+f 75 74 58
+f 75 74 51
+f 75 49 58
+f 75 49 51
+f 66 74 67
+f 66 55 67
+f 66 55 69
+f 66 69 58
+f 66 74 58
+f 31 27 69
+f 31 76 69
+f 31 76 48
+f 31 27 78
+f 31 78 18
+f 31 53 18
+f 31 53 48
+f 47 34 52
+f 47 27 52
+f 47 84 27
+f 14 55 52
+f 14 55 69
+f 14 27 52
+f 14 27 69
+f 36 60 77
+f 36 77 73
+f 36 40 73
+f 36 40 6
+f 36 60 6
+f 68 77 73
+f 68 77 38
+f 83 47 34
+f 83 47 84
+f 83 41 34
+f 83 41 20
+f 54 29 20
+f 54 29 11
+f 54 41 20
+f 54 39 41
+f 10 45 2
+f 10 39 2
+f 10 45 11
+f 10 54 11
+f 10 54 39
+f 79 65 71
+f 15 42 7
+f 15 68 38
+f 15 71 73
+f 15 68 73
+f 15 35 38
+f 15 42 35
+f 24 84 44
+f 24 6 20
+f 24 6 4
+f 24 44 4
+f 1 17 33
+f 1 65 33
+f 1 65 17
+f 37 15 71
+f 37 62 7
+f 37 15 7
+f 9 79 71
+f 9 22 19
+f 9 22 73
+f 9 71 73
+f 9 19 17
+f 9 65 17
+f 9 79 65
+f 81 83 84
+f 81 24 84
+f 81 83 20
+f 81 24 20
+f 50 65 71
+f 50 37 71
+f 72 37 62
+f 72 28 33
+f 72 28 62
+f 72 65 33
+f 72 50 65
+f 72 50 37
+g
diff --git a/data/relative_effector_positions/anymal_LH_ADAPTER_TO_FOOT_constraints_in_RHleg.obj b/data/relative_effector_positions/anymal_LH_ADAPTER_TO_FOOT_constraints_in_RHleg.obj
index d5b37aae253804b5d0a1f2c4021750787c20c2ce..2f0c7a713d1be2497d3644d62b27a2702698aa14 100644
--- a/data/relative_effector_positions/anymal_LH_ADAPTER_TO_FOOT_constraints_in_RHleg.obj
+++ b/data/relative_effector_positions/anymal_LH_ADAPTER_TO_FOOT_constraints_in_RHleg.obj
@@ -1,232 +1,232 @@
-v 0.49035404684180456 0.7508195962621832 -0.20710932848648453
-v 0.4050652996290509 0.07293609784286129 -0.12279310282122347
-v -0.059837573410679246 -0.07307401305991121 -0.041467626588350126
-v -0.3816803507817759 0.5210425052683973 -0.29141478772394847
-v -0.2671638874526956 0.13646405505130157 -0.09697368272294998
-v 0.4033567764343186 0.24717180532654714 -0.17847729641757393
-v -0.31529992537834967 0.31841870267725 0.26384490637891994
-v 0.1488372687627699 1.0100810241314693 -0.011094219530157656
-v 0.22822334258956245 0.17844301125207426 -0.18890757124838142
-v -0.42664132037843583 0.9145354891046135 0.04562142027729832
-v -0.5278010851244931 0.638498209015532 -0.1529901858840942
-v 0.49660345793515714 0.9410445131089225 -0.0038339878707469577
-v -0.45803361714824553 0.5556808289860353 0.2326103838132796
-v 0.11248598618142125 1.0147799870296141 -0.019825014897939752
-v -0.040174155646179155 0.05090398757754408 0.11920278610054486
-v 0.41738591682364634 -0.05386742425731528 -0.03878944048706329
-v -0.43618428701021283 0.06578581917394626 0.032144105879301776
-v 0.4109389221684284 0.7290767140680336 0.24924489636790398
-v 0.309188855542628 0.9547270523662104 0.09869302295263493
-v -0.5378043885267283 0.7771566564281541 0.06042669951702301
-v 0.554350144323299 0.6297604805555409 -0.030773565541039327
-v -0.46397963173038503 0.8916526936415663 0.05389175832010479
-v 0.23640798757114578 0.47075554417758997 0.26194719424932844
-v -0.44622303966129984 0.3250073565628946 0.18196122334791986
-v -0.5197425249057676 0.4130780334761292 -0.07364695424451567
-v -0.2982113111161163 0.24945014692532655 -0.15880097177741637
-v -0.33038532238468554 0.22100370967903712 0.20141619453512882
-v 0.5176256864696188 0.07553947639194739 -0.029477483497242163
-v 0.5710689970134102 0.36256064664991683 0.08172486961183673
-v -0.28897876815534274 0.5927650503159021 0.2694363551239839
-v -0.12778045221120443 0.8997507803736604 -0.16548093091304256
-v 0.3199936085327011 0.7770579650315614 -0.24539295606910622
-v 0.5841723122525182 0.39017478692334395 0.005814312587710424
-v 0.3433599325948032 0.14132831784034222 0.12146417866474968
-v -0.20113686945609366 0.856749222435805 0.20068794345274976
-v -0.5133626771884913 0.6091519980938929 0.1701595377268259
-v 0.013042031736554632 -0.09964150494478768 0.04090025992898472
-v -0.4022181169078592 0.8008092007074127 0.20365922866779018
-v -0.22596974005833623 0.8755178267606942 -0.1695342011943427
-v -0.3379965067218512 0.36095135188899846 0.26239908473793705
-v -0.265623541727769 0.8666952888287187 0.1950215836170865
-v 0.23581837506841938 0.969980446109994 -0.11285308761424234
-v -0.577182165540969 0.6449183331649162 -0.1036570200604619
-v 0.30513696565361376 -0.07514048440638793 -0.036001927104926945
-v 0.27789663354135974 0.875809322203152 0.18640673472509095
-v -0.23163470133613664 0.7932175329564175 0.21926654911012927
-v 0.14873752234421445 -0.06224330716183318 -0.06499379042991699
-v -0.2526584529394196 0.8078633248157828 -0.2144429023938194
-v -0.3280823939175139 0.9684206364816764 0.0029364345199799358
-v 0.5308312703084884 0.5221170443493908 -0.14705424359046348
-v -0.18705839426998228 0.9703276432895905 0.06834223408864826
-v 0.455856139274934 0.4519063345115004 -0.21306289340360524
-v 0.5493283955824602 0.4968686422685934 -0.11908709640470505
-v 0.09747443980861421 0.997744643958862 0.0475342563066114
-v -0.1196707656175333 -0.003970011518543509 -0.06775007533122707
-v 0.5793886582092684 0.15935666250232042 -0.04519077861416765
-v -0.11654041448946251 0.45449690403793813 0.2737918652497615
-v -0.477114405674948 0.8838700695447552 0.05178016758617521
-v 0.5252886606795855 0.8107879640241918 0.07100084738689005
-v -0.23136306966307157 -0.048968914916677125 0.05457841264383645
-v -0.11521880077837346 0.9880757513474421 -0.065169217112505
-v 0.2772368476882204 0.43866792722173487 -0.3071436617443535
-v -0.12820422358979938 1.0147910941404015 0.00037987169618913796
-v -0.4486977367347964 0.8305337863091568 -0.08229829375180475
-v -0.1000640522700581 0.6133809256454366 0.2728713686954044
-v 0.35541155683324166 0.852499562691177 -0.1806914292768645
-v -0.1083097392495182 -0.07322920039086209 -0.011147487262492595
-v 0.2656389899141308 0.53799703317415 -0.28949939622814136
-v 0.5301088145487259 0.7721775041768428 -0.04796618125488927
-v -0.5848495626062704 0.5233491907382427 0.08585640643110859
-v 0.17785277901932994 -0.13327829982503808 0.013586926594875592
-v 0.510660527713293 0.4294475863796823 0.16219785810830878
-v -0.22677325840184198 0.9299041145375939 0.11844469766265003
-v 0.06307237777808383 0.9843540567848387 -0.11872265905235485
-v -0.4104153020047618 0.052855972033004794 0.0813691260195285
-v -0.2839673096358367 0.457394823903357 0.2774938083595907
-v 0.11856395790689961 -0.03186884203599763 -0.09597630464028356
-v 0.23338845193202024 0.6266459600533196 -0.2735374740236767
+v 0.49035404684180456 0.7508195962621832 -0.20710932848648453
+v 0.4050652996290509 0.07293609784286129 -0.12279310282122347
+v -0.059837573410679246 -0.07307401305991121 -0.041467626588350126
+v -0.3816803507817759 0.5210425052683973 -0.29141478772394847
+v -0.2671638874526956 0.13646405505130157 -0.09697368272294998
+v 0.4033567764343186 0.24717180532654714 -0.17847729641757393
+v -0.31529992537834967 0.31841870267725 0.26384490637891994
+v 0.1488372687627699 1.0100810241314693 -0.011094219530157656
+v 0.22822334258956245 0.17844301125207426 -0.18890757124838142
+v -0.42664132037843583 0.9145354891046135 0.04562142027729832
+v -0.5278010851244931 0.638498209015532 -0.1529901858840942
+v 0.49660345793515714 0.9410445131089225 -0.0038339878707469577
+v -0.45803361714824553 0.5556808289860353 0.2326103838132796
+v 0.11248598618142125 1.0147799870296141 -0.019825014897939752
+v -0.040174155646179155 0.05090398757754408 0.11920278610054486
+v 0.41738591682364634 -0.05386742425731528 -0.03878944048706329
+v -0.43618428701021283 0.06578581917394626 0.032144105879301776
+v 0.4109389221684284 0.7290767140680336 0.24924489636790398
+v 0.309188855542628 0.9547270523662104 0.09869302295263493
+v -0.5378043885267283 0.7771566564281541 0.06042669951702301
+v 0.554350144323299 0.6297604805555409 -0.030773565541039327
+v -0.46397963173038503 0.8916526936415663 0.05389175832010479
+v 0.23640798757114578 0.47075554417758997 0.26194719424932844
+v -0.44622303966129984 0.3250073565628946 0.18196122334791986
+v -0.5197425249057676 0.4130780334761292 -0.07364695424451567
+v -0.2982113111161163 0.24945014692532655 -0.15880097177741637
+v -0.33038532238468554 0.22100370967903712 0.20141619453512882
+v 0.5176256864696188 0.07553947639194739 -0.029477483497242163
+v 0.5710689970134102 0.36256064664991683 0.08172486961183673
+v -0.28897876815534274 0.5927650503159021 0.2694363551239839
+v -0.12778045221120443 0.8997507803736604 -0.16548093091304256
+v 0.3199936085327011 0.7770579650315614 -0.24539295606910622
+v 0.5841723122525182 0.39017478692334395 0.005814312587710424
+v 0.3433599325948032 0.14132831784034222 0.12146417866474968
+v -0.20113686945609366 0.856749222435805 0.20068794345274976
+v -0.5133626771884913 0.6091519980938929 0.1701595377268259
+v 0.013042031736554632 -0.09964150494478768 0.04090025992898472
+v -0.4022181169078592 0.8008092007074127 0.20365922866779018
+v -0.22596974005833623 0.8755178267606942 -0.1695342011943427
+v -0.3379965067218512 0.36095135188899846 0.26239908473793705
+v -0.265623541727769 0.8666952888287187 0.1950215836170865
+v 0.23581837506841938 0.969980446109994 -0.11285308761424234
+v -0.577182165540969 0.6449183331649162 -0.1036570200604619
+v 0.30513696565361376 -0.07514048440638793 -0.036001927104926945
+v 0.27789663354135974 0.875809322203152 0.18640673472509095
+v -0.23163470133613664 0.7932175329564175 0.21926654911012927
+v 0.14873752234421445 -0.06224330716183318 -0.06499379042991699
+v -0.2526584529394196 0.8078633248157828 -0.2144429023938194
+v -0.3280823939175139 0.9684206364816764 0.0029364345199799358
+v 0.5308312703084884 0.5221170443493908 -0.14705424359046348
+v -0.18705839426998228 0.9703276432895905 0.06834223408864826
+v 0.455856139274934 0.4519063345115004 -0.21306289340360524
+v 0.5493283955824602 0.4968686422685934 -0.11908709640470505
+v 0.09747443980861421 0.997744643958862 0.0475342563066114
+v -0.1196707656175333 -0.003970011518543509 -0.06775007533122707
+v 0.5793886582092684 0.15935666250232042 -0.04519077861416765
+v -0.11654041448946251 0.45449690403793813 0.2737918652497615
+v -0.477114405674948 0.8838700695447552 0.05178016758617521
+v 0.5252886606795855 0.8107879640241918 0.07100084738689005
+v -0.23136306966307157 -0.048968914916677125 0.05457841264383645
+v -0.11521880077837346 0.9880757513474421 -0.065169217112505
+v 0.2772368476882204 0.43866792722173487 -0.3071436617443535
+v -0.12820422358979938 1.0147910941404015 0.00037987169618913796
+v -0.4486977367347964 0.8305337863091568 -0.08229829375180475
+v -0.1000640522700581 0.6133809256454366 0.2728713686954044
+v 0.35541155683324166 0.852499562691177 -0.1806914292768645
+v -0.1083097392495182 -0.07322920039086209 -0.011147487262492595
+v 0.2656389899141308 0.53799703317415 -0.28949939622814136
+v 0.5301088145487259 0.7721775041768428 -0.04796618125488927
+v -0.5848495626062704 0.5233491907382427 0.08585640643110859
+v 0.17785277901932994 -0.13327829982503808 0.013586926594875592
+v 0.510660527713293 0.4294475863796823 0.16219785810830878
+v -0.22677325840184198 0.9299041145375939 0.11844469766265003
+v 0.06307237777808383 0.9843540567848387 -0.11872265905235485
+v -0.4104153020047618 0.052855972033004794 0.0813691260195285
+v -0.2839673096358367 0.457394823903357 0.2774938083595907
+v 0.11856395790689961 -0.03186884203599763 -0.09597630464028356
+v 0.23338845193202024 0.6266459600533196 -0.2735374740236767
g foo
-f 53 56 33
-f 52 1 62
-f 18 12 59
-f 2 52 56
-f 4 77 62
-f 15 34 71
-f 15 37 71
-f 72 34 23
-f 72 18 23
-f 72 18 59
-f 50 53 1
-f 50 52 1
-f 50 53 56
-f 50 52 56
-f 3 47 71
-f 3 47 77
-f 36 13 70
-f 36 13 38
-f 24 75 70
-f 24 13 70
-f 69 53 1
-f 69 12 1
-f 69 12 59
-f 9 77 62
-f 9 2 62
-f 9 2 77
-f 6 52 62
-f 6 2 62
-f 6 2 52
-f 16 2 77
-f 16 2 56
-f 16 34 71
-f 43 25 70
-f 43 4 25
-f 60 75 27
-f 29 56 33
-f 29 59 33
-f 29 72 59
-f 7 75 27
-f 7 24 75
-f 7 57 76
-f 7 57 23
-f 7 34 23
-f 7 15 34
-f 7 15 37
-f 7 60 27
-f 7 60 37
-f 30 13 38
-f 30 76 13
-f 21 53 33
-f 21 69 53
-f 21 59 33
-f 21 69 59
-f 44 47 77
-f 44 16 77
-f 44 47 71
-f 44 16 71
-f 74 63 14
-f 17 60 75
-f 17 75 70
-f 17 25 70
-f 17 4 25
-f 17 4 26
-f 20 43 70
-f 20 43 58
-f 20 36 70
-f 20 58 38
-f 20 36 38
-f 11 48 4
-f 11 43 4
-f 28 29 72
-f 28 72 34
-f 28 16 34
-f 28 16 56
-f 28 29 56
-f 22 58 38
-f 22 10 38
-f 22 10 58
-f 49 10 58
-f 19 8 12
-f 19 18 12
-f 19 45 18
-f 65 57 76
-f 65 30 76
-f 65 18 23
-f 65 57 23
-f 40 76 13
-f 40 7 76
-f 40 24 13
-f 40 7 24
-f 68 4 62
-f 68 78 4
-f 32 68 78
-f 32 48 4
-f 32 78 4
-f 32 1 62
-f 32 68 62
-f 42 74 14
-f 42 8 14
-f 42 8 12
-f 42 32 74
-f 67 17 3
-f 67 17 60
-f 67 3 71
-f 67 37 71
-f 67 60 37
-f 55 17 3
-f 55 4 26
-f 55 4 77
-f 55 3 77
-f 41 19 45
-f 41 10 38
-f 66 32 1
-f 66 42 32
-f 66 12 1
-f 66 42 12
-f 5 17 26
-f 5 55 26
-f 5 55 17
-f 64 49 58
-f 64 39 49
-f 64 43 58
-f 64 11 43
-f 64 11 48
-f 64 39 48
-f 54 41 19
-f 54 19 8
-f 54 63 14
-f 54 8 14
-f 35 45 18
-f 35 41 45
-f 46 65 30
-f 46 30 38
-f 46 41 38
-f 46 35 41
-f 46 65 18
-f 46 35 18
-f 31 39 48
-f 31 39 74
-f 31 32 48
-f 31 32 74
-f 61 39 74
-f 61 39 49
-f 61 74 63
-f 61 49 63
-f 51 54 63
-f 51 49 63
-f 73 54 41
-f 73 51 54
-f 73 41 10
-f 73 49 10
-f 73 51 49
-g
+f 53 56 33
+f 52 1 62
+f 18 12 59
+f 2 52 56
+f 4 77 62
+f 15 34 71
+f 15 37 71
+f 72 34 23
+f 72 18 23
+f 72 18 59
+f 50 53 1
+f 50 52 1
+f 50 53 56
+f 50 52 56
+f 3 47 71
+f 3 47 77
+f 36 13 70
+f 36 13 38
+f 24 75 70
+f 24 13 70
+f 69 53 1
+f 69 12 1
+f 69 12 59
+f 9 77 62
+f 9 2 62
+f 9 2 77
+f 6 52 62
+f 6 2 62
+f 6 2 52
+f 16 2 77
+f 16 2 56
+f 16 34 71
+f 43 25 70
+f 43 4 25
+f 60 75 27
+f 29 56 33
+f 29 59 33
+f 29 72 59
+f 7 75 27
+f 7 24 75
+f 7 57 76
+f 7 57 23
+f 7 34 23
+f 7 15 34
+f 7 15 37
+f 7 60 27
+f 7 60 37
+f 30 13 38
+f 30 76 13
+f 21 53 33
+f 21 69 53
+f 21 59 33
+f 21 69 59
+f 44 47 77
+f 44 16 77
+f 44 47 71
+f 44 16 71
+f 74 63 14
+f 17 60 75
+f 17 75 70
+f 17 25 70
+f 17 4 25
+f 17 4 26
+f 20 43 70
+f 20 43 58
+f 20 36 70
+f 20 58 38
+f 20 36 38
+f 11 48 4
+f 11 43 4
+f 28 29 72
+f 28 72 34
+f 28 16 34
+f 28 16 56
+f 28 29 56
+f 22 58 38
+f 22 10 38
+f 22 10 58
+f 49 10 58
+f 19 8 12
+f 19 18 12
+f 19 45 18
+f 65 57 76
+f 65 30 76
+f 65 18 23
+f 65 57 23
+f 40 76 13
+f 40 7 76
+f 40 24 13
+f 40 7 24
+f 68 4 62
+f 68 78 4
+f 32 68 78
+f 32 48 4
+f 32 78 4
+f 32 1 62
+f 32 68 62
+f 42 74 14
+f 42 8 14
+f 42 8 12
+f 42 32 74
+f 67 17 3
+f 67 17 60
+f 67 3 71
+f 67 37 71
+f 67 60 37
+f 55 17 3
+f 55 4 26
+f 55 4 77
+f 55 3 77
+f 41 19 45
+f 41 10 38
+f 66 32 1
+f 66 42 32
+f 66 12 1
+f 66 42 12
+f 5 17 26
+f 5 55 26
+f 5 55 17
+f 64 49 58
+f 64 39 49
+f 64 43 58
+f 64 11 43
+f 64 11 48
+f 64 39 48
+f 54 41 19
+f 54 19 8
+f 54 63 14
+f 54 8 14
+f 35 45 18
+f 35 41 45
+f 46 65 30
+f 46 30 38
+f 46 41 38
+f 46 35 41
+f 46 65 18
+f 46 35 18
+f 31 39 48
+f 31 39 74
+f 31 32 48
+f 31 32 74
+f 61 39 74
+f 61 39 49
+f 61 74 63
+f 61 49 63
+f 51 54 63
+f 51 49 63
+f 73 54 41
+f 73 51 54
+f 73 41 10
+f 73 49 10
+f 73 51 49
+g
diff --git a/data/relative_effector_positions/anymal_RF_ADAPTER_TO_FOOT_constraints_in_LFleg.obj b/data/relative_effector_positions/anymal_RF_ADAPTER_TO_FOOT_constraints_in_LFleg.obj
index 4637a0a07cabe313cd4ff51596c0324f9c7a5214..e534ad0938012b9e62e6409299dddb2598889af5 100644
--- a/data/relative_effector_positions/anymal_RF_ADAPTER_TO_FOOT_constraints_in_LFleg.obj
+++ b/data/relative_effector_positions/anymal_RF_ADAPTER_TO_FOOT_constraints_in_LFleg.obj
@@ -1,223 +1,223 @@
-v 0.25371647695714983 -0.6799748787759159 0.3428533269427781
-v -0.29020573392093846 -0.4545565255836158 0.24521987513475973
-v 0.4423121034011809 -0.23719421623788245 0.23489987589073458
-v 0.4215051155125492 -0.9306048887493676 -0.0456912479311572
-v -0.3126383655419672 -0.6403380339063044 0.23865624924444484
-v -0.61316825014685 -0.3664100700552299 -0.11019183893255691
-v 0.22089366768258656 -0.9994067753315382 0.07643021005041595
-v -0.5989754733635821 -0.41801385159116095 0.09416438794709481
-v 0.6174217129315323 -0.7320992050784687 0.11660061080704831
-v -0.34053836469221843 -0.9344305148683328 -0.12672019136402968
-v -0.3600430314368503 -0.6192025213711285 -0.3155360997830766
-v -0.5241878224481961 -0.23645301588769227 -0.18999445004477328
-v -0.20540463359376865 -0.9862352993580824 -0.06860222345051709
-v 0.48375904489711824 -0.8999870151081437 0.10776015398792232
-v 0.5371930152583232 -0.22269417285423693 -0.02045675873758812
-v 0.3918878231502397 -0.027465961810078113 0.0016181923535628084
-v -0.40915831185998935 -0.03297366281328307 -0.07700244997159622
-v 0.336897812780938 -0.9751425242103723 -0.05317926933888001
-v 0.274278084009981 -0.9440397354068651 -0.11561985961273624
-v -0.41755813145918597 -0.05460615743586433 0.009793124373953577
-v -0.36250803429999845 -0.8383621762129823 0.20284968499796352
-v -0.41051462662911037 -0.3882140508065027 -0.30819988896512646
-v -0.03020023981258907 -0.9629316886563344 -0.13987835188864456
-v -0.5743109629768014 -0.22407914471146836 -0.09092520636830537
-v 0.1778384644806908 0.10239833199086085 0.0004956392695580902
-v 0.21912888125563373 -1.0239733929908685 -0.06587135229089303
-v -0.5684770249856574 -0.6564159916089021 0.12680307392519152
-v 0.27096335587156894 -0.3911301733105495 -0.22948223792472489
-v -0.04961377547430673 -1.0419146556852357 0.0478136448525559
-v -0.43601024145618517 -0.8896948788135565 0.12294641199285317
-v -0.0545881602160318 0.10646731198268644 0.10017866000678238
-v 0.4438834989797132 -0.6263888465317633 0.32678270919827485
-v 0.1099633438699708 -1.0262462673115818 -0.054165787698413626
-v 0.3254668360846248 -0.2521348306949759 0.27523566281656087
-v 0.27134384933032885 -1.0119464194995222 -0.010109746363874461
-v 0.25009797893305613 -0.37595849056572495 0.3001850012727912
-v 0.43089001372482577 -0.15333849070567118 0.20243695388536953
-v -0.47040947945888256 -0.6777023673102476 0.18331117352485116
-v 0.23624564782887847 0.07473138531298552 0.12715804180322737
-v -0.22833332887557367 0.08424620890667525 -0.07158777056452631
-v 0.5278412382939461 -0.8312467418851792 -0.09364010559580405
-v 0.5960052675990208 -0.2118193120460683 0.11503062446742218
-v -0.6466914797378791 -0.6294322388813847 -0.1611989959856323
-v 0.3912613945253691 -0.36093161706826954 -0.20032652343654378
-v 0.2768972416012521 0.06025881977240541 -0.0341237682960609
-v 0.1449270179947676 -0.8862963948777918 0.218664024706125
-v -0.574987346047534 -0.7017131290595637 0.11551276667772015
-v 0.3320173501822831 -0.981528759477555 -0.006191029385312352
-v -0.04414446859499627 -0.018904634462761902 -0.13811550145868756
-v 0.3126434917175691 -0.5888975611161336 -0.25852067323745165
-v -0.25804744863885476 -0.7737451994017154 -0.31617049760513327
-v -0.07725048665876016 0.11849335600540556 0.0708652813862698
-v -0.23068828433193012 -0.586377342568983 -0.3405151127150782
-v -0.49007430073053226 -0.3539033888496617 0.1537118419221717
-v 0.6350878496850334 -0.4320894586568175 -0.08972668120887006
-v -0.24765359116593877 -0.4764808254687489 -0.34069430688615054
-v -0.30795028791982876 -0.33744288676172074 -0.30671608945810586
-v -0.3445481865768727 -0.8420167053412856 -0.24112741895288564
-v -0.561763644248093 -0.8550269594908949 -0.12920156583351156
-v 0.5641509660218031 -0.5266078005221415 -0.1299628977194905
-v 0.6232446643217986 -0.4058485800957602 0.2315105211889302
-v 0.3502974868603396 -0.30664724303348445 0.2968670621138706
-v -0.30232762042498135 -0.2562038647208614 -0.2781981130672706
-v 0.42692518656199974 -0.7693250195607457 0.3041170661249278
-v -0.520102320485851 -0.8267905675090668 0.05405224826619465
-v -0.4096745712315625 -0.15958176808256141 -0.2334377255697942
-v 0.5478130716873577 -0.8116993878824705 0.1540750217752953
-v -0.42525819492000216 -0.4222404686243919 -0.3097750783690063
-v 0.45803672040374976 -0.7791665371004801 -0.14174636000730362
-v -0.141431925481868 0.1229584587579747 -0.06541965113978288
-v -0.3383782430551372 -0.9914309840347261 0.09236619146460462
-v -0.016657211453283527 -1.023590186759229 -0.024038672388621174
-v -0.373846434625112 -0.04809248987858458 0.11223272750799007
-v 0.37973898640408277 -0.9249358587269481 0.15998527975194204
-v 0.3927156168080518 -0.6175336795473476 -0.24137912033908682
+v 0.25371647695714983 -0.6799748787759159 0.3428533269427781
+v -0.29020573392093846 -0.4545565255836158 0.24521987513475973
+v 0.4423121034011809 -0.23719421623788245 0.23489987589073458
+v 0.4215051155125492 -0.9306048887493676 -0.0456912479311572
+v -0.3126383655419672 -0.6403380339063044 0.23865624924444484
+v -0.61316825014685 -0.3664100700552299 -0.11019183893255691
+v 0.22089366768258656 -0.9994067753315382 0.07643021005041595
+v -0.5989754733635821 -0.41801385159116095 0.09416438794709481
+v 0.6174217129315323 -0.7320992050784687 0.11660061080704831
+v -0.34053836469221843 -0.9344305148683328 -0.12672019136402968
+v -0.3600430314368503 -0.6192025213711285 -0.3155360997830766
+v -0.5241878224481961 -0.23645301588769227 -0.18999445004477328
+v -0.20540463359376865 -0.9862352993580824 -0.06860222345051709
+v 0.48375904489711824 -0.8999870151081437 0.10776015398792232
+v 0.5371930152583232 -0.22269417285423693 -0.02045675873758812
+v 0.3918878231502397 -0.027465961810078113 0.0016181923535628084
+v -0.40915831185998935 -0.03297366281328307 -0.07700244997159622
+v 0.336897812780938 -0.9751425242103723 -0.05317926933888001
+v 0.274278084009981 -0.9440397354068651 -0.11561985961273624
+v -0.41755813145918597 -0.05460615743586433 0.009793124373953577
+v -0.36250803429999845 -0.8383621762129823 0.20284968499796352
+v -0.41051462662911037 -0.3882140508065027 -0.30819988896512646
+v -0.03020023981258907 -0.9629316886563344 -0.13987835188864456
+v -0.5743109629768014 -0.22407914471146836 -0.09092520636830537
+v 0.1778384644806908 0.10239833199086085 0.0004956392695580902
+v 0.21912888125563373 -1.0239733929908685 -0.06587135229089303
+v -0.5684770249856574 -0.6564159916089021 0.12680307392519152
+v 0.27096335587156894 -0.3911301733105495 -0.22948223792472489
+v -0.04961377547430673 -1.0419146556852357 0.0478136448525559
+v -0.43601024145618517 -0.8896948788135565 0.12294641199285317
+v -0.0545881602160318 0.10646731198268644 0.10017866000678238
+v 0.4438834989797132 -0.6263888465317633 0.32678270919827485
+v 0.1099633438699708 -1.0262462673115818 -0.054165787698413626
+v 0.3254668360846248 -0.2521348306949759 0.27523566281656087
+v 0.27134384933032885 -1.0119464194995222 -0.010109746363874461
+v 0.25009797893305613 -0.37595849056572495 0.3001850012727912
+v 0.43089001372482577 -0.15333849070567118 0.20243695388536953
+v -0.47040947945888256 -0.6777023673102476 0.18331117352485116
+v 0.23624564782887847 0.07473138531298552 0.12715804180322737
+v -0.22833332887557367 0.08424620890667525 -0.07158777056452631
+v 0.5278412382939461 -0.8312467418851792 -0.09364010559580405
+v 0.5960052675990208 -0.2118193120460683 0.11503062446742218
+v -0.6466914797378791 -0.6294322388813847 -0.1611989959856323
+v 0.3912613945253691 -0.36093161706826954 -0.20032652343654378
+v 0.2768972416012521 0.06025881977240541 -0.0341237682960609
+v 0.1449270179947676 -0.8862963948777918 0.218664024706125
+v -0.574987346047534 -0.7017131290595637 0.11551276667772015
+v 0.3320173501822831 -0.981528759477555 -0.006191029385312352
+v -0.04414446859499627 -0.018904634462761902 -0.13811550145868756
+v 0.3126434917175691 -0.5888975611161336 -0.25852067323745165
+v -0.25804744863885476 -0.7737451994017154 -0.31617049760513327
+v -0.07725048665876016 0.11849335600540556 0.0708652813862698
+v -0.23068828433193012 -0.586377342568983 -0.3405151127150782
+v -0.49007430073053226 -0.3539033888496617 0.1537118419221717
+v 0.6350878496850334 -0.4320894586568175 -0.08972668120887006
+v -0.24765359116593877 -0.4764808254687489 -0.34069430688615054
+v -0.30795028791982876 -0.33744288676172074 -0.30671608945810586
+v -0.3445481865768727 -0.8420167053412856 -0.24112741895288564
+v -0.561763644248093 -0.8550269594908949 -0.12920156583351156
+v 0.5641509660218031 -0.5266078005221415 -0.1299628977194905
+v 0.6232446643217986 -0.4058485800957602 0.2315105211889302
+v 0.3502974868603396 -0.30664724303348445 0.2968670621138706
+v -0.30232762042498135 -0.2562038647208614 -0.2781981130672706
+v 0.42692518656199974 -0.7693250195607457 0.3041170661249278
+v -0.520102320485851 -0.8267905675090668 0.05405224826619465
+v -0.4096745712315625 -0.15958176808256141 -0.2334377255697942
+v 0.5478130716873577 -0.8116993878824705 0.1540750217752953
+v -0.42525819492000216 -0.4222404686243919 -0.3097750783690063
+v 0.45803672040374976 -0.7791665371004801 -0.14174636000730362
+v -0.141431925481868 0.1229584587579747 -0.06541965113978288
+v -0.3383782430551372 -0.9914309840347261 0.09236619146460462
+v -0.016657211453283527 -1.023590186759229 -0.024038672388621174
+v -0.373846434625112 -0.04809248987858458 0.11223272750799007
+v 0.37973898640408277 -0.9249358587269481 0.15998527975194204
+v 0.3927156168080518 -0.6175336795473476 -0.24137912033908682
g foo
-f 45 44 55
-f 12 68 43
-f 25 45 70
-f 25 39 45
-f 9 41 55
-f 9 61 55
-f 19 51 26
-f 60 41 55
-f 64 9 67
-f 64 9 61
-f 42 61 55
-f 42 61 37
-f 42 39 37
-f 22 68 56
-f 22 12 66
-f 22 12 68
-f 11 68 43
-f 8 73 54
-f 17 12 66
-f 59 10 71
-f 59 30 71
-f 59 30 65
-f 59 58 10
-f 59 58 51
-f 59 11 43
-f 59 11 51
-f 75 19 51
-f 75 60 41
-f 75 44 55
-f 75 60 55
-f 49 45 44
-f 49 45 70
-f 49 66 70
-f 62 36 1
-f 2 36 1
-f 2 62 73
-f 2 62 36
-f 2 73 54
-f 2 38 54
-f 47 30 65
-f 47 8 43
-f 47 59 43
-f 47 59 65
-f 21 47 38
-f 21 47 30
-f 21 30 71
-f 46 64 74
-f 46 29 71
-f 46 29 74
-f 46 21 71
-f 46 64 1
-f 46 21 1
-f 15 45 55
-f 15 42 55
-f 16 39 45
-f 16 42 39
-f 16 15 45
-f 16 15 42
-f 7 29 74
-f 33 29 26
-f 13 10 71
-f 13 29 71
-f 18 19 26
-f 18 19 41
-f 4 18 41
-f 14 64 67
-f 14 64 74
-f 14 4 41
-f 14 9 67
-f 14 9 41
-f 53 68 56
-f 53 11 68
-f 53 11 51
-f 6 12 43
-f 6 8 43
-f 40 66 70
-f 40 17 66
-f 69 19 41
-f 69 75 41
-f 69 75 19
-f 63 49 44
-f 63 49 66
-f 3 61 37
-f 3 62 37
-f 3 62 61
-f 32 64 61
-f 32 62 61
-f 32 64 1
-f 32 62 1
-f 34 31 73
-f 34 62 73
-f 34 31 39
-f 34 39 37
-f 34 62 37
-f 27 47 38
-f 27 47 8
-f 27 38 54
-f 27 8 54
-f 5 2 38
-f 5 21 38
-f 5 2 1
-f 5 21 1
-f 23 58 10
-f 23 13 10
-f 23 13 33
-f 23 33 26
-f 23 51 26
-f 23 58 51
-f 72 33 29
-f 72 13 29
-f 72 13 33
-f 35 7 74
-f 35 14 74
-f 35 29 26
-f 35 7 29
-f 35 18 26
-f 50 75 51
-f 50 53 51
-f 50 53 56
-f 50 28 56
-f 50 28 44
-f 50 75 44
-f 24 6 8
-f 24 8 73
-f 24 17 12
-f 24 6 12
-f 52 31 73
-f 52 40 73
-f 52 40 70
-f 52 25 70
-f 52 25 39
-f 52 31 39
-f 57 63 44
-f 57 28 56
-f 57 28 44
-f 57 22 56
-f 57 22 66
-f 57 63 66
-f 48 14 4
-f 48 35 14
-f 48 4 18
-f 48 35 18
-f 20 24 73
-f 20 24 17
-f 20 40 73
-f 20 40 17
-g
+f 45 44 55
+f 12 68 43
+f 25 45 70
+f 25 39 45
+f 9 41 55
+f 9 61 55
+f 19 51 26
+f 60 41 55
+f 64 9 67
+f 64 9 61
+f 42 61 55
+f 42 61 37
+f 42 39 37
+f 22 68 56
+f 22 12 66
+f 22 12 68
+f 11 68 43
+f 8 73 54
+f 17 12 66
+f 59 10 71
+f 59 30 71
+f 59 30 65
+f 59 58 10
+f 59 58 51
+f 59 11 43
+f 59 11 51
+f 75 19 51
+f 75 60 41
+f 75 44 55
+f 75 60 55
+f 49 45 44
+f 49 45 70
+f 49 66 70
+f 62 36 1
+f 2 36 1
+f 2 62 73
+f 2 62 36
+f 2 73 54
+f 2 38 54
+f 47 30 65
+f 47 8 43
+f 47 59 43
+f 47 59 65
+f 21 47 38
+f 21 47 30
+f 21 30 71
+f 46 64 74
+f 46 29 71
+f 46 29 74
+f 46 21 71
+f 46 64 1
+f 46 21 1
+f 15 45 55
+f 15 42 55
+f 16 39 45
+f 16 42 39
+f 16 15 45
+f 16 15 42
+f 7 29 74
+f 33 29 26
+f 13 10 71
+f 13 29 71
+f 18 19 26
+f 18 19 41
+f 4 18 41
+f 14 64 67
+f 14 64 74
+f 14 4 41
+f 14 9 67
+f 14 9 41
+f 53 68 56
+f 53 11 68
+f 53 11 51
+f 6 12 43
+f 6 8 43
+f 40 66 70
+f 40 17 66
+f 69 19 41
+f 69 75 41
+f 69 75 19
+f 63 49 44
+f 63 49 66
+f 3 61 37
+f 3 62 37
+f 3 62 61
+f 32 64 61
+f 32 62 61
+f 32 64 1
+f 32 62 1
+f 34 31 73
+f 34 62 73
+f 34 31 39
+f 34 39 37
+f 34 62 37
+f 27 47 38
+f 27 47 8
+f 27 38 54
+f 27 8 54
+f 5 2 38
+f 5 21 38
+f 5 2 1
+f 5 21 1
+f 23 58 10
+f 23 13 10
+f 23 13 33
+f 23 33 26
+f 23 51 26
+f 23 58 51
+f 72 33 29
+f 72 13 29
+f 72 13 33
+f 35 7 74
+f 35 14 74
+f 35 29 26
+f 35 7 29
+f 35 18 26
+f 50 75 51
+f 50 53 51
+f 50 53 56
+f 50 28 56
+f 50 28 44
+f 50 75 44
+f 24 6 8
+f 24 8 73
+f 24 17 12
+f 24 6 12
+f 52 31 73
+f 52 40 73
+f 52 40 70
+f 52 25 70
+f 52 25 39
+f 52 31 39
+f 57 63 44
+f 57 28 56
+f 57 28 44
+f 57 22 56
+f 57 22 66
+f 57 63 66
+f 48 14 4
+f 48 35 14
+f 48 4 18
+f 48 35 18
+f 20 24 73
+f 20 24 17
+f 20 40 73
+f 20 40 17
+g
diff --git a/data/relative_effector_positions/anymal_RF_ADAPTER_TO_FOOT_constraints_in_LHleg.obj b/data/relative_effector_positions/anymal_RF_ADAPTER_TO_FOOT_constraints_in_LHleg.obj
index c69ce09925ca14f6c537dcbc9c533609d7d11aa7..825586942c1338bf8059df4aeb1ecb3e9d53a33d 100644
--- a/data/relative_effector_positions/anymal_RF_ADAPTER_TO_FOOT_constraints_in_LHleg.obj
+++ b/data/relative_effector_positions/anymal_RF_ADAPTER_TO_FOOT_constraints_in_LHleg.obj
@@ -1,250 +1,250 @@
-v 0.9405757361672906 -0.8616709792727616 0.12767764753906743
-v 1.1240514042041592 -0.14112301160674312 0.04216637712125565
-v -0.0421115032636957 -0.47786623497445113 0.04286973445164116
-v 0.06429912844013033 -0.6405177883092998 -0.2589738425172482
-v 0.034782600518695514 -0.4615542232932315 0.18863347347633774
-v 0.7278064932288637 -0.7669784142229005 -0.27356999736946325
-v 0.23842506095580374 -0.8801725928826065 0.16108289236873197
-v -0.06697442644417703 -0.34780705738001494 -0.06853315485595646
-v 0.8143268884886785 -0.9773742454590657 -0.041876964290914254
-v 1.091873926733319 -0.6365353392464423 -0.05574685199000878
-v 1.080228461477513 -0.7926153260301343 -0.026967577869018988
-v 0.8106468583148366 -0.374066961428389 0.32684506616040543
-v 1.0894906211517597 -0.07826072620517467 -0.002214919197831733
-v 0.6100043381507293 0.07075142495213126 -0.07210387414650965
-v 0.3947835146937222 -1.0199177761172908 0.04194748398418657
-v 1.1100386019325565 -0.14501922006841017 0.008082807132256953
-v 1.0588015736763814 -0.8309529422572521 0.10000290017317137
-v -0.08252526014887376 -0.5307512907657159 -0.1865755878823704
-v 0.9461727107226728 -0.8926844628427666 -0.06779045476446466
-v 0.8696675959436577 -0.6146032831947359 -0.30808479662435523
-v 0.17550185013634886 -0.6071575543183397 0.2820633172400618
-v 0.8145679302832763 -0.9487502754268866 -0.10713984858903991
-v -0.012402136297563732 -0.7648849998394194 0.05112919109904346
-v 0.7936182618448755 -0.5646763264932246 -0.319730891896664
-v 1.064620934930934 -0.038921249848529654 -0.04274125538366125
-v 0.8210560601953649 -0.5114797226693114 0.3499785265463613
-v 0.6057788370351336 0.006782098637873241 -0.13805315496163262
-v 1.000336386460533 -0.6703049848416837 0.2371294588209189
-v 0.9094123909267458 -0.706137826046562 -0.23260128386883316
-v -0.039825285740750616 -0.7120347479852323 -0.025245083171578087
-v 0.24021693908235867 0.002979059748146995 -0.11513803764678077
-v 0.8539043935243397 -0.040660062816138404 0.18595094308930182
-v 0.8644674674667996 -0.8227313976097552 0.2024601564878642
-v 0.9183786087937096 -0.10213151077674762 -0.1542784418486095
-v 0.12380887966989113 -0.944281767664171 0.063013063792205
-v 0.5279172199683972 -0.9140843508198795 -0.16613271274882613
-v 0.5757273286663042 -0.9141649884213497 0.17434029616975277
-v 0.09944023038027339 -0.9747734604881828 -0.017981121342632356
-v 1.1011671114305914 -0.36678125244599996 -0.07991646496763588
-v 0.6073976341169258 -0.9597650482836105 -0.12043026671171819
-v 0.9356203957448052 -0.43578128109988556 -0.23760431966499274
-v 0.2265821371971911 -0.9451948161848855 0.09329939010631977
-v 1.0415982137637454 -0.14056254785672886 0.11063575619445715
-v 0.6309673232578825 -0.5233429585504008 -0.3154324448276982
-v 1.1084771244357725 -0.5884126062022549 0.1487300274151031
-v 0.8717572608392643 -0.7961376346283748 -0.21461994431182724
-v 0.7935503679930702 -0.10381562175747021 -0.18064150493670278
-v 0.1930045861227373 0.016472567669853083 -0.055588103122916455
-v 0.2993876552253006 0.068558130299813 0.05000292959714747
-v 0.6924979778477462 -0.9668359505626445 0.09270230189174977
-v 0.6058417937103333 0.02638670731363664 0.13330177816314193
-v 0.8799156176905546 -0.05316924253091149 -0.14030335805749
-v -0.03079873151815908 -0.0802799299948706 0.021596095982981622
-v 1.0641264046687584 -0.5831225665734691 -0.13240524374201096
-v 0.8975839327885125 0.03171601038986818 -0.03175023217440753
-v 0.7485534984930229 -0.6251246158792332 0.3277257829104002
-v 0.00044654153001363883 -0.5196138546850763 0.1291549211950357
-v 0.33772493966100053 0.1132855735710503 0.023160301600170685
-v 0.8608413160501167 -0.9056034514194153 -0.13558463504892249
-v 0.5605073386486454 -0.8097792079852102 -0.23967071303923726
-v 1.0822732334889182 -0.5308882222561354 0.19977503297159924
-v 0.7099585314967243 -0.7597139791843466 0.2813809762769149
-v 0.12525657743891688 -0.4504680405145489 0.23678194793554713
-v 0.02220599896631595 -0.833916447723732 -0.15362716581839286
-v 0.8027646816452443 -0.9234334483485782 0.11700518232667789
-v 0.4585150943223921 0.13886381502531764 -0.01077830594725282
-v 0.6444362119717503 0.10480736805227411 0.03115727318618422
-v 0.35715072635957984 -1.0147167589258765 -0.043519543542239525
-v 0.44655818297057626 0.1278474340611117 -0.03982371044300115
-v 0.12918631114482187 -0.7174134161851835 0.18865575303773596
-v 0.6564281019615854 -0.9971038952658463 0.054806541006044984
-v 0.7799593668382123 -0.8282157236495682 0.2272305331051926
-v 0.46496795109363115 -1.0229147298990597 -0.05495869788773361
-v 0.42250500513503464 0.12738031165599628 0.02216266906471931
-v 0.37388741816348675 0.1155213358708857 0.031046522038815194
-v 0.23330502706074513 0.07836863229235402 0.004294196279049811
-v 0.18514171450456227 -0.9108464599628172 -0.14969176329083134
-v 0.21508494550247922 -0.33639531608209455 -0.24700349005588523
-v 0.7370612161626663 -0.9777753370574719 0.03579474750828032
-v 0.9378244560335888 -0.053457606680938075 0.13553624092047073
-v 0.8428810322893587 -0.41346073701448866 -0.27404067758492584
-v 0.9907265435532993 -0.399146286546847 0.24757822471673163
-v 0.8503141543566188 -0.38781613084587124 -0.26586269707141263
-v 0.7556473978696131 -0.417790726206639 -0.28918464416774486
+v 0.9405757361672906 -0.8616709792727616 0.12767764753906743
+v 1.1240514042041592 -0.14112301160674312 0.04216637712125565
+v -0.0421115032636957 -0.47786623497445113 0.04286973445164116
+v 0.06429912844013033 -0.6405177883092998 -0.2589738425172482
+v 0.034782600518695514 -0.4615542232932315 0.18863347347633774
+v 0.7278064932288637 -0.7669784142229005 -0.27356999736946325
+v 0.23842506095580374 -0.8801725928826065 0.16108289236873197
+v -0.06697442644417703 -0.34780705738001494 -0.06853315485595646
+v 0.8143268884886785 -0.9773742454590657 -0.041876964290914254
+v 1.091873926733319 -0.6365353392464423 -0.05574685199000878
+v 1.080228461477513 -0.7926153260301343 -0.026967577869018988
+v 0.8106468583148366 -0.374066961428389 0.32684506616040543
+v 1.0894906211517597 -0.07826072620517467 -0.002214919197831733
+v 0.6100043381507293 0.07075142495213126 -0.07210387414650965
+v 0.3947835146937222 -1.0199177761172908 0.04194748398418657
+v 1.1100386019325565 -0.14501922006841017 0.008082807132256953
+v 1.0588015736763814 -0.8309529422572521 0.10000290017317137
+v -0.08252526014887376 -0.5307512907657159 -0.1865755878823704
+v 0.9461727107226728 -0.8926844628427666 -0.06779045476446466
+v 0.8696675959436577 -0.6146032831947359 -0.30808479662435523
+v 0.17550185013634886 -0.6071575543183397 0.2820633172400618
+v 0.8145679302832763 -0.9487502754268866 -0.10713984858903991
+v -0.012402136297563732 -0.7648849998394194 0.05112919109904346
+v 0.7936182618448755 -0.5646763264932246 -0.319730891896664
+v 1.064620934930934 -0.038921249848529654 -0.04274125538366125
+v 0.8210560601953649 -0.5114797226693114 0.3499785265463613
+v 0.6057788370351336 0.006782098637873241 -0.13805315496163262
+v 1.000336386460533 -0.6703049848416837 0.2371294588209189
+v 0.9094123909267458 -0.706137826046562 -0.23260128386883316
+v -0.039825285740750616 -0.7120347479852323 -0.025245083171578087
+v 0.24021693908235867 0.002979059748146995 -0.11513803764678077
+v 0.8539043935243397 -0.040660062816138404 0.18595094308930182
+v 0.8644674674667996 -0.8227313976097552 0.2024601564878642
+v 0.9183786087937096 -0.10213151077674762 -0.1542784418486095
+v 0.12380887966989113 -0.944281767664171 0.063013063792205
+v 0.5279172199683972 -0.9140843508198795 -0.16613271274882613
+v 0.5757273286663042 -0.9141649884213497 0.17434029616975277
+v 0.09944023038027339 -0.9747734604881828 -0.017981121342632356
+v 1.1011671114305914 -0.36678125244599996 -0.07991646496763588
+v 0.6073976341169258 -0.9597650482836105 -0.12043026671171819
+v 0.9356203957448052 -0.43578128109988556 -0.23760431966499274
+v 0.2265821371971911 -0.9451948161848855 0.09329939010631977
+v 1.0415982137637454 -0.14056254785672886 0.11063575619445715
+v 0.6309673232578825 -0.5233429585504008 -0.3154324448276982
+v 1.1084771244357725 -0.5884126062022549 0.1487300274151031
+v 0.8717572608392643 -0.7961376346283748 -0.21461994431182724
+v 0.7935503679930702 -0.10381562175747021 -0.18064150493670278
+v 0.1930045861227373 0.016472567669853083 -0.055588103122916455
+v 0.2993876552253006 0.068558130299813 0.05000292959714747
+v 0.6924979778477462 -0.9668359505626445 0.09270230189174977
+v 0.6058417937103333 0.02638670731363664 0.13330177816314193
+v 0.8799156176905546 -0.05316924253091149 -0.14030335805749
+v -0.03079873151815908 -0.0802799299948706 0.021596095982981622
+v 1.0641264046687584 -0.5831225665734691 -0.13240524374201096
+v 0.8975839327885125 0.03171601038986818 -0.03175023217440753
+v 0.7485534984930229 -0.6251246158792332 0.3277257829104002
+v 0.00044654153001363883 -0.5196138546850763 0.1291549211950357
+v 0.33772493966100053 0.1132855735710503 0.023160301600170685
+v 0.8608413160501167 -0.9056034514194153 -0.13558463504892249
+v 0.5605073386486454 -0.8097792079852102 -0.23967071303923726
+v 1.0822732334889182 -0.5308882222561354 0.19977503297159924
+v 0.7099585314967243 -0.7597139791843466 0.2813809762769149
+v 0.12525657743891688 -0.4504680405145489 0.23678194793554713
+v 0.02220599896631595 -0.833916447723732 -0.15362716581839286
+v 0.8027646816452443 -0.9234334483485782 0.11700518232667789
+v 0.4585150943223921 0.13886381502531764 -0.01077830594725282
+v 0.6444362119717503 0.10480736805227411 0.03115727318618422
+v 0.35715072635957984 -1.0147167589258765 -0.043519543542239525
+v 0.44655818297057626 0.1278474340611117 -0.03982371044300115
+v 0.12918631114482187 -0.7174134161851835 0.18865575303773596
+v 0.6564281019615854 -0.9971038952658463 0.054806541006044984
+v 0.7799593668382123 -0.8282157236495682 0.2272305331051926
+v 0.46496795109363115 -1.0229147298990597 -0.05495869788773361
+v 0.42250500513503464 0.12738031165599628 0.02216266906471931
+v 0.37388741816348675 0.1155213358708857 0.031046522038815194
+v 0.23330502706074513 0.07836863229235402 0.004294196279049811
+v 0.18514171450456227 -0.9108464599628172 -0.14969176329083134
+v 0.21508494550247922 -0.33639531608209455 -0.24700349005588523
+v 0.7370612161626663 -0.9777753370574719 0.03579474750828032
+v 0.9378244560335888 -0.053457606680938075 0.13553624092047073
+v 0.8428810322893587 -0.41346073701448866 -0.27404067758492584
+v 0.9907265435532993 -0.399146286546847 0.24757822471673163
+v 0.8503141543566188 -0.38781613084587124 -0.26586269707141263
+v 0.7556473978696131 -0.417790726206639 -0.28918464416774486
g foo
-f 8 3 18
-f 21 23 70
-f 12 21 26
-f 12 21 63
-f 51 12 32
-f 51 12 63
-f 80 43 2
-f 80 43 32
-f 55 80 32
-f 55 80 25
-f 45 61 2
-f 82 43 2
-f 82 61 2
-f 82 43 32
-f 82 12 32
-f 82 61 26
-f 82 12 26
-f 28 61 26
-f 28 45 61
-f 68 77 73
-f 68 77 38
-f 71 9 73
-f 53 8 3
-f 53 8 18
-f 53 31 18
-f 30 3 18
-f 30 23 3
-f 56 21 26
-f 56 21 62
-f 56 28 26
-f 56 28 62
-f 7 62 37
-f 7 21 62
-f 57 23 3
-f 57 53 3
-f 46 6 20
-f 46 59 6
-f 46 29 20
-f 46 59 11
-f 46 29 11
-f 19 59 11
-f 78 4 18
-f 78 31 18
-f 16 25 2
-f 10 45 2
-f 10 45 11
-f 39 16 25
-f 39 16 2
-f 39 10 2
-f 13 25 2
-f 13 80 2
-f 13 80 25
-f 60 6 4
-f 60 77 4
-f 64 77 4
-f 64 77 38
-f 64 4 18
-f 64 30 18
-f 64 23 38
-f 64 30 23
-f 15 71 37
-f 15 71 73
-f 15 7 37
-f 15 7 42
-f 15 68 73
-f 15 68 38
-f 49 51 63
-f 49 53 63
-f 76 69 58
-f 76 69 31
-f 76 49 58
-f 76 49 53
-f 35 21 70
-f 35 7 21
-f 35 23 70
-f 35 23 38
-f 35 7 42
-f 35 15 38
-f 35 15 42
-f 5 53 63
-f 5 57 53
-f 5 21 63
-f 5 21 23
-f 5 57 23
-f 22 59 6
-f 22 19 9
-f 22 19 59
-f 22 9 73
-f 44 78 4
-f 52 55 25
-f 54 39 10
-f 54 10 11
-f 54 29 20
-f 54 29 11
-f 75 49 58
-f 75 49 51
-f 36 60 77
-f 36 77 73
-f 36 60 6
-f 17 28 45
-f 17 45 11
-f 17 19 11
-f 17 19 9
-f 33 17 28
-f 33 17 1
-f 48 53 31
-f 48 76 31
-f 48 76 53
-f 24 6 20
-f 24 6 4
-f 24 44 4
-f 84 44 78
-f 84 24 44
-f 14 69 55
-f 14 52 55
-f 34 52 25
-f 34 52 47
-f 67 55 32
-f 67 51 32
-f 40 22 6
-f 40 36 6
-f 40 22 73
-f 40 36 73
-f 65 17 1
-f 65 33 1
-f 65 17 9
-f 72 28 62
-f 72 33 28
-f 72 62 37
-f 72 65 33
-f 27 52 47
-f 27 14 52
-f 27 14 69
-f 27 69 31
-f 27 78 31
-f 27 84 78
-f 27 84 47
-f 41 34 25
-f 41 54 20
-f 41 54 39
-f 41 39 25
-f 81 84 24
-f 81 24 20
-f 66 69 58
-f 66 69 55
-f 66 67 55
-f 79 71 9
-f 79 65 9
-f 79 65 71
-f 50 71 37
-f 50 65 71
-f 50 72 37
-f 50 72 65
-f 83 41 34
-f 83 34 47
-f 83 41 20
-f 83 81 20
-f 83 84 47
-f 83 81 84
-f 74 66 67
-f 74 75 51
-f 74 67 51
-f 74 75 58
-f 74 66 58
-g
+f 8 3 18
+f 21 23 70
+f 12 21 26
+f 12 21 63
+f 51 12 32
+f 51 12 63
+f 80 43 2
+f 80 43 32
+f 55 80 32
+f 55 80 25
+f 45 61 2
+f 82 43 2
+f 82 61 2
+f 82 43 32
+f 82 12 32
+f 82 61 26
+f 82 12 26
+f 28 61 26
+f 28 45 61
+f 68 77 73
+f 68 77 38
+f 71 9 73
+f 53 8 3
+f 53 8 18
+f 53 31 18
+f 30 3 18
+f 30 23 3
+f 56 21 26
+f 56 21 62
+f 56 28 26
+f 56 28 62
+f 7 62 37
+f 7 21 62
+f 57 23 3
+f 57 53 3
+f 46 6 20
+f 46 59 6
+f 46 29 20
+f 46 59 11
+f 46 29 11
+f 19 59 11
+f 78 4 18
+f 78 31 18
+f 16 25 2
+f 10 45 2
+f 10 45 11
+f 39 16 25
+f 39 16 2
+f 39 10 2
+f 13 25 2
+f 13 80 2
+f 13 80 25
+f 60 6 4
+f 60 77 4
+f 64 77 4
+f 64 77 38
+f 64 4 18
+f 64 30 18
+f 64 23 38
+f 64 30 23
+f 15 71 37
+f 15 71 73
+f 15 7 37
+f 15 7 42
+f 15 68 73
+f 15 68 38
+f 49 51 63
+f 49 53 63
+f 76 69 58
+f 76 69 31
+f 76 49 58
+f 76 49 53
+f 35 21 70
+f 35 7 21
+f 35 23 70
+f 35 23 38
+f 35 7 42
+f 35 15 38
+f 35 15 42
+f 5 53 63
+f 5 57 53
+f 5 21 63
+f 5 21 23
+f 5 57 23
+f 22 59 6
+f 22 19 9
+f 22 19 59
+f 22 9 73
+f 44 78 4
+f 52 55 25
+f 54 39 10
+f 54 10 11
+f 54 29 20
+f 54 29 11
+f 75 49 58
+f 75 49 51
+f 36 60 77
+f 36 77 73
+f 36 60 6
+f 17 28 45
+f 17 45 11
+f 17 19 11
+f 17 19 9
+f 33 17 28
+f 33 17 1
+f 48 53 31
+f 48 76 31
+f 48 76 53
+f 24 6 20
+f 24 6 4
+f 24 44 4
+f 84 44 78
+f 84 24 44
+f 14 69 55
+f 14 52 55
+f 34 52 25
+f 34 52 47
+f 67 55 32
+f 67 51 32
+f 40 22 6
+f 40 36 6
+f 40 22 73
+f 40 36 73
+f 65 17 1
+f 65 33 1
+f 65 17 9
+f 72 28 62
+f 72 33 28
+f 72 62 37
+f 72 65 33
+f 27 52 47
+f 27 14 52
+f 27 14 69
+f 27 69 31
+f 27 78 31
+f 27 84 78
+f 27 84 47
+f 41 34 25
+f 41 54 20
+f 41 54 39
+f 41 39 25
+f 81 84 24
+f 81 24 20
+f 66 69 58
+f 66 69 55
+f 66 67 55
+f 79 71 9
+f 79 65 9
+f 79 65 71
+f 50 71 37
+f 50 65 71
+f 50 72 37
+f 50 72 65
+f 83 41 34
+f 83 34 47
+f 83 41 20
+f 83 81 20
+f 83 84 47
+f 83 81 84
+f 74 66 67
+f 74 75 51
+f 74 67 51
+f 74 75 58
+f 74 66 58
+g
diff --git a/data/relative_effector_positions/anymal_RF_ADAPTER_TO_FOOT_constraints_in_RHleg.obj b/data/relative_effector_positions/anymal_RF_ADAPTER_TO_FOOT_constraints_in_RHleg.obj
index 35759ce61df4264d66918b0f96b5a4dc025f571e..c99c71c5696d9a7615f6142c95598be40ba37397 100644
--- a/data/relative_effector_positions/anymal_RF_ADAPTER_TO_FOOT_constraints_in_RHleg.obj
+++ b/data/relative_effector_positions/anymal_RF_ADAPTER_TO_FOOT_constraints_in_RHleg.obj
@@ -1,202 +1,202 @@
-v 0.7995008890889097 -0.037430715321231656 -0.25778026294319334
-v 0.6299239151041307 0.2526087975231621 0.13376235700745231
-v 0.5011281235480092 0.5527318205813154 -0.2045760630002587
-v 0.8085414732955162 0.04055738868164677 -0.2726520379552897
-v 0.34040344016935264 0.5437123067289984 -0.18471109032792854
-v 1.0755576163332368 0.33629494157668505 -0.2055362956311268
-v 0.25312437756353157 -0.41502767833850596 -0.02211863848968071
-v 0.11894810270688218 0.5082609391049642 -0.14815942914660168
-v -0.07754358345760307 -0.2744658636857253 0.1324913278281874
-v 0.6733221971209462 -0.46947193242845714 -0.04004424038823595
-v 1.0855099591527342 -0.21031823588232634 0.040778569396475395
-v 0.8213564076728714 0.23556957018493824 -0.2950534015121074
-v 0.1594168259050384 -0.20808749336280918 -0.10424805072223242
-v 0.1053574938741048 -0.5081522914191288 0.11864028796152531
-v 0.7972107882122684 0.30589826958651467 0.12958175405604855
-v 0.894974437673588 -0.2050254219834608 -0.15109311917611606
-v 0.8272939072185617 -0.07425641631609514 0.3092837983625556
-v 0.8707192934976812 0.4990345156492644 -0.22633811583011876
-v 0.8716034344303767 -0.4482151383398527 0.31239788590715745
-v 1.0520985143578243 0.32788638640667467 -0.0035506155481735413
-v 0.1767198262154077 -0.30313896785805855 0.23879874341489826
-v 1.0850327552724046 -0.3102265035384123 0.13841747841248053
-v -0.16416707026263902 -0.04618063273405393 0.004678686494098416
-v 0.8896048142065289 -0.408025913231162 -0.02467495572200018
-v 0.8050630350928981 0.49480026224647794 -0.026625747904963193
-v 1.114143213800602 -0.03580076704606726 -0.04275051237836108
-v 0.6772087100267056 -0.34963359565274765 0.32576161812259974
-v 0.18884534383112048 0.14624670231384285 -0.23366504479883415
-v 1.0753993718633668 0.37252495806151603 -0.11617972082102179
-v 0.5540419649359039 -0.5664645894264464 0.08116530800949351
-v 0.32392736394976446 0.4648372280272943 -0.02903530301019902
-v 0.9804861530431368 0.17675142879778688 0.1608353785359427
-v 0.6331739205463829 -0.3850193700992113 -0.08724943704258281
-v 1.1276494387340228 -0.1736006404963331 0.09789140570691213
-v 0.8988434992584291 0.19207694092514377 0.18390739766541975
-v 1.0569887327635274 -0.3003634650779785 0.06237004934330681
-v 0.7468032310959294 0.38891436026584136 -0.295840139686222
-v 0.3206510007580119 0.5635453149472821 -0.08893211260306277
-v 0.9847730004868003 -0.24630042012534034 -0.05506022496627527
-v -0.080612096031309 0.12622324511785143 0.042570667518200433
-v 0.624782895923035 0.5273383585149805 -0.26202982450305445
-v 0.4637188649895066 -0.5311404795700341 0.20736752216092996
-v 0.11082502642105899 0.08550067915111376 -0.1832756005763544
-v 1.0604201755828075 -0.20633833898211373 -0.016194040120910635
-v 0.25311159363727137 0.3201359797202251 -0.28638023264881574
-v -0.06140716536537011 0.11620931674058455 -0.06407797952816924
-v 0.7134509715421731 0.5407671295601071 -0.0914767475520532
-v 0.9544433178916244 -0.4989982113142462 0.0892614045652631
-v -0.039051749144813575 0.329384152787916 -0.01675149880407273
-v 0.6264908425117959 -0.6088651101787147 0.1302381567807287
-v 1.0844144570054575 0.27604844986444926 -0.016696530182826186
-v 0.9356440545270603 0.32563900132165785 0.08698485680697388
-v 1.0265837239303774 -0.4035704625535096 0.13424644125130802
-v 1.1082398587642468 -0.13201077384134258 0.17334059391579934
-v 0.9889888375644555 0.06290275164087944 0.19237260056584843
-v 0.8436163000991587 -0.3010668564505948 -0.10826224385781125
-v 0.13675266769067246 0.22829098833207687 -0.20500770473868363
-v 1.0707103395745001 0.23513475477780407 -0.19999303857692147
-v 0.6982681731203968 -0.5195829663064552 0.0025317547941619
-v 0.7693130147970225 -0.2616839496990216 -0.14861252050685358
-v 0.9541841875935588 -0.39938505785874406 0.2564313508783953
-v 0.7457778792832361 -0.355847626314003 -0.0987946703231401
-v 0.16696490085997173 -0.06077350777958962 0.18625132560518443
-v 1.0469293334557639 -0.2532589528190562 0.21014577254574457
-v 1.0562675660410572 0.3857101520806666 -0.042829050767627275
-v 0.530136313327302 0.5008234031854517 -0.015536390185122095
-v 0.8801440958947598 0.5003760144285251 -0.22142196540086112
-v 0.4567063356442729 0.5716089999383555 -0.1454220031117277
+v 0.7995008890889097 -0.037430715321231656 -0.25778026294319334
+v 0.6299239151041307 0.2526087975231621 0.13376235700745231
+v 0.5011281235480092 0.5527318205813154 -0.2045760630002587
+v 0.8085414732955162 0.04055738868164677 -0.2726520379552897
+v 0.34040344016935264 0.5437123067289984 -0.18471109032792854
+v 1.0755576163332368 0.33629494157668505 -0.2055362956311268
+v 0.25312437756353157 -0.41502767833850596 -0.02211863848968071
+v 0.11894810270688218 0.5082609391049642 -0.14815942914660168
+v -0.07754358345760307 -0.2744658636857253 0.1324913278281874
+v 0.6733221971209462 -0.46947193242845714 -0.04004424038823595
+v 1.0855099591527342 -0.21031823588232634 0.040778569396475395
+v 0.8213564076728714 0.23556957018493824 -0.2950534015121074
+v 0.1594168259050384 -0.20808749336280918 -0.10424805072223242
+v 0.1053574938741048 -0.5081522914191288 0.11864028796152531
+v 0.7972107882122684 0.30589826958651467 0.12958175405604855
+v 0.894974437673588 -0.2050254219834608 -0.15109311917611606
+v 0.8272939072185617 -0.07425641631609514 0.3092837983625556
+v 0.8707192934976812 0.4990345156492644 -0.22633811583011876
+v 0.8716034344303767 -0.4482151383398527 0.31239788590715745
+v 1.0520985143578243 0.32788638640667467 -0.0035506155481735413
+v 0.1767198262154077 -0.30313896785805855 0.23879874341489826
+v 1.0850327552724046 -0.3102265035384123 0.13841747841248053
+v -0.16416707026263902 -0.04618063273405393 0.004678686494098416
+v 0.8896048142065289 -0.408025913231162 -0.02467495572200018
+v 0.8050630350928981 0.49480026224647794 -0.026625747904963193
+v 1.114143213800602 -0.03580076704606726 -0.04275051237836108
+v 0.6772087100267056 -0.34963359565274765 0.32576161812259974
+v 0.18884534383112048 0.14624670231384285 -0.23366504479883415
+v 1.0753993718633668 0.37252495806151603 -0.11617972082102179
+v 0.5540419649359039 -0.5664645894264464 0.08116530800949351
+v 0.32392736394976446 0.4648372280272943 -0.02903530301019902
+v 0.9804861530431368 0.17675142879778688 0.1608353785359427
+v 0.6331739205463829 -0.3850193700992113 -0.08724943704258281
+v 1.1276494387340228 -0.1736006404963331 0.09789140570691213
+v 0.8988434992584291 0.19207694092514377 0.18390739766541975
+v 1.0569887327635274 -0.3003634650779785 0.06237004934330681
+v 0.7468032310959294 0.38891436026584136 -0.295840139686222
+v 0.3206510007580119 0.5635453149472821 -0.08893211260306277
+v 0.9847730004868003 -0.24630042012534034 -0.05506022496627527
+v -0.080612096031309 0.12622324511785143 0.042570667518200433
+v 0.624782895923035 0.5273383585149805 -0.26202982450305445
+v 0.4637188649895066 -0.5311404795700341 0.20736752216092996
+v 0.11082502642105899 0.08550067915111376 -0.1832756005763544
+v 1.0604201755828075 -0.20633833898211373 -0.016194040120910635
+v 0.25311159363727137 0.3201359797202251 -0.28638023264881574
+v -0.06140716536537011 0.11620931674058455 -0.06407797952816924
+v 0.7134509715421731 0.5407671295601071 -0.0914767475520532
+v 0.9544433178916244 -0.4989982113142462 0.0892614045652631
+v -0.039051749144813575 0.329384152787916 -0.01675149880407273
+v 0.6264908425117959 -0.6088651101787147 0.1302381567807287
+v 1.0844144570054575 0.27604844986444926 -0.016696530182826186
+v 0.9356440545270603 0.32563900132165785 0.08698485680697388
+v 1.0265837239303774 -0.4035704625535096 0.13424644125130802
+v 1.1082398587642468 -0.13201077384134258 0.17334059391579934
+v 0.9889888375644555 0.06290275164087944 0.19237260056584843
+v 0.8436163000991587 -0.3010668564505948 -0.10826224385781125
+v 0.13675266769067246 0.22829098833207687 -0.20500770473868363
+v 1.0707103395745001 0.23513475477780407 -0.19999303857692147
+v 0.6982681731203968 -0.5195829663064552 0.0025317547941619
+v 0.7693130147970225 -0.2616839496990216 -0.14861252050685358
+v 0.9541841875935588 -0.39938505785874406 0.2564313508783953
+v 0.7457778792832361 -0.355847626314003 -0.0987946703231401
+v 0.16696490085997173 -0.06077350777958962 0.18625132560518443
+v 1.0469293334557639 -0.2532589528190562 0.21014577254574457
+v 1.0562675660410572 0.3857101520806666 -0.042829050767627275
+v 0.530136313327302 0.5008234031854517 -0.015536390185122095
+v 0.8801440958947598 0.5003760144285251 -0.22142196540086112
+v 0.4567063356442729 0.5716089999383555 -0.1454220031117277
g foo
-f 19 48 50
-f 19 54 17
-f 5 38 68
-f 5 38 8
-f 67 41 68
-f 49 38 8
-f 49 8 23
-f 49 40 23
-f 46 8 23
-f 46 57 23
-f 44 58 16
-f 45 41 37
-f 45 43 28
-f 45 5 8
-f 45 5 41
-f 45 57 23
-f 45 43 23
-f 45 46 8
-f 45 46 57
-f 30 50 14
-f 30 7 14
-f 42 50 14
-f 42 19 50
-f 42 21 14
-f 22 54 34
-f 3 41 68
-f 3 5 68
-f 3 5 41
-f 18 41 37
-f 18 67 41
-f 63 21 17
-f 63 49 17
-f 63 49 40
-f 9 14 23
-f 9 21 14
-f 9 40 23
-f 9 63 40
-f 9 63 21
-f 31 49 38
-f 13 43 28
-f 13 43 23
-f 13 14 23
-f 13 7 14
-f 26 44 34
-f 26 44 58
-f 26 29 34
-f 56 24 16
-f 39 44 48
-f 39 24 48
-f 39 44 16
-f 39 24 16
-f 1 58 16
-f 1 4 58
-f 1 13 28
-f 1 45 28
-f 1 4 45
-f 12 4 58
-f 12 45 37
-f 12 4 45
-f 59 7 10
-f 59 30 7
-f 59 30 50
-f 59 24 10
-f 59 48 50
-f 59 24 48
-f 27 42 19
-f 27 42 21
-f 27 19 17
-f 27 21 17
-f 64 19 54
-f 64 22 54
-f 53 19 48
-f 53 22 48
-f 11 44 34
-f 36 44 48
-f 36 22 48
-f 36 11 44
-f 36 22 34
-f 36 11 34
-f 6 18 67
-f 6 67 29
-f 6 26 29
-f 6 26 58
-f 6 12 58
-f 6 18 37
-f 6 12 37
-f 60 56 16
-f 60 1 16
-f 61 64 19
-f 61 64 22
-f 61 53 19
-f 61 53 22
-f 2 49 17
-f 2 31 49
-f 47 38 68
-f 47 25 38
-f 47 67 68
-f 47 25 67
-f 62 60 56
-f 62 24 10
-f 62 56 24
-f 33 60 1
-f 33 13 7
-f 33 1 13
-f 33 7 10
-f 33 62 10
-f 33 62 60
-f 15 2 17
-f 55 54 17
-f 51 29 34
-f 51 54 34
-f 52 15 25
-f 66 25 38
-f 66 15 25
-f 66 31 38
-f 66 2 31
-f 66 15 2
-f 32 55 17
-f 32 55 54
-f 35 52 15
-f 35 15 17
-f 35 32 52
-f 35 32 17
-f 65 52 25
-f 65 67 29
-f 65 25 67
-f 65 51 29
-f 65 32 52
-f 20 65 51
-f 20 65 32
-f 20 51 54
-f 20 32 54
-g
+f 19 48 50
+f 19 54 17
+f 5 38 68
+f 5 38 8
+f 67 41 68
+f 49 38 8
+f 49 8 23
+f 49 40 23
+f 46 8 23
+f 46 57 23
+f 44 58 16
+f 45 41 37
+f 45 43 28
+f 45 5 8
+f 45 5 41
+f 45 57 23
+f 45 43 23
+f 45 46 8
+f 45 46 57
+f 30 50 14
+f 30 7 14
+f 42 50 14
+f 42 19 50
+f 42 21 14
+f 22 54 34
+f 3 41 68
+f 3 5 68
+f 3 5 41
+f 18 41 37
+f 18 67 41
+f 63 21 17
+f 63 49 17
+f 63 49 40
+f 9 14 23
+f 9 21 14
+f 9 40 23
+f 9 63 40
+f 9 63 21
+f 31 49 38
+f 13 43 28
+f 13 43 23
+f 13 14 23
+f 13 7 14
+f 26 44 34
+f 26 44 58
+f 26 29 34
+f 56 24 16
+f 39 44 48
+f 39 24 48
+f 39 44 16
+f 39 24 16
+f 1 58 16
+f 1 4 58
+f 1 13 28
+f 1 45 28
+f 1 4 45
+f 12 4 58
+f 12 45 37
+f 12 4 45
+f 59 7 10
+f 59 30 7
+f 59 30 50
+f 59 24 10
+f 59 48 50
+f 59 24 48
+f 27 42 19
+f 27 42 21
+f 27 19 17
+f 27 21 17
+f 64 19 54
+f 64 22 54
+f 53 19 48
+f 53 22 48
+f 11 44 34
+f 36 44 48
+f 36 22 48
+f 36 11 44
+f 36 22 34
+f 36 11 34
+f 6 18 67
+f 6 67 29
+f 6 26 29
+f 6 26 58
+f 6 12 58
+f 6 18 37
+f 6 12 37
+f 60 56 16
+f 60 1 16
+f 61 64 19
+f 61 64 22
+f 61 53 19
+f 61 53 22
+f 2 49 17
+f 2 31 49
+f 47 38 68
+f 47 25 38
+f 47 67 68
+f 47 25 67
+f 62 60 56
+f 62 24 10
+f 62 56 24
+f 33 60 1
+f 33 13 7
+f 33 1 13
+f 33 7 10
+f 33 62 10
+f 33 62 60
+f 15 2 17
+f 55 54 17
+f 51 29 34
+f 51 54 34
+f 52 15 25
+f 66 25 38
+f 66 15 25
+f 66 31 38
+f 66 2 31
+f 66 15 2
+f 32 55 17
+f 32 55 54
+f 35 52 15
+f 35 15 17
+f 35 32 52
+f 35 32 17
+f 65 52 25
+f 65 67 29
+f 65 25 67
+f 65 51 29
+f 65 32 52
+f 20 65 51
+f 20 65 32
+f 20 51 54
+f 20 32 54
+g
diff --git a/data/relative_effector_positions/anymal_RH_ADAPTER_TO_FOOT_constraints_in_LFleg.obj b/data/relative_effector_positions/anymal_RH_ADAPTER_TO_FOOT_constraints_in_LFleg.obj
index 21255c6d445707ae1a3572e0ec3fbaa7e4c90177..02ab2146461ba952cd0d32168a1ec074cafb97c8 100644
--- a/data/relative_effector_positions/anymal_RH_ADAPTER_TO_FOOT_constraints_in_LFleg.obj
+++ b/data/relative_effector_positions/anymal_RH_ADAPTER_TO_FOOT_constraints_in_LFleg.obj
@@ -1,229 +1,229 @@
-v 0.052186058738587476 -0.6715354267181292 -0.04146096289394907
-v -0.6006569890441394 -1.0174113872167452 0.04781523554606598
-v -0.08657547209554617 -0.21622480246035297 0.15177681086674588
-v -0.14696110946763008 -0.883261733043069 -0.11547685036682531
-v -0.4793769786576849 0.09524519730759502 0.04759682856964276
-v 0.054745809892117026 -0.7866903898995912 0.0661358827018198
-v -1.1076522382609182 -0.5956914766788222 0.06540724741611711
-v -0.012250292888808578 -0.8959270108712803 0.02023259946858863
-v -1.1033295559388543 -0.4421053253896936 0.08713479119623119
-v -0.8814999756273814 -0.7584183788889511 -0.283491384324239
-v -0.8797213095418484 -0.9777070875342824 0.08089397431765177
-v -0.7080328949749688 -0.6808029378883678 0.29499516620088606
-v -0.03784467925055483 0.05689051843397952 -0.06930311781158971
-v -1.057620212431891 -0.055223461804426624 -0.010680785624305855
-v -0.8490707050420219 -0.4387164176103174 -0.3157548738660349
-v -0.9510951544228079 -0.7877949024842967 0.1624984368157601
-v -0.2549390284751417 -0.9834619955822634 -0.0948385183443024
-v -1.0772869195099077 -0.6906440110294891 0.04461464186407205
-v -0.2594457596969465 -0.11354787678827069 0.1402528316430397
-v -0.66733217283874 -0.9654866934019485 -0.100081889111727
-v -1.141387125392382 -0.4268531261482441 -0.15808471360596116
-v -0.6633944703595794 -1.013766557759125 0.016568880096893168
-v -0.41676682342997373 -1.0064140285759904 0.11651875199185818
-v -1.0397619277740482 -0.42056929497750295 -0.22477857871637863
-v -0.7949290025411306 0.06697468890968604 0.1261807016385631
-v -0.8436392683520442 -0.369839093555039 0.2633822911886875
-v -0.22472064101720873 -0.616200182084464 -0.2524745013685493
-v -0.9176990754633856 -0.42628332076368813 0.24870390372211387
-v -1.0309170190233656 -0.5073761650063267 0.16932365049530507
-v -0.8507781532039418 -0.1879800586374831 -0.21328736642953516
-v -0.3090766954834622 -0.8001386363593801 0.22141574013936965
-v 0.0014103055057584496 -0.29080686276796447 -0.09293266318790205
-v -1.0959610138634153 -0.6327658369706911 -0.1096331243740582
-v -1.0451034405685347 -0.7823085156173795 -0.06449226330854962
-v -0.23132525736514206 -0.7226364402876801 0.2404773988263515
-v -0.5205018154938208 0.09494204187239988 -0.09318562654040702
-v -0.8766411295924498 -0.9400285161065368 -0.09353237413725479
-v -0.9401852152365353 -0.8584116878298973 -0.15422226855187393
-v -0.5553316189654864 -1.0128035212349558 -0.048846194236874974
-v -0.5696783381323665 0.11455256053067364 -0.045835757652572084
-v -0.12038297555648408 -0.44450809327969865 0.2730842199224136
-v -0.270915513071742 0.06436773339745047 0.022021891796854653
-v -0.8196913598720024 -0.8340568004536715 -0.2431444809807837
-v -0.13520060676929163 -0.7485861952639247 -0.20838697392652494
-v -0.14388104422057885 -0.6182119272019748 0.24361459155251958
-v -0.7662953995464215 -0.6281042057119652 0.3018902524153172
-v -0.7384722364924339 -0.8685961547305201 0.2316251441802929
-v -0.9331109314548167 -0.632590491951534 0.211117048863943
-v -0.7501239019792867 -0.35510572571080884 0.2645005930270417
-v -0.8618430000106856 -0.8804060375983322 -0.18101625768941731
-v -0.7565060814483878 -0.8205190296265314 -0.2681242154880791
-v -1.1020221651815858 -0.18796898994581263 -0.049333389898626345
-v 0.05475479237363495 -0.1106410927757966 -0.024125409761160288
-v -0.452243934667251 0.11140593005588145 0.006629172364510494
-v -0.14034732411469725 -0.6709909895799657 -0.22204642689107895
-v 0.09187059949551221 -0.6773256703071341 0.16542086710986476
-v -1.027761514275682 -0.19230141903193215 0.11590759915466187
-v -0.9489051065769997 -0.7569046566357206 -0.23219396553569283
-v 0.024091571044383725 -0.6935778127599832 -0.1363722317891768
-v -1.0272791251984015 -0.7766771705789324 0.12190827206447286
-v -1.0613117575537845 -0.06208516926256423 -0.1148192527181065
-v -0.3696867115823105 -1.0089913098634167 -0.07039448838131557
-v -0.10573807738718366 -0.9582558385287894 0.03679889094355765
-v -0.10128673406694527 -0.9441022156794496 -0.0290454112670524
-v -0.39900143272857647 -1.031673049688779 -0.026667210877306224
-v -0.8815558957052275 0.054699156569096574 0.05042943830157365
-v 0.019497343059730388 -0.7770183442803242 0.1696617413182928
-v -0.9687114618726644 -0.07445403539800795 0.14968986261994854
-v -1.0206489116057722 -0.8194063419321018 -0.05333748500210095
-v -0.8554592814889905 -0.34556515248731057 -0.29127073575355455
-v -0.8394520423159423 -0.9496887112783993 0.13010608361112852
-v -0.17034746085503719 -0.9633191364930949 0.11647580522762713
-v -1.0466934234441458 -9.486403272819521e-05 -0.03373576847878018
-v -0.681701109248365 -0.018751379757344325 -0.14273172055320815
-v -1.0683278426779181 -0.6868443353587095 -0.16630615283798655
-v -0.6815207369323745 -0.6191727876618469 -0.2947564137553465
-v -0.16255218932724552 -0.3881222096544618 -0.20199869472483867
+v 0.052186058738587476 -0.6715354267181292 -0.04146096289394907
+v -0.6006569890441394 -1.0174113872167452 0.04781523554606598
+v -0.08657547209554617 -0.21622480246035297 0.15177681086674588
+v -0.14696110946763008 -0.883261733043069 -0.11547685036682531
+v -0.4793769786576849 0.09524519730759502 0.04759682856964276
+v 0.054745809892117026 -0.7866903898995912 0.0661358827018198
+v -1.1076522382609182 -0.5956914766788222 0.06540724741611711
+v -0.012250292888808578 -0.8959270108712803 0.02023259946858863
+v -1.1033295559388543 -0.4421053253896936 0.08713479119623119
+v -0.8814999756273814 -0.7584183788889511 -0.283491384324239
+v -0.8797213095418484 -0.9777070875342824 0.08089397431765177
+v -0.7080328949749688 -0.6808029378883678 0.29499516620088606
+v -0.03784467925055483 0.05689051843397952 -0.06930311781158971
+v -1.057620212431891 -0.055223461804426624 -0.010680785624305855
+v -0.8490707050420219 -0.4387164176103174 -0.3157548738660349
+v -0.9510951544228079 -0.7877949024842967 0.1624984368157601
+v -0.2549390284751417 -0.9834619955822634 -0.0948385183443024
+v -1.0772869195099077 -0.6906440110294891 0.04461464186407205
+v -0.2594457596969465 -0.11354787678827069 0.1402528316430397
+v -0.66733217283874 -0.9654866934019485 -0.100081889111727
+v -1.141387125392382 -0.4268531261482441 -0.15808471360596116
+v -0.6633944703595794 -1.013766557759125 0.016568880096893168
+v -0.41676682342997373 -1.0064140285759904 0.11651875199185818
+v -1.0397619277740482 -0.42056929497750295 -0.22477857871637863
+v -0.7949290025411306 0.06697468890968604 0.1261807016385631
+v -0.8436392683520442 -0.369839093555039 0.2633822911886875
+v -0.22472064101720873 -0.616200182084464 -0.2524745013685493
+v -0.9176990754633856 -0.42628332076368813 0.24870390372211387
+v -1.0309170190233656 -0.5073761650063267 0.16932365049530507
+v -0.8507781532039418 -0.1879800586374831 -0.21328736642953516
+v -0.3090766954834622 -0.8001386363593801 0.22141574013936965
+v 0.0014103055057584496 -0.29080686276796447 -0.09293266318790205
+v -1.0959610138634153 -0.6327658369706911 -0.1096331243740582
+v -1.0451034405685347 -0.7823085156173795 -0.06449226330854962
+v -0.23132525736514206 -0.7226364402876801 0.2404773988263515
+v -0.5205018154938208 0.09494204187239988 -0.09318562654040702
+v -0.8766411295924498 -0.9400285161065368 -0.09353237413725479
+v -0.9401852152365353 -0.8584116878298973 -0.15422226855187393
+v -0.5553316189654864 -1.0128035212349558 -0.048846194236874974
+v -0.5696783381323665 0.11455256053067364 -0.045835757652572084
+v -0.12038297555648408 -0.44450809327969865 0.2730842199224136
+v -0.270915513071742 0.06436773339745047 0.022021891796854653
+v -0.8196913598720024 -0.8340568004536715 -0.2431444809807837
+v -0.13520060676929163 -0.7485861952639247 -0.20838697392652494
+v -0.14388104422057885 -0.6182119272019748 0.24361459155251958
+v -0.7662953995464215 -0.6281042057119652 0.3018902524153172
+v -0.7384722364924339 -0.8685961547305201 0.2316251441802929
+v -0.9331109314548167 -0.632590491951534 0.211117048863943
+v -0.7501239019792867 -0.35510572571080884 0.2645005930270417
+v -0.8618430000106856 -0.8804060375983322 -0.18101625768941731
+v -0.7565060814483878 -0.8205190296265314 -0.2681242154880791
+v -1.1020221651815858 -0.18796898994581263 -0.049333389898626345
+v 0.05475479237363495 -0.1106410927757966 -0.024125409761160288
+v -0.452243934667251 0.11140593005588145 0.006629172364510494
+v -0.14034732411469725 -0.6709909895799657 -0.22204642689107895
+v 0.09187059949551221 -0.6773256703071341 0.16542086710986476
+v -1.027761514275682 -0.19230141903193215 0.11590759915466187
+v -0.9489051065769997 -0.7569046566357206 -0.23219396553569283
+v 0.024091571044383725 -0.6935778127599832 -0.1363722317891768
+v -1.0272791251984015 -0.7766771705789324 0.12190827206447286
+v -1.0613117575537845 -0.06208516926256423 -0.1148192527181065
+v -0.3696867115823105 -1.0089913098634167 -0.07039448838131557
+v -0.10573807738718366 -0.9582558385287894 0.03679889094355765
+v -0.10128673406694527 -0.9441022156794496 -0.0290454112670524
+v -0.39900143272857647 -1.031673049688779 -0.026667210877306224
+v -0.8815558957052275 0.054699156569096574 0.05042943830157365
+v 0.019497343059730388 -0.7770183442803242 0.1696617413182928
+v -0.9687114618726644 -0.07445403539800795 0.14968986261994854
+v -1.0206489116057722 -0.8194063419321018 -0.05333748500210095
+v -0.8554592814889905 -0.34556515248731057 -0.29127073575355455
+v -0.8394520423159423 -0.9496887112783993 0.13010608361112852
+v -0.17034746085503719 -0.9633191364930949 0.11647580522762713
+v -1.0466934234441458 -9.486403272819521e-05 -0.03373576847878018
+v -0.681701109248365 -0.018751379757344325 -0.14273172055320815
+v -1.0683278426779181 -0.6868443353587095 -0.16630615283798655
+v -0.6815207369323745 -0.6191727876618469 -0.2947564137553465
+v -0.16255218932724552 -0.3881222096544618 -0.20199869472483867
g foo
-f 36 13 40
-f 75 10 21
-f 3 41 56
-f 47 71 23
-f 47 72 23
-f 45 35 41
-f 65 72 23
-f 11 71 23
-f 11 71 60
-f 53 3 56
-f 53 3 13
-f 77 59 13
-f 77 15 27
-f 77 15 13
-f 33 7 21
-f 33 75 21
-f 24 10 21
-f 24 15 10
-f 76 15 27
-f 76 15 10
-f 19 25 41
-f 19 3 41
-f 19 5 25
-f 73 36 40
-f 73 25 68
-f 29 7 60
-f 29 57 68
-f 67 45 35
-f 67 6 56
-f 67 41 56
-f 67 45 41
-f 12 47 46
-f 12 41 46
-f 12 35 41
-f 31 47 72
-f 31 67 35
-f 31 67 72
-f 31 12 35
-f 31 12 47
-f 16 71 60
-f 16 47 71
-f 16 47 46
-f 2 65 22
-f 2 65 23
-f 2 11 22
-f 2 11 23
-f 63 65 72
-f 63 17 64
-f 63 17 65
-f 51 76 27
-f 51 76 10
-f 1 53 56
-f 1 53 59
-f 1 6 56
-f 1 6 59
-f 32 59 13
-f 32 53 13
-f 32 53 59
-f 55 77 27
-f 55 77 59
-f 54 13 40
-f 54 25 40
-f 54 5 25
-f 49 25 41
-f 49 41 46
-f 61 73 36
-f 61 24 21
-f 61 52 21
-f 61 73 52
-f 70 36 13
-f 70 15 13
-f 70 24 15
-f 70 61 30
-f 70 61 24
-f 18 7 60
-f 18 33 7
-f 42 19 5
-f 42 54 13
-f 42 54 5
-f 42 3 13
-f 42 19 3
-f 66 25 40
-f 66 73 40
-f 66 73 25
-f 14 57 68
-f 14 73 68
-f 14 73 52
-f 48 29 60
-f 48 16 46
-f 48 16 60
-f 28 29 68
-f 28 48 46
-f 28 48 29
-f 39 65 22
-f 37 11 22
-f 37 39 22
-f 58 75 10
-f 8 63 64
-f 8 59 64
-f 8 6 59
-f 8 67 6
-f 8 67 72
-f 8 63 72
-f 43 50 10
-f 43 51 10
-f 43 51 50
-f 4 59 64
-f 4 17 64
-f 44 51 17
-f 44 4 17
-f 44 51 27
-f 44 55 27
-f 44 55 59
-f 44 4 59
-f 74 61 36
-f 74 61 30
-f 74 70 36
-f 74 70 30
-f 9 29 57
-f 9 14 57
-f 9 29 7
-f 9 14 52
-f 9 7 21
-f 9 52 21
-f 26 49 46
-f 26 28 46
-f 26 28 68
-f 26 25 68
-f 26 49 25
-f 62 51 17
-f 62 17 65
-f 62 39 65
-f 20 37 50
-f 20 37 39
-f 20 51 50
-f 20 62 51
-f 20 62 39
-f 69 37 11
-f 69 11 60
-f 38 37 50
-f 38 50 10
-f 38 58 10
-f 38 58 75
-f 38 69 37
-f 34 18 60
-f 34 69 60
-f 34 18 33
-f 34 33 75
-f 34 38 75
-f 34 38 69
-g
+f 36 13 40
+f 75 10 21
+f 3 41 56
+f 47 71 23
+f 47 72 23
+f 45 35 41
+f 65 72 23
+f 11 71 23
+f 11 71 60
+f 53 3 56
+f 53 3 13
+f 77 59 13
+f 77 15 27
+f 77 15 13
+f 33 7 21
+f 33 75 21
+f 24 10 21
+f 24 15 10
+f 76 15 27
+f 76 15 10
+f 19 25 41
+f 19 3 41
+f 19 5 25
+f 73 36 40
+f 73 25 68
+f 29 7 60
+f 29 57 68
+f 67 45 35
+f 67 6 56
+f 67 41 56
+f 67 45 41
+f 12 47 46
+f 12 41 46
+f 12 35 41
+f 31 47 72
+f 31 67 35
+f 31 67 72
+f 31 12 35
+f 31 12 47
+f 16 71 60
+f 16 47 71
+f 16 47 46
+f 2 65 22
+f 2 65 23
+f 2 11 22
+f 2 11 23
+f 63 65 72
+f 63 17 64
+f 63 17 65
+f 51 76 27
+f 51 76 10
+f 1 53 56
+f 1 53 59
+f 1 6 56
+f 1 6 59
+f 32 59 13
+f 32 53 13
+f 32 53 59
+f 55 77 27
+f 55 77 59
+f 54 13 40
+f 54 25 40
+f 54 5 25
+f 49 25 41
+f 49 41 46
+f 61 73 36
+f 61 24 21
+f 61 52 21
+f 61 73 52
+f 70 36 13
+f 70 15 13
+f 70 24 15
+f 70 61 30
+f 70 61 24
+f 18 7 60
+f 18 33 7
+f 42 19 5
+f 42 54 13
+f 42 54 5
+f 42 3 13
+f 42 19 3
+f 66 25 40
+f 66 73 40
+f 66 73 25
+f 14 57 68
+f 14 73 68
+f 14 73 52
+f 48 29 60
+f 48 16 46
+f 48 16 60
+f 28 29 68
+f 28 48 46
+f 28 48 29
+f 39 65 22
+f 37 11 22
+f 37 39 22
+f 58 75 10
+f 8 63 64
+f 8 59 64
+f 8 6 59
+f 8 67 6
+f 8 67 72
+f 8 63 72
+f 43 50 10
+f 43 51 10
+f 43 51 50
+f 4 59 64
+f 4 17 64
+f 44 51 17
+f 44 4 17
+f 44 51 27
+f 44 55 27
+f 44 55 59
+f 44 4 59
+f 74 61 36
+f 74 61 30
+f 74 70 36
+f 74 70 30
+f 9 29 57
+f 9 14 57
+f 9 29 7
+f 9 14 52
+f 9 7 21
+f 9 52 21
+f 26 49 46
+f 26 28 46
+f 26 28 68
+f 26 25 68
+f 26 49 25
+f 62 51 17
+f 62 17 65
+f 62 39 65
+f 20 37 50
+f 20 37 39
+f 20 51 50
+f 20 62 51
+f 20 62 39
+f 69 37 11
+f 69 11 60
+f 38 37 50
+f 38 50 10
+f 38 58 10
+f 38 58 75
+f 38 69 37
+f 34 18 60
+f 34 69 60
+f 34 18 33
+f 34 33 75
+f 34 38 75
+f 34 38 69
+g
diff --git a/data/relative_effector_positions/anymal_RH_ADAPTER_TO_FOOT_constraints_in_LHleg.obj b/data/relative_effector_positions/anymal_RH_ADAPTER_TO_FOOT_constraints_in_LHleg.obj
index 534c39523d3226ecf9cde93246512672bcfc455d..691caeca1b88815560b50b42793fcd75a6e800e4 100644
--- a/data/relative_effector_positions/anymal_RH_ADAPTER_TO_FOOT_constraints_in_LHleg.obj
+++ b/data/relative_effector_positions/anymal_RH_ADAPTER_TO_FOOT_constraints_in_LHleg.obj
@@ -1,232 +1,232 @@
-v -0.49035404684180456 -0.7508195962621832 0.20710932848648453
-v -0.4050652996290509 -0.07293609784286129 0.12279310282122347
-v 0.059837573410679246 0.07307401305991121 0.041467626588350126
-v 0.3816803507817759 -0.5210425052683973 0.29141478772394847
-v 0.2671638874526956 -0.13646405505130157 0.09697368272294998
-v -0.4033567764343186 -0.24717180532654714 0.17847729641757393
-v 0.31529992537834967 -0.31841870267725 -0.26384490637891994
-v -0.1488372687627699 -1.0100810241314693 0.011094219530157656
-v -0.22822334258956245 -0.17844301125207426 0.18890757124838142
-v 0.42664132037843583 -0.9145354891046135 -0.04562142027729832
-v 0.5278010851244931 -0.638498209015532 0.1529901858840942
-v -0.49660345793515714 -0.9410445131089225 0.0038339878707469577
-v 0.45803361714824553 -0.5556808289860353 -0.2326103838132796
-v -0.11248598618142125 -1.0147799870296141 0.019825014897939752
-v 0.040174155646179155 -0.05090398757754408 -0.11920278610054486
-v -0.41738591682364634 0.05386742425731528 0.03878944048706329
-v 0.43618428701021283 -0.06578581917394626 -0.032144105879301776
-v -0.4109389221684284 -0.7290767140680336 -0.24924489636790398
-v -0.309188855542628 -0.9547270523662104 -0.09869302295263493
-v 0.5378043885267283 -0.7771566564281541 -0.06042669951702301
-v -0.554350144323299 -0.6297604805555409 0.030773565541039327
-v 0.46397963173038503 -0.8916526936415663 -0.05389175832010479
-v -0.23640798757114578 -0.47075554417758997 -0.26194719424932844
-v 0.44622303966129984 -0.3250073565628946 -0.18196122334791986
-v 0.5197425249057676 -0.4130780334761292 0.07364695424451567
-v 0.2982113111161163 -0.24945014692532655 0.15880097177741637
-v 0.33038532238468554 -0.22100370967903712 -0.20141619453512882
-v -0.5176256864696188 -0.07553947639194739 0.029477483497242163
-v -0.5710689970134102 -0.36256064664991683 -0.08172486961183673
-v 0.28897876815534274 -0.5927650503159021 -0.2694363551239839
-v 0.12778045221120443 -0.8997507803736604 0.16548093091304256
-v -0.3199936085327011 -0.7770579650315614 0.24539295606910622
-v -0.5841723122525182 -0.39017478692334395 -0.005814312587710424
-v -0.3433599325948032 -0.14132831784034222 -0.12146417866474968
-v 0.20113686945609366 -0.856749222435805 -0.20068794345274976
-v 0.5133626771884913 -0.6091519980938929 -0.1701595377268259
-v -0.013042031736554632 0.09964150494478768 -0.04090025992898472
-v 0.4022181169078592 -0.8008092007074127 -0.20365922866779018
-v 0.22596974005833623 -0.8755178267606942 0.1695342011943427
-v 0.3379965067218512 -0.36095135188899846 -0.26239908473793705
-v 0.265623541727769 -0.8666952888287187 -0.1950215836170865
-v -0.23581837506841938 -0.969980446109994 0.11285308761424234
-v 0.577182165540969 -0.6449183331649162 0.1036570200604619
-v -0.30513696565361376 0.07514048440638793 0.036001927104926945
-v -0.27789663354135974 -0.875809322203152 -0.18640673472509095
-v 0.23163470133613664 -0.7932175329564175 -0.21926654911012927
-v -0.14873752234421445 0.06224330716183318 0.06499379042991699
-v 0.2526584529394196 -0.8078633248157828 0.2144429023938194
-v 0.3280823939175139 -0.9684206364816764 -0.0029364345199799358
-v -0.5308312703084884 -0.5221170443493908 0.14705424359046348
-v 0.18705839426998228 -0.9703276432895905 -0.06834223408864826
-v -0.455856139274934 -0.4519063345115004 0.21306289340360524
-v -0.5493283955824602 -0.4968686422685934 0.11908709640470505
-v -0.09747443980861421 -0.997744643958862 -0.0475342563066114
-v 0.1196707656175333 0.003970011518543509 0.06775007533122707
-v -0.5793886582092684 -0.15935666250232042 0.04519077861416765
-v 0.11654041448946251 -0.45449690403793813 -0.2737918652497615
-v 0.477114405674948 -0.8838700695447552 -0.05178016758617521
-v -0.5252886606795855 -0.8107879640241918 -0.07100084738689005
-v 0.23136306966307157 0.048968914916677125 -0.05457841264383645
-v 0.11521880077837346 -0.9880757513474421 0.065169217112505
-v -0.2772368476882204 -0.43866792722173487 0.3071436617443535
-v 0.12820422358979938 -1.0147910941404015 -0.00037987169618913796
-v 0.4486977367347964 -0.8305337863091568 0.08229829375180475
-v 0.1000640522700581 -0.6133809256454366 -0.2728713686954044
-v -0.35541155683324166 -0.852499562691177 0.1806914292768645
-v 0.1083097392495182 0.07322920039086209 0.011147487262492595
-v -0.2656389899141308 -0.53799703317415 0.28949939622814136
-v -0.5301088145487259 -0.7721775041768428 0.04796618125488927
-v 0.5848495626062704 -0.5233491907382427 -0.08585640643110859
-v -0.17785277901932994 0.13327829982503808 -0.013586926594875592
-v -0.510660527713293 -0.4294475863796823 -0.16219785810830878
-v 0.22677325840184198 -0.9299041145375939 -0.11844469766265003
-v -0.06307237777808383 -0.9843540567848387 0.11872265905235485
-v 0.4104153020047618 -0.052855972033004794 -0.0813691260195285
-v 0.2839673096358367 -0.457394823903357 -0.2774938083595907
-v -0.11856395790689961 0.03186884203599763 0.09597630464028356
-v -0.23338845193202024 -0.6266459600533196 0.2735374740236767
+v -0.49035404684180456 -0.7508195962621832 0.20710932848648453
+v -0.4050652996290509 -0.07293609784286129 0.12279310282122347
+v 0.059837573410679246 0.07307401305991121 0.041467626588350126
+v 0.3816803507817759 -0.5210425052683973 0.29141478772394847
+v 0.2671638874526956 -0.13646405505130157 0.09697368272294998
+v -0.4033567764343186 -0.24717180532654714 0.17847729641757393
+v 0.31529992537834967 -0.31841870267725 -0.26384490637891994
+v -0.1488372687627699 -1.0100810241314693 0.011094219530157656
+v -0.22822334258956245 -0.17844301125207426 0.18890757124838142
+v 0.42664132037843583 -0.9145354891046135 -0.04562142027729832
+v 0.5278010851244931 -0.638498209015532 0.1529901858840942
+v -0.49660345793515714 -0.9410445131089225 0.0038339878707469577
+v 0.45803361714824553 -0.5556808289860353 -0.2326103838132796
+v -0.11248598618142125 -1.0147799870296141 0.019825014897939752
+v 0.040174155646179155 -0.05090398757754408 -0.11920278610054486
+v -0.41738591682364634 0.05386742425731528 0.03878944048706329
+v 0.43618428701021283 -0.06578581917394626 -0.032144105879301776
+v -0.4109389221684284 -0.7290767140680336 -0.24924489636790398
+v -0.309188855542628 -0.9547270523662104 -0.09869302295263493
+v 0.5378043885267283 -0.7771566564281541 -0.06042669951702301
+v -0.554350144323299 -0.6297604805555409 0.030773565541039327
+v 0.46397963173038503 -0.8916526936415663 -0.05389175832010479
+v -0.23640798757114578 -0.47075554417758997 -0.26194719424932844
+v 0.44622303966129984 -0.3250073565628946 -0.18196122334791986
+v 0.5197425249057676 -0.4130780334761292 0.07364695424451567
+v 0.2982113111161163 -0.24945014692532655 0.15880097177741637
+v 0.33038532238468554 -0.22100370967903712 -0.20141619453512882
+v -0.5176256864696188 -0.07553947639194739 0.029477483497242163
+v -0.5710689970134102 -0.36256064664991683 -0.08172486961183673
+v 0.28897876815534274 -0.5927650503159021 -0.2694363551239839
+v 0.12778045221120443 -0.8997507803736604 0.16548093091304256
+v -0.3199936085327011 -0.7770579650315614 0.24539295606910622
+v -0.5841723122525182 -0.39017478692334395 -0.005814312587710424
+v -0.3433599325948032 -0.14132831784034222 -0.12146417866474968
+v 0.20113686945609366 -0.856749222435805 -0.20068794345274976
+v 0.5133626771884913 -0.6091519980938929 -0.1701595377268259
+v -0.013042031736554632 0.09964150494478768 -0.04090025992898472
+v 0.4022181169078592 -0.8008092007074127 -0.20365922866779018
+v 0.22596974005833623 -0.8755178267606942 0.1695342011943427
+v 0.3379965067218512 -0.36095135188899846 -0.26239908473793705
+v 0.265623541727769 -0.8666952888287187 -0.1950215836170865
+v -0.23581837506841938 -0.969980446109994 0.11285308761424234
+v 0.577182165540969 -0.6449183331649162 0.1036570200604619
+v -0.30513696565361376 0.07514048440638793 0.036001927104926945
+v -0.27789663354135974 -0.875809322203152 -0.18640673472509095
+v 0.23163470133613664 -0.7932175329564175 -0.21926654911012927
+v -0.14873752234421445 0.06224330716183318 0.06499379042991699
+v 0.2526584529394196 -0.8078633248157828 0.2144429023938194
+v 0.3280823939175139 -0.9684206364816764 -0.0029364345199799358
+v -0.5308312703084884 -0.5221170443493908 0.14705424359046348
+v 0.18705839426998228 -0.9703276432895905 -0.06834223408864826
+v -0.455856139274934 -0.4519063345115004 0.21306289340360524
+v -0.5493283955824602 -0.4968686422685934 0.11908709640470505
+v -0.09747443980861421 -0.997744643958862 -0.0475342563066114
+v 0.1196707656175333 0.003970011518543509 0.06775007533122707
+v -0.5793886582092684 -0.15935666250232042 0.04519077861416765
+v 0.11654041448946251 -0.45449690403793813 -0.2737918652497615
+v 0.477114405674948 -0.8838700695447552 -0.05178016758617521
+v -0.5252886606795855 -0.8107879640241918 -0.07100084738689005
+v 0.23136306966307157 0.048968914916677125 -0.05457841264383645
+v 0.11521880077837346 -0.9880757513474421 0.065169217112505
+v -0.2772368476882204 -0.43866792722173487 0.3071436617443535
+v 0.12820422358979938 -1.0147910941404015 -0.00037987169618913796
+v 0.4486977367347964 -0.8305337863091568 0.08229829375180475
+v 0.1000640522700581 -0.6133809256454366 -0.2728713686954044
+v -0.35541155683324166 -0.852499562691177 0.1806914292768645
+v 0.1083097392495182 0.07322920039086209 0.011147487262492595
+v -0.2656389899141308 -0.53799703317415 0.28949939622814136
+v -0.5301088145487259 -0.7721775041768428 0.04796618125488927
+v 0.5848495626062704 -0.5233491907382427 -0.08585640643110859
+v -0.17785277901932994 0.13327829982503808 -0.013586926594875592
+v -0.510660527713293 -0.4294475863796823 -0.16219785810830878
+v 0.22677325840184198 -0.9299041145375939 -0.11844469766265003
+v -0.06307237777808383 -0.9843540567848387 0.11872265905235485
+v 0.4104153020047618 -0.052855972033004794 -0.0813691260195285
+v 0.2839673096358367 -0.457394823903357 -0.2774938083595907
+v -0.11856395790689961 0.03186884203599763 0.09597630464028356
+v -0.23338845193202024 -0.6266459600533196 0.2735374740236767
g foo
-f 34 7 23
-f 53 56 33
-f 52 1 62
-f 52 2 56
-f 72 18 23
-f 72 34 23
-f 16 2 56
-f 16 34 71
-f 15 34 71
-f 15 34 7
-f 25 17 70
-f 4 17 26
-f 4 25 17
-f 6 2 62
-f 6 52 62
-f 6 52 2
-f 50 53 1
-f 50 52 1
-f 50 53 56
-f 50 52 56
-f 69 53 1
-f 69 12 1
-f 19 12 18
-f 29 56 33
-f 3 47 71
-f 44 16 71
-f 44 47 71
-f 77 4 62
-f 77 16 2
-f 77 3 47
-f 77 44 16
-f 77 44 47
-f 32 1 62
-f 43 25 70
-f 43 4 25
-f 21 53 33
-f 21 69 53
-f 8 19 12
-f 28 29 72
-f 28 72 34
-f 28 16 34
-f 28 16 56
-f 28 29 56
-f 59 72 18
-f 59 29 72
-f 59 12 18
-f 59 69 12
-f 59 21 69
-f 59 29 33
-f 59 21 33
-f 55 3 17
-f 55 4 26
-f 55 77 4
-f 55 77 3
-f 9 2 62
-f 9 77 62
-f 9 77 2
-f 75 60 17
-f 75 24 7
-f 75 17 70
-f 75 24 70
-f 37 15 7
-f 37 60 7
-f 37 15 71
-f 67 3 17
-f 67 60 17
-f 67 3 71
-f 67 37 71
-f 67 37 60
-f 20 58 38
-f 20 36 38
-f 20 36 70
-f 20 43 70
-f 20 43 58
-f 54 8 14
-f 54 8 19
-f 54 41 19
-f 78 32 4
-f 66 12 1
-f 66 32 1
-f 63 54 14
-f 63 54 51
-f 48 32 4
-f 45 19 18
-f 45 41 19
-f 65 76 30
-f 65 18 23
-f 22 58 38
-f 57 7 23
-f 57 76 7
-f 57 65 23
-f 57 65 76
-f 5 17 26
-f 5 55 26
-f 5 55 17
-f 27 60 7
-f 27 75 7
-f 27 75 60
-f 40 24 7
-f 40 76 7
-f 13 36 38
-f 13 30 38
-f 13 24 70
-f 13 36 70
-f 13 40 24
-f 13 76 30
-f 13 40 76
-f 73 54 51
-f 73 54 41
-f 68 4 62
-f 68 78 4
-f 68 32 62
-f 68 78 32
-f 11 43 4
-f 11 48 4
-f 74 63 14
-f 35 45 18
-f 35 45 41
-f 46 65 30
-f 46 30 38
-f 46 41 38
-f 46 35 41
-f 46 65 18
-f 46 35 18
-f 10 22 58
-f 10 73 41
-f 10 41 38
-f 10 22 38
-f 64 11 48
-f 64 43 58
-f 64 11 43
-f 31 48 32
-f 31 74 32
-f 61 74 63
-f 42 74 14
-f 42 8 14
-f 42 8 12
-f 42 66 12
-f 42 66 32
-f 42 74 32
-f 39 64 48
-f 39 31 48
-f 39 31 74
-f 39 61 74
-f 49 39 64
-f 49 39 61
-f 49 10 58
-f 49 64 58
-f 49 61 63
-f 49 63 51
-f 49 73 51
-f 49 10 73
-g
+f 34 7 23
+f 53 56 33
+f 52 1 62
+f 52 2 56
+f 72 18 23
+f 72 34 23
+f 16 2 56
+f 16 34 71
+f 15 34 71
+f 15 34 7
+f 25 17 70
+f 4 17 26
+f 4 25 17
+f 6 2 62
+f 6 52 62
+f 6 52 2
+f 50 53 1
+f 50 52 1
+f 50 53 56
+f 50 52 56
+f 69 53 1
+f 69 12 1
+f 19 12 18
+f 29 56 33
+f 3 47 71
+f 44 16 71
+f 44 47 71
+f 77 4 62
+f 77 16 2
+f 77 3 47
+f 77 44 16
+f 77 44 47
+f 32 1 62
+f 43 25 70
+f 43 4 25
+f 21 53 33
+f 21 69 53
+f 8 19 12
+f 28 29 72
+f 28 72 34
+f 28 16 34
+f 28 16 56
+f 28 29 56
+f 59 72 18
+f 59 29 72
+f 59 12 18
+f 59 69 12
+f 59 21 69
+f 59 29 33
+f 59 21 33
+f 55 3 17
+f 55 4 26
+f 55 77 4
+f 55 77 3
+f 9 2 62
+f 9 77 62
+f 9 77 2
+f 75 60 17
+f 75 24 7
+f 75 17 70
+f 75 24 70
+f 37 15 7
+f 37 60 7
+f 37 15 71
+f 67 3 17
+f 67 60 17
+f 67 3 71
+f 67 37 71
+f 67 37 60
+f 20 58 38
+f 20 36 38
+f 20 36 70
+f 20 43 70
+f 20 43 58
+f 54 8 14
+f 54 8 19
+f 54 41 19
+f 78 32 4
+f 66 12 1
+f 66 32 1
+f 63 54 14
+f 63 54 51
+f 48 32 4
+f 45 19 18
+f 45 41 19
+f 65 76 30
+f 65 18 23
+f 22 58 38
+f 57 7 23
+f 57 76 7
+f 57 65 23
+f 57 65 76
+f 5 17 26
+f 5 55 26
+f 5 55 17
+f 27 60 7
+f 27 75 7
+f 27 75 60
+f 40 24 7
+f 40 76 7
+f 13 36 38
+f 13 30 38
+f 13 24 70
+f 13 36 70
+f 13 40 24
+f 13 76 30
+f 13 40 76
+f 73 54 51
+f 73 54 41
+f 68 4 62
+f 68 78 4
+f 68 32 62
+f 68 78 32
+f 11 43 4
+f 11 48 4
+f 74 63 14
+f 35 45 18
+f 35 45 41
+f 46 65 30
+f 46 30 38
+f 46 41 38
+f 46 35 41
+f 46 65 18
+f 46 35 18
+f 10 22 58
+f 10 73 41
+f 10 41 38
+f 10 22 38
+f 64 11 48
+f 64 43 58
+f 64 11 43
+f 31 48 32
+f 31 74 32
+f 61 74 63
+f 42 74 14
+f 42 8 14
+f 42 8 12
+f 42 66 12
+f 42 66 32
+f 42 74 32
+f 39 64 48
+f 39 31 48
+f 39 31 74
+f 39 61 74
+f 49 39 64
+f 49 39 61
+f 49 10 58
+f 49 64 58
+f 49 61 63
+f 49 63 51
+f 49 73 51
+f 49 10 73
+g
diff --git a/data/relative_effector_positions/anymal_RH_ADAPTER_TO_FOOT_constraints_in_RFleg.obj b/data/relative_effector_positions/anymal_RH_ADAPTER_TO_FOOT_constraints_in_RFleg.obj
index 1a943fead6cb99bc997e54d6e9eab211040836d5..3736a7b659bb677b6c8c84c5bd4233b6bd441f3b 100644
--- a/data/relative_effector_positions/anymal_RH_ADAPTER_TO_FOOT_constraints_in_RFleg.obj
+++ b/data/relative_effector_positions/anymal_RH_ADAPTER_TO_FOOT_constraints_in_RFleg.obj
@@ -1,202 +1,202 @@
-v -0.7995008890889097 0.037430715321231656 0.25778026294319334
-v -0.6299239151041307 -0.2526087975231621 -0.13376235700745231
-v -0.5011281235480092 -0.5527318205813154 0.2045760630002587
-v -0.8085414732955162 -0.04055738868164677 0.2726520379552897
-v -0.34040344016935264 -0.5437123067289984 0.18471109032792854
-v -1.0755576163332368 -0.33629494157668505 0.2055362956311268
-v -0.25312437756353157 0.41502767833850596 0.02211863848968071
-v -0.11894810270688218 -0.5082609391049642 0.14815942914660168
-v 0.07754358345760307 0.2744658636857253 -0.1324913278281874
-v -0.6733221971209462 0.46947193242845714 0.04004424038823595
-v -1.0855099591527342 0.21031823588232634 -0.040778569396475395
-v -0.8213564076728714 -0.23556957018493824 0.2950534015121074
-v -0.1594168259050384 0.20808749336280918 0.10424805072223242
-v -0.1053574938741048 0.5081522914191288 -0.11864028796152531
-v -0.7972107882122684 -0.30589826958651467 -0.12958175405604855
-v -0.894974437673588 0.2050254219834608 0.15109311917611606
-v -0.8272939072185617 0.07425641631609514 -0.3092837983625556
-v -0.8707192934976812 -0.4990345156492644 0.22633811583011876
-v -0.8716034344303767 0.4482151383398527 -0.31239788590715745
-v -1.0520985143578243 -0.32788638640667467 0.0035506155481735413
-v -0.1767198262154077 0.30313896785805855 -0.23879874341489826
-v -1.0850327552724046 0.3102265035384123 -0.13841747841248053
-v 0.16416707026263902 0.04618063273405393 -0.004678686494098416
-v -0.8896048142065289 0.408025913231162 0.02467495572200018
-v -0.8050630350928981 -0.49480026224647794 0.026625747904963193
-v -1.114143213800602 0.03580076704606726 0.04275051237836108
-v -0.6772087100267056 0.34963359565274765 -0.32576161812259974
-v -0.18884534383112048 -0.14624670231384285 0.23366504479883415
-v -1.0753993718633668 -0.37252495806151603 0.11617972082102179
-v -0.5540419649359039 0.5664645894264464 -0.08116530800949351
-v -0.32392736394976446 -0.4648372280272943 0.02903530301019902
-v -0.9804861530431368 -0.17675142879778688 -0.1608353785359427
-v -0.6331739205463829 0.3850193700992113 0.08724943704258281
-v -1.1276494387340228 0.1736006404963331 -0.09789140570691213
-v -0.8988434992584291 -0.19207694092514377 -0.18390739766541975
-v -1.0569887327635274 0.3003634650779785 -0.06237004934330681
-v -0.7468032310959294 -0.38891436026584136 0.295840139686222
-v -0.3206510007580119 -0.5635453149472821 0.08893211260306277
-v -0.9847730004868003 0.24630042012534034 0.05506022496627527
-v 0.080612096031309 -0.12622324511785143 -0.042570667518200433
-v -0.624782895923035 -0.5273383585149805 0.26202982450305445
-v -0.4637188649895066 0.5311404795700341 -0.20736752216092996
-v -0.11082502642105899 -0.08550067915111376 0.1832756005763544
-v -1.0604201755828075 0.20633833898211373 0.016194040120910635
-v -0.25311159363727137 -0.3201359797202251 0.28638023264881574
-v 0.06140716536537011 -0.11620931674058455 0.06407797952816924
-v -0.7134509715421731 -0.5407671295601071 0.0914767475520532
-v -0.9544433178916244 0.4989982113142462 -0.0892614045652631
-v 0.039051749144813575 -0.329384152787916 0.01675149880407273
-v -0.6264908425117959 0.6088651101787147 -0.1302381567807287
-v -1.0844144570054575 -0.27604844986444926 0.016696530182826186
-v -0.9356440545270603 -0.32563900132165785 -0.08698485680697388
-v -1.0265837239303774 0.4035704625535096 -0.13424644125130802
-v -1.1082398587642468 0.13201077384134258 -0.17334059391579934
-v -0.9889888375644555 -0.06290275164087944 -0.19237260056584843
-v -0.8436163000991587 0.3010668564505948 0.10826224385781125
-v -0.13675266769067246 -0.22829098833207687 0.20500770473868363
-v -1.0707103395745001 -0.23513475477780407 0.19999303857692147
-v -0.6982681731203968 0.5195829663064552 -0.0025317547941619
-v -0.7693130147970225 0.2616839496990216 0.14861252050685358
-v -0.9541841875935588 0.39938505785874406 -0.2564313508783953
-v -0.7457778792832361 0.355847626314003 0.0987946703231401
-v -0.16696490085997173 0.06077350777958962 -0.18625132560518443
-v -1.0469293334557639 0.2532589528190562 -0.21014577254574457
-v -1.0562675660410572 -0.3857101520806666 0.042829050767627275
-v -0.530136313327302 -0.5008234031854517 0.015536390185122095
-v -0.8801440958947598 -0.5003760144285251 0.22142196540086112
-v -0.4567063356442729 -0.5716089999383555 0.1454220031117277
+v -0.7995008890889097 0.037430715321231656 0.25778026294319334
+v -0.6299239151041307 -0.2526087975231621 -0.13376235700745231
+v -0.5011281235480092 -0.5527318205813154 0.2045760630002587
+v -0.8085414732955162 -0.04055738868164677 0.2726520379552897
+v -0.34040344016935264 -0.5437123067289984 0.18471109032792854
+v -1.0755576163332368 -0.33629494157668505 0.2055362956311268
+v -0.25312437756353157 0.41502767833850596 0.02211863848968071
+v -0.11894810270688218 -0.5082609391049642 0.14815942914660168
+v 0.07754358345760307 0.2744658636857253 -0.1324913278281874
+v -0.6733221971209462 0.46947193242845714 0.04004424038823595
+v -1.0855099591527342 0.21031823588232634 -0.040778569396475395
+v -0.8213564076728714 -0.23556957018493824 0.2950534015121074
+v -0.1594168259050384 0.20808749336280918 0.10424805072223242
+v -0.1053574938741048 0.5081522914191288 -0.11864028796152531
+v -0.7972107882122684 -0.30589826958651467 -0.12958175405604855
+v -0.894974437673588 0.2050254219834608 0.15109311917611606
+v -0.8272939072185617 0.07425641631609514 -0.3092837983625556
+v -0.8707192934976812 -0.4990345156492644 0.22633811583011876
+v -0.8716034344303767 0.4482151383398527 -0.31239788590715745
+v -1.0520985143578243 -0.32788638640667467 0.0035506155481735413
+v -0.1767198262154077 0.30313896785805855 -0.23879874341489826
+v -1.0850327552724046 0.3102265035384123 -0.13841747841248053
+v 0.16416707026263902 0.04618063273405393 -0.004678686494098416
+v -0.8896048142065289 0.408025913231162 0.02467495572200018
+v -0.8050630350928981 -0.49480026224647794 0.026625747904963193
+v -1.114143213800602 0.03580076704606726 0.04275051237836108
+v -0.6772087100267056 0.34963359565274765 -0.32576161812259974
+v -0.18884534383112048 -0.14624670231384285 0.23366504479883415
+v -1.0753993718633668 -0.37252495806151603 0.11617972082102179
+v -0.5540419649359039 0.5664645894264464 -0.08116530800949351
+v -0.32392736394976446 -0.4648372280272943 0.02903530301019902
+v -0.9804861530431368 -0.17675142879778688 -0.1608353785359427
+v -0.6331739205463829 0.3850193700992113 0.08724943704258281
+v -1.1276494387340228 0.1736006404963331 -0.09789140570691213
+v -0.8988434992584291 -0.19207694092514377 -0.18390739766541975
+v -1.0569887327635274 0.3003634650779785 -0.06237004934330681
+v -0.7468032310959294 -0.38891436026584136 0.295840139686222
+v -0.3206510007580119 -0.5635453149472821 0.08893211260306277
+v -0.9847730004868003 0.24630042012534034 0.05506022496627527
+v 0.080612096031309 -0.12622324511785143 -0.042570667518200433
+v -0.624782895923035 -0.5273383585149805 0.26202982450305445
+v -0.4637188649895066 0.5311404795700341 -0.20736752216092996
+v -0.11082502642105899 -0.08550067915111376 0.1832756005763544
+v -1.0604201755828075 0.20633833898211373 0.016194040120910635
+v -0.25311159363727137 -0.3201359797202251 0.28638023264881574
+v 0.06140716536537011 -0.11620931674058455 0.06407797952816924
+v -0.7134509715421731 -0.5407671295601071 0.0914767475520532
+v -0.9544433178916244 0.4989982113142462 -0.0892614045652631
+v 0.039051749144813575 -0.329384152787916 0.01675149880407273
+v -0.6264908425117959 0.6088651101787147 -0.1302381567807287
+v -1.0844144570054575 -0.27604844986444926 0.016696530182826186
+v -0.9356440545270603 -0.32563900132165785 -0.08698485680697388
+v -1.0265837239303774 0.4035704625535096 -0.13424644125130802
+v -1.1082398587642468 0.13201077384134258 -0.17334059391579934
+v -0.9889888375644555 -0.06290275164087944 -0.19237260056584843
+v -0.8436163000991587 0.3010668564505948 0.10826224385781125
+v -0.13675266769067246 -0.22829098833207687 0.20500770473868363
+v -1.0707103395745001 -0.23513475477780407 0.19999303857692147
+v -0.6982681731203968 0.5195829663064552 -0.0025317547941619
+v -0.7693130147970225 0.2616839496990216 0.14861252050685358
+v -0.9541841875935588 0.39938505785874406 -0.2564313508783953
+v -0.7457778792832361 0.355847626314003 0.0987946703231401
+v -0.16696490085997173 0.06077350777958962 -0.18625132560518443
+v -1.0469293334557639 0.2532589528190562 -0.21014577254574457
+v -1.0562675660410572 -0.3857101520806666 0.042829050767627275
+v -0.530136313327302 -0.5008234031854517 0.015536390185122095
+v -0.8801440958947598 -0.5003760144285251 0.22142196540086112
+v -0.4567063356442729 -0.5716089999383555 0.1454220031117277
g foo
-f 54 19 17
-f 41 45 37
-f 41 67 68
-f 41 3 68
-f 56 24 16
-f 22 54 34
-f 44 58 16
-f 6 67 29
-f 43 45 23
-f 43 45 28
-f 46 8 23
-f 46 45 8
-f 62 24 10
-f 62 56 24
-f 13 43 28
-f 13 43 23
-f 47 67 68
-f 20 32 54
-f 18 41 37
-f 18 41 67
-f 18 6 37
-f 18 6 67
-f 5 45 8
-f 5 41 45
-f 5 41 3
-f 5 3 68
-f 9 40 23
-f 9 63 40
-f 64 54 19
-f 64 22 54
-f 48 50 19
-f 26 44 34
-f 26 44 58
-f 26 6 58
-f 26 29 34
-f 26 6 29
-f 39 24 16
-f 39 44 16
-f 39 48 24
-f 39 48 44
-f 11 44 34
-f 57 45 23
-f 57 46 23
-f 57 46 45
-f 33 13 7
-f 33 62 10
-f 33 7 10
-f 1 13 28
-f 1 33 13
-f 1 45 28
-f 1 58 16
-f 55 54 17
-f 55 32 17
-f 55 32 54
-f 25 47 67
-f 25 15 52
-f 35 32 17
-f 35 15 17
-f 35 32 52
-f 35 15 52
-f 49 63 17
-f 49 63 40
-f 49 8 23
-f 49 40 23
-f 51 20 54
-f 51 29 34
-f 51 54 34
-f 14 9 23
-f 14 13 23
-f 14 13 7
-f 14 30 7
-f 14 30 50
-f 21 9 63
-f 21 63 17
-f 21 14 9
-f 61 64 19
-f 61 64 22
-f 59 48 50
-f 59 30 50
-f 59 24 10
-f 59 48 24
-f 59 7 10
-f 59 30 7
-f 36 48 22
-f 36 48 44
-f 36 11 44
-f 36 22 34
-f 36 11 34
-f 60 33 62
-f 60 1 33
-f 60 1 16
-f 60 56 16
-f 60 62 56
-f 12 45 37
-f 12 6 37
-f 12 6 58
-f 65 67 29
-f 65 25 67
-f 65 51 29
-f 65 51 20
-f 65 25 52
-f 65 32 52
-f 65 20 32
-f 2 15 17
-f 2 49 17
-f 31 2 49
-f 42 21 14
-f 42 50 19
-f 42 14 50
-f 53 48 22
-f 53 61 22
-f 53 48 19
-f 53 61 19
-f 4 1 45
-f 4 12 45
-f 4 1 58
-f 4 12 58
-f 38 49 8
-f 38 31 49
-f 38 5 68
-f 38 5 8
-f 38 47 68
-f 38 25 47
-f 27 42 19
-f 27 42 21
-f 27 19 17
-f 27 21 17
-f 66 38 31
-f 66 2 15
-f 66 31 2
-f 66 25 15
-f 66 38 25
-g
+f 54 19 17
+f 41 45 37
+f 41 67 68
+f 41 3 68
+f 56 24 16
+f 22 54 34
+f 44 58 16
+f 6 67 29
+f 43 45 23
+f 43 45 28
+f 46 8 23
+f 46 45 8
+f 62 24 10
+f 62 56 24
+f 13 43 28
+f 13 43 23
+f 47 67 68
+f 20 32 54
+f 18 41 37
+f 18 41 67
+f 18 6 37
+f 18 6 67
+f 5 45 8
+f 5 41 45
+f 5 41 3
+f 5 3 68
+f 9 40 23
+f 9 63 40
+f 64 54 19
+f 64 22 54
+f 48 50 19
+f 26 44 34
+f 26 44 58
+f 26 6 58
+f 26 29 34
+f 26 6 29
+f 39 24 16
+f 39 44 16
+f 39 48 24
+f 39 48 44
+f 11 44 34
+f 57 45 23
+f 57 46 23
+f 57 46 45
+f 33 13 7
+f 33 62 10
+f 33 7 10
+f 1 13 28
+f 1 33 13
+f 1 45 28
+f 1 58 16
+f 55 54 17
+f 55 32 17
+f 55 32 54
+f 25 47 67
+f 25 15 52
+f 35 32 17
+f 35 15 17
+f 35 32 52
+f 35 15 52
+f 49 63 17
+f 49 63 40
+f 49 8 23
+f 49 40 23
+f 51 20 54
+f 51 29 34
+f 51 54 34
+f 14 9 23
+f 14 13 23
+f 14 13 7
+f 14 30 7
+f 14 30 50
+f 21 9 63
+f 21 63 17
+f 21 14 9
+f 61 64 19
+f 61 64 22
+f 59 48 50
+f 59 30 50
+f 59 24 10
+f 59 48 24
+f 59 7 10
+f 59 30 7
+f 36 48 22
+f 36 48 44
+f 36 11 44
+f 36 22 34
+f 36 11 34
+f 60 33 62
+f 60 1 33
+f 60 1 16
+f 60 56 16
+f 60 62 56
+f 12 45 37
+f 12 6 37
+f 12 6 58
+f 65 67 29
+f 65 25 67
+f 65 51 29
+f 65 51 20
+f 65 25 52
+f 65 32 52
+f 65 20 32
+f 2 15 17
+f 2 49 17
+f 31 2 49
+f 42 21 14
+f 42 50 19
+f 42 14 50
+f 53 48 22
+f 53 61 22
+f 53 48 19
+f 53 61 19
+f 4 1 45
+f 4 12 45
+f 4 1 58
+f 4 12 58
+f 38 49 8
+f 38 31 49
+f 38 5 68
+f 38 5 8
+f 38 47 68
+f 38 25 47
+f 27 42 19
+f 27 42 21
+f 27 19 17
+f 27 21 17
+f 66 38 31
+f 66 2 15
+f 66 31 2
+f 66 25 15
+f 66 38 25
+g
diff --git a/pyproject.toml b/pyproject.toml
new file mode 100644
index 0000000000000000000000000000000000000000..7ad22b44c945457602878cb6d16e799856c6634f
--- /dev/null
+++ b/pyproject.toml
@@ -0,0 +1,2 @@
+[tool.black]
+exclude = "cmake"
diff --git a/script/relative_foot_positions/constants_and_tools.py b/script/relative_foot_positions/constants_and_tools.py
index 890906ea707b1b3f4db6d6bacfc8d6e010ba6b08..3f9dbaacf0baeef6393c09693950eb3793aeb307 100644
--- a/script/relative_foot_positions/constants_and_tools.py
+++ b/script/relative_foot_positions/constants_and_tools.py
@@ -1,20 +1,23 @@
# from hpp.corbaserver.rbprm.hrp2 import Robot as rob
-# from hpp.corbaserver.rbprm.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities
+# from hpp.corbaserver.rbprm.tools.obj_to_constraints import load_obj,
+# as_inequalities, rotate_inequalities
# from hpp_centroidal_dynamics import *
# from hpp_spline import *e
from numpy import array, matrix, zeros, ones, vstack, hstack, identity, concatenate
import numpy as np
from scipy.spatial import ConvexHull
+
# from hpp_bezier_com_traj import *
# from qp import solve_lp
# import eigenpy
import cdd
+
# from curves import bezier3
-Id = matrix([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
-z = array([0., 0., 1.])
+Id = matrix([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
+z = array([0.0, 0.0, 1.0])
zero3 = zeros(3)
@@ -34,7 +37,7 @@ def generators(A, b, Aeq=None, beq=None):
def filter(pts):
- hull = ConvexHull(pts, qhull_options='Q12')
+ hull = ConvexHull(pts, qhull_options="Q12")
return [pts[i] for i in hull.vertices.tolist()]
@@ -102,13 +105,18 @@ def convex_hull_ineq(pts, cData=None, ineqFromCdata=None, gX=None, g=None, w=Non
D = zeros((6, 3))
D[3:, :] = m * gX
- d = zeros((6, ))
+ d = zeros((6,))
d[:3] = -m * g
A = H.dot(D)
- b = h.reshape((-1, )) - H.dot(d)
+ b = h.reshape((-1,)) - H.dot(d)
# add kinematic polytope
- (K, k) = (cData.Kin_[0], cData.Kin_[1].reshape(-1, ))
+ (K, k) = (
+ cData.Kin_[0],
+ cData.Kin_[1].reshape(
+ -1,
+ ),
+ )
resA = vstack([A, K])
resb = concatenate([b, k]).reshape((-1, 1))
@@ -118,11 +126,17 @@ def convex_hull_ineq(pts, cData=None, ineqFromCdata=None, gX=None, g=None, w=Non
error = False
for pt in allpts:
print("pt ", pt)
- assert (resA.dot(pt.reshape((-1, 1))) - resb).max() < 0.001, "antecedent point not in End polytope" + str(
- (resA.dot(pt.reshape((-1, 1))) - resb).max())
+ assert (
+ resA.dot(pt.reshape((-1, 1))) - resb
+ ).max() < 0.001, "antecedent point not in End polytope" + str(
+ (resA.dot(pt.reshape((-1, 1))) - resb).max()
+ )
if (H.dot(w(m, pt).reshape((-1, 1))) - h).max() > 0.001:
error = True
- print("antecedent point not in End polytope" + str((H.dot(w(m, pt).reshape((-1, 1))) - h).max()))
+ print(
+ "antecedent point not in End polytope"
+ + str((H.dot(w(m, pt).reshape((-1, 1))) - h).max())
+ )
assert not error, str(len(allpts))
return (resA, resb)
@@ -133,7 +147,7 @@ def convex_hull_ineq(pts, cData=None, ineqFromCdata=None, gX=None, g=None, w=Non
def default_transform_from_pos_normal(pos, normal):
# print "pos ", pos
# print "normal ", normal
- f = array([0., 0., 1.])
+ f = array([0.0, 0.0, 1.0])
t = array(normal)
v = np.cross(f, t)
c = np.dot(f, t)
@@ -141,13 +155,17 @@ def default_transform_from_pos_normal(pos, normal):
rot = identity(3)
else:
# u = v / norm(v)
- h = (1. - c) / (1. - c**2)
+ h = (1.0 - c) / (1.0 - c**2)
vx, vy, vz = v
- rot = array([[c + h * vx**2, h * vx * vy - vz, h * vx * vz + vy],
- [h * vx * vy + vz, c + h * vy**2, h * vy * vz - vx],
- [h * vx * vz - vy, h * vy * vz + vx, c + h * vz**2]])
- return vstack([hstack([rot, pos.reshape((-1, 1))]), [0., 0., 0., 1.]])
+ rot = array(
+ [
+ [c + h * vx**2, h * vx * vy - vz, h * vx * vz + vy],
+ [h * vx * vy + vz, c + h * vy**2, h * vy * vz - vx],
+ [h * vx * vz - vy, h * vy * vz + vx, c + h * vz**2],
+ ]
+ )
+ return vstack([hstack([rot, pos.reshape((-1, 1))]), [0.0, 0.0, 0.0, 1.0]])
def continuous(h, initpts):
@@ -168,17 +186,18 @@ def hull_to_obj(h, pts, name):
# first write points
for pt in pts:
# print "??"
- f.write('v ' + str(pt[0]) + ' ' + str(pt[1]) + ' ' + str(pt[2]) + ' \n')
- f.write('g foo\n')
+ f.write("v " + str(pt[0]) + " " + str(pt[1]) + " " + str(pt[2]) + " \n")
+ f.write("g foo\n")
for pt in faces:
# print "???"
- f.write('f ' + str(pt[0]) + ' ' + str(pt[1]) + ' ' + str(pt[2]) + ' \n')
- f.write('g \n')
+ f.write("f " + str(pt[0]) + " " + str(pt[1]) + " " + str(pt[2]) + " \n")
+ f.write("g \n")
f.close()
# function vertface2obj(v,f,name)
-# % VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
+# % VERTFACE2OBJ Save a set of vertice coordinates and faces
+# as a Wavefront/Alias Obj file
# % VERTFACE2OBJ(v,f,fname)
# % v is a Nx3 matrix of vertex coordinates.
# % f is a Mx3 matrix of vertex indices.
diff --git a/script/relative_foot_positions/gen_and_copy.sh b/script/relative_foot_positions/gen_and_copy.sh
index 4cc97109faf23c14ff01f320cd22055b004f9286..d9bdabeb025756ca874a73f61f792a9979f5c624 100644
--- a/script/relative_foot_positions/gen_and_copy.sh
+++ b/script/relative_foot_positions/gen_and_copy.sh
@@ -1,4 +1,4 @@
-#!/bin/bash
+#!/bin/bash
cd /media/data/dev/linux/hpp/src/anymal-rbprm/script/relative_foot_positions;
./run.sh relativeFootPositionQuasiFlat.py
diff --git a/script/relative_foot_positions/obj_to_constraints.py b/script/relative_foot_positions/obj_to_constraints.py
index 053635c230a5db84b550e51e8efec47210cd2034..ccb031428d971a10c9ae6f189e5ded32c85a225a 100644
--- a/script/relative_foot_positions/obj_to_constraints.py
+++ b/script/relative_foot_positions/obj_to_constraints.py
@@ -22,20 +22,20 @@ def load_obj(filename):
fh = open(filename)
for line in fh:
- if line[0] == '#':
+ if line[0] == "#":
continue
- line = line.strip().split(' ')
- if line[0] == 'v': # vertex
+ line = line.strip().split(" ")
+ if line[0] == "v": # vertex
V.append(toFloat(line[1:]))
- elif line[0] == 'vt': # tex-coord
+ elif line[0] == "vt": # tex-coord
T.append(line[1:])
- elif line[0] == 'vn': # normal vector
+ elif line[0] == "vn": # normal vector
N.append(toFloat(line[1:]))
- elif line[0] == 'f': # face
+ elif line[0] == "f": # face
face = line[1:]
for i in range(0, len(face)):
- face[i] = face[i].split('/')
+ face[i] = face[i].split("/")
# OBJ indexies are 1 based not 0 based hence the -1
# convert indexies to integer
for j in range(0, len(face[i])):
@@ -100,16 +100,16 @@ def rotate_inequalities(ineq, transform):
def ineq_to_file(ineq, filename):
- f1 = open(filename, 'w+')
- res = {'A': ineq.A, 'b': ineq.b, 'N': ineq.N, 'V': ineq.V}
+ f1 = open(filename, "w+")
+ res = {"A": ineq.A, "b": ineq.b, "N": ineq.N, "V": ineq.V}
dump(res, f1)
f1.close()
def ineq_from_file(filename):
- f1 = open(filename, 'r')
+ f1 = open(filename, "r")
res = load(f1)
- return Inequalities(res['A'], res['b'], res['N'], res['V'])
+ return Inequalities(res["A"], res["b"], res["N"], res["V"])
def test_inequality():
@@ -122,7 +122,7 @@ def test_inequality():
def __gen_data():
- obj = load_obj('./hrp2/RL_com._reduced.obj')
+ obj = load_obj("./hrp2/RL_com._reduced.obj")
ineq = as_inequalities(obj)
ok_points = [[0, 0, 0], [0.0813, 0.0974, 0.2326], [-0.3387, 0.1271, -0.5354]]
not_ok_points = [[-0.3399, 0.2478, -0.722], [-0.1385, -0.4401, -0.1071]]
@@ -135,25 +135,37 @@ def test_belonging():
ok_points = data[2]
not_ok_points = data[3]
for p in ok_points:
- assert (is_inside(ineq, np.array(p))), "point " + str(p) + " should be inside object"
+ assert is_inside(ineq, np.array(p)), (
+ "point " + str(p) + " should be inside object"
+ )
for p in not_ok_points:
- assert (not is_inside(ineq, np.array(p))), "point " + str(p) + " should NOT be inside object"
+ assert not is_inside(ineq, np.array(p)), (
+ "point " + str(p) + " should NOT be inside object"
+ )
print("test_belonging successful")
def test_rotate_inequalities():
- tr = np.array([[1., 0., 0., 0.], [0., 0.98006658, -0.19866933, 2.], [0., 0.19866933, 0.98006658, 0.],
- [0., 0., 0., 1.]])
+ tr = np.array(
+ [
+ [1.0, 0.0, 0.0, 0.0],
+ [0.0, 0.98006658, -0.19866933, 2.0],
+ [0.0, 0.19866933, 0.98006658, 0.0],
+ [0.0, 0.0, 0.0, 1.0],
+ ]
+ )
data = __gen_data()
ineq = rotate_inequalities(data[1], tr)
ok_points = [tr.dot(np.array(el + [1]))[0:3] for el in data[2]]
not_ok_points = [tr.dot(np.array(el + [1]))[0:3] for el in data[3]]
for p in ok_points:
- assert (is_inside(ineq, p)), "point " + str(p) + " should be inside object"
+ assert is_inside(ineq, p), "point " + str(p) + " should be inside object"
for p in not_ok_points:
- assert (not is_inside(ineq, p)), "point " + str(p) + " should NOT be inside object"
+ assert not is_inside(ineq, p), (
+ "point " + str(p) + " should NOT be inside object"
+ )
print("test_rotate_inequalities successful")
@@ -163,7 +175,7 @@ def load_obj_and_save_ineq(in_name, out_name):
ineq_to_file(ineq, out_name)
-load_obj_and_save_ineq('./lfleg_com_reduced.obj', './lfleg_com.ineq')
-load_obj_and_save_ineq('./lhleg_com_reduced.obj', './lhleg_com.ineq')
-load_obj_and_save_ineq('./rhleg_com_reduced.obj', './rhleg_com.ineq')
-load_obj_and_save_ineq('./rfleg_com_reduced.obj', './rfleg_com.ineq')
+load_obj_and_save_ineq("./lfleg_com_reduced.obj", "./lfleg_com.ineq")
+load_obj_and_save_ineq("./lhleg_com_reduced.obj", "./lhleg_com.ineq")
+load_obj_and_save_ineq("./rhleg_com_reduced.obj", "./rhleg_com.ineq")
+load_obj_and_save_ineq("./rfleg_com_reduced.obj", "./rfleg_com.ineq")
diff --git a/script/relative_foot_positions/plot_polytopes.py b/script/relative_foot_positions/plot_polytopes.py
index 322329df0aac9706121654d8c2785f650be7325a..3d48a6a8b8d9381ae577980cd23fbb807e73689c 100644
--- a/script/relative_foot_positions/plot_polytopes.py
+++ b/script/relative_foot_positions/plot_polytopes.py
@@ -1,4 +1,5 @@
import numpy as np
+
# from hpp_centroidal_dynamics import *
# from hpp_spline import *
from numpy import array
@@ -8,6 +9,7 @@ from constants_and_tools import genPolytope
import matplotlib.pyplot as plt
from scipy.spatial import ConvexHull
+
# from hpp_bezier_com_traj import *
# from qp import solve_lp
@@ -23,7 +25,9 @@ def plot_hull_in_subplot(hull, pts, apts, ax, color="r", just_pts=False):
ax.plot(apts[s, 0], apts[s, 1], apts[s, 2], color + "-")
-def plot_hull(hull, pts, apts, color="r", just_pts=False, plot=False, fig=None, ax=None):
+def plot_hull(
+ hull, pts, apts, color="r", just_pts=False, plot=False, fig=None, ax=None
+):
if fig is None:
fig = plt.figure()
if ax is None:
@@ -44,7 +48,7 @@ def plot_polytope_H_rep(A_in, b_in, color="r", just_pts=False):
def plot_polytope_V_rep(pts, color="r", just_pts=False):
pts = [array(el) for el in pts]
apts = array(pts)
- hull = ConvexHull(apts, qhull_options='Q12')
+ hull = ConvexHull(apts, qhull_options="Q12")
plot_hull(hull, pts, apts, color, just_pts)
diff --git a/script/relative_foot_positions/quaternion.py b/script/relative_foot_positions/quaternion.py
index 5eb6cfa7aa57ea08c76e43baddf0fd2a484159c7..46a070cc557f3a6765cfc663b79d652d42bbaf23 100644
--- a/script/relative_foot_positions/quaternion.py
+++ b/script/relative_foot_positions/quaternion.py
@@ -38,6 +38,7 @@ class Quaternion(object):
It can also return a rotation vector, a rotation matrix, or a SO3
(see the methods : to...() for more information).
"""
+
def __init__(self, *args):
"""
Instanciation of the quaternion with 1, 2 or 4 arguments :
@@ -89,7 +90,7 @@ class Quaternion(object):
error = False
if len(args) == 0: # By default, if no argument is given
- self.array = np.array([1., 0., 0., 0.])
+ self.array = np.array([1.0, 0.0, 0.0, 0.0])
elif len(args) == 4: # From 4 elements
if np.array(args).size == 4:
self.array = np.double(np.array(args))
@@ -101,23 +102,34 @@ class Quaternion(object):
elif np.array(args[0]).size == 1:
# From one sized element, this element will be the scalar part,
# the vector part will be set at (0,0,0)
- self.array = np.double(np.hstack([np.array(args[0]), np.array([0, 0, 0])]))
- elif np.array(args[0]).size == 4 and max(np.array(
- args[0]).shape) == 4: # From an array, matrix, tuple or list of 4 elements
- self.array = np.double(np.array(args[0])).reshape(4, )
- elif np.array(args[0]).size == 3 and max(
- np.array(args[0]).shape
- ) == 3: # From an array, matrix, tuple or list of 3 elements interpreted as a rotation vector
- rV = np.double(np.array(args[0])).reshape(3, )
+ self.array = np.double(
+ np.hstack([np.array(args[0]), np.array([0, 0, 0])])
+ )
+ elif (
+ np.array(args[0]).size == 4 and max(np.array(args[0]).shape) == 4
+ ): # From an array, matrix, tuple or list of 4 elements
+ self.array = np.double(np.array(args[0])).reshape(
+ 4,
+ )
+ elif np.array(args[0]).size == 3 and max(np.array(args[0]).shape) == 3:
+ # From an array, matrix, tuple or list of 3 elements
+ # interpreted as a rotation vector
+ rV = np.double(np.array(args[0])).reshape(
+ 3,
+ )
alpha = np.double(linalg.norm(rV))
if alpha != 0:
e = rV / alpha
else:
e = rV
- self.array = np.hstack([np.cos(alpha / 2.), np.sin(alpha / 2.) * e])
- elif len(np.array(
- args[0]).shape) == 2 and np.array(args[0]).shape[0] >= 3 and np.array(args[0]).shape[1] >= 3:
- # From a to 2 dimension array convertible array, matrix, tuple or list with at least (3*3)
+ self.array = np.hstack([np.cos(alpha / 2.0), np.sin(alpha / 2.0) * e])
+ elif (
+ len(np.array(args[0]).shape) == 2
+ and np.array(args[0]).shape[0] >= 3
+ and np.array(args[0]).shape[1] >= 3
+ ):
+ # From a to 2 dimension array convertible array, matrix,
+ # tuple or list with at least (3*3)
# elements interpreted as a rotation matrix
rM = np.double(np.array(args[0])[:3, :3])
selec = np.zeros(4)
@@ -160,7 +172,9 @@ class Quaternion(object):
error = True
else:
error = True
- elif len(args) == 2: # From a scalar part (1 element) and a vector part (3 elements)
+ elif (
+ len(args) == 2
+ ): # From a scalar part (1 element) and a vector part (3 elements)
arg0 = np.double(np.array(args[0]))
arg1 = np.double(np.array(args[1]))
if arg0.size == 1 and arg1.size == 3:
@@ -177,21 +191,23 @@ class Quaternion(object):
else:
error = True
- if not error and self.array.shape != (4, ):
+ if not error and self.array.shape != (4,):
del self.array
error = True
if error:
- raise TypeError("Impossible to instanciate the Quaternion object with the given arguments")
+ raise TypeError(
+ "Impossible to instanciate a Quaternion object with the given arguments"
+ )
def __str__(self):
"""
String representation of the quaternion.
"""
- aff = '[ '
- aff += str(self.array[0]) + ' + '
- aff += str(self.array[1]) + ' i + '
- aff += str(self.array[2]) + ' j + '
- aff += str(self.array[3]) + ' k ]'
+ aff = "[ "
+ aff += str(self.array[0]) + " + "
+ aff += str(self.array[1]) + " i + "
+ aff += str(self.array[2]) + " j + "
+ aff += str(self.array[3]) + " k ]"
return aff
def __neg__(self):
@@ -234,8 +250,11 @@ class Quaternion(object):
q2 = other
qr = np.zeros(4)
qr[0] = self.array[0] * q2.array[0] - np.vdot(self.array[1:], q2.array[1:])
- qr[1:4] = np.cross(self.array[1:4],
- q2.array[1:4]) + self.array[0] * q2.array[1:4] + q2.array[0] * self.array[1:4]
+ qr[1:4] = (
+ np.cross(self.array[1:4], q2.array[1:4])
+ + self.array[0] * q2.array[1:4]
+ + q2.array[0] * self.array[1:4]
+ )
return Quaternion(qr)
def __rmul__(self, other):
@@ -269,7 +288,7 @@ class Quaternion(object):
"""
Returns the inverse of the quaternion.
"""
- return Quaternion(self.conjugate().array / (abs(self)**2))
+ return Quaternion(self.conjugate().array / (abs(self) ** 2))
def __div__(self, other):
"""
@@ -317,15 +336,15 @@ class Quaternion(object):
"""
q = self.normalized().array
rm = np.zeros((3, 3))
- rm[0, 0] = 1 - 2 * (q[2]**2 + q[3]**2)
+ rm[0, 0] = 1 - 2 * (q[2] ** 2 + q[3] ** 2)
rm[0, 1] = 2 * q[1] * q[2] - 2 * q[0] * q[3]
rm[0, 2] = 2 * q[1] * q[3] + 2 * q[0] * q[2]
rm[1, 0] = 2 * q[1] * q[2] + 2 * q[0] * q[3]
- rm[1, 1] = 1 - 2 * (q[1]**2 + q[3]**2)
+ rm[1, 1] = 1 - 2 * (q[1] ** 2 + q[3] ** 2)
rm[1, 2] = 2 * q[2] * q[3] - 2 * q[0] * q[1]
rm[2, 0] = 2 * q[1] * q[3] - 2 * q[0] * q[2]
rm[2, 1] = 2 * q[2] * q[3] + 2 * q[0] * q[1]
- rm[2, 2] = 1 - 2 * (q[1]**2 + q[2]**2)
+ rm[2, 2] = 1 - 2 * (q[1] ** 2 + q[2] ** 2)
return rm
def toRotationVector(self):
@@ -363,11 +382,15 @@ class Quaternion(object):
followed by a rotation of R about the new x-axis.
"""
q = self.normalized().array
- r = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1]**2 + q[2]**2))
- p = np.arctan2(2 * (q[0] * q[2] - q[3] * q[1]),
- np.sqrt((2 * (q[0] * q[1] + q[2] * q[3]))**2 +
- (1 - 2 * (q[1]**2 + q[2]**2))**2)) # We cas use arcsin but arctan2 is more robust
- y = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2]**2 + q[3]**2))
+ r = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] ** 2 + q[2] ** 2))
+ p = np.arctan2(
+ 2 * (q[0] * q[2] - q[3] * q[1]),
+ np.sqrt(
+ (2 * (q[0] * q[1] + q[2] * q[3])) ** 2
+ + (1 - 2 * (q[1] ** 2 + q[2] ** 2)) ** 2
+ ),
+ ) # We cas use arcsin but arctan2 is more robust
+ y = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] ** 2 + q[3] ** 2))
return np.array([r, p, y])
def fromRPY(self, R, P, Y):
@@ -381,13 +404,21 @@ class Quaternion(object):
followed by a rotation of P about the new y-axis,
followed by a rotation of R about the new x-axis.
"""
- r = R / 2.
- p = P / 2.
- y = Y / 2.
- self.array[0] = np.cos(r) * np.cos(p) * np.cos(y) + np.sin(r) * np.sin(p) * np.sin(y)
- self.array[1] = np.sin(r) * np.cos(p) * np.cos(y) - np.cos(r) * np.sin(p) * np.sin(y)
- self.array[2] = np.cos(r) * np.sin(p) * np.cos(y) + np.sin(r) * np.cos(p) * np.sin(y)
- self.array[3] = np.cos(r) * np.cos(p) * np.sin(y) - np.sin(r) * np.sin(p) * np.cos(y)
+ r = R / 2.0
+ p = P / 2.0
+ y = Y / 2.0
+ self.array[0] = np.cos(r) * np.cos(p) * np.cos(y) + np.sin(r) * np.sin(
+ p
+ ) * np.sin(y)
+ self.array[1] = np.sin(r) * np.cos(p) * np.cos(y) - np.cos(r) * np.sin(
+ p
+ ) * np.sin(y)
+ self.array[2] = np.cos(r) * np.sin(p) * np.cos(y) + np.sin(r) * np.cos(
+ p
+ ) * np.sin(y)
+ self.array[3] = np.cos(r) * np.cos(p) * np.sin(y) - np.sin(r) * np.sin(
+ p
+ ) * np.cos(y)
return self.normalize()
def toTuple(self):
diff --git a/script/relative_foot_positions/reduce.py b/script/relative_foot_positions/reduce.py
index 1e0b55f695a1098d7722e0ed3734d8e748a1c9ff..5556fd1290aeb7fe777f9c76d5672d990ff02aa9 100644
--- a/script/relative_foot_positions/reduce.py
+++ b/script/relative_foot_positions/reduce.py
@@ -7,8 +7,10 @@ import os
#
# change those parameters according to your needs
-TARGET_NUM_FACES = 24.
-FOLDER_PATH = "/media/data/dev/linux/hpp/src/anymal-rbprm/script/relative_foot_positions/"
+TARGET_NUM_FACES = 24.0
+FOLDER_PATH = (
+ "/media/data/dev/linux/hpp/src/anymal-rbprm/script/relative_foot_positions/"
+)
OUTPUT_PATH = FOLDER_PATH + "output/"
EXTENSION = ".obj"
@@ -22,7 +24,7 @@ def decimate(obj):
def load_obj(file):
- # ~ bpy.ops.import_scene.obj(filepath=FOLDER_PATH+file, axis_forward='X', axis_up='Z')
+ # bpy.ops.import_scene.obj(filepath=FOLDER_PATH+file, axis_forward='X', axis_up='Z')
bpy.ops.import_scene.obj(filepath=FOLDER_PATH + file)
obj = bpy.data.objects[-1]
@@ -31,9 +33,9 @@ def load_obj(file):
# bpy.context.scene.objects.active = obj
bpy.context.view_layer.objects.active = obj
bpy.ops.object.editmode_toggle()
- bpy.ops.mesh.delete(type='EDGE_FACE')
+ bpy.ops.mesh.delete(type="EDGE_FACE")
bpy.ops.mesh.select_mode(type="VERT")
- bpy.ops.mesh.select_all(action='SELECT')
+ bpy.ops.mesh.select_all(action="SELECT")
bpy.ops.mesh.convex_hull()
decimate(obj)
@@ -62,13 +64,14 @@ def load_obj(file):
keep_vertex_order=False,
# ~ global_scale=1.0, path_mode='AUTO', axis_forward='X', axis_up='Z')
global_scale=1.0,
- path_mode='AUTO')
+ path_mode="AUTO",
+ )
# delete all objects
bpy.ops.object.delete()
-bpy.ops.object.select_all(action='SELECT')
+bpy.ops.object.select_all(action="SELECT")
bpy.ops.object.delete()
os.chdir(FOLDER_PATH)
diff --git a/script/relative_foot_positions/relativeFootPositionQuasiFlat.py b/script/relative_foot_positions/relativeFootPositionQuasiFlat.py
index 42d995c09ef78a47e3e4aa0557e411e07d8cbcd9..14dd5a30967d46ac88a15ef24a26db5ff254ac14 100644
--- a/script/relative_foot_positions/relativeFootPositionQuasiFlat.py
+++ b/script/relative_foot_positions/relativeFootPositionQuasiFlat.py
@@ -19,23 +19,24 @@ MIN_DIST_BETWEEN_FEET_X = 0.10
MAX_DIST_BETWEEN_FEET_X = 0.35
MAX_DIST_BETWEEN_FEET_Z = 0.35
MIN_HEIGHT_COM = 0.3
-# margin used to constrain the com y position : if it's on the left of the left foot or on the right of the right foot
+# margin used to constrain the com y position : if it's on the left of the left foot
+# or on the right of the right foot
# for more than this margin, we reject this sample:
MARGIN_FEET_SIDE = 0.05
fullBody = Robot()
fullBody.setConstrainedJointsBounds()
-fullBody.setJointBounds("LF_KFE", [-1.4, 0.])
-fullBody.setJointBounds("RF_KFE", [-1.4, 0.])
-fullBody.setJointBounds("LH_KFE", [0., 1.4])
-fullBody.setJointBounds("RH_KFE", [0., 1.4])
+fullBody.setJointBounds("LF_KFE", [-1.4, 0.0])
+fullBody.setJointBounds("RF_KFE", [-1.4, 0.0])
+fullBody.setJointBounds("LH_KFE", [0.0, 1.4])
+fullBody.setJointBounds("RH_KFE", [0.0, 1.4])
fullBody.setJointBounds("root_joint", [-20, 20, -20, 20, -20, 20])
dict_heuristic = {
fullBody.rLegId: "static",
fullBody.lLegId: "static",
fullBody.rArmId: "fixedStep04",
- fullBody.lArmId: "fixedStep04"
+ fullBody.lArmId: "fixedStep04",
}
fullBody.loadAllLimbs(dict_heuristic, "ReferenceConfiguration", nbSamples=12)
@@ -44,9 +45,9 @@ nbSamples = 1
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
v = vf.createViewer()
-rootName = 'root_joint'
+rootName = "root_joint"
-zero = [0., 0., 0.]
+zero = [0.0, 0.0, 0.0]
rLegId = fullBody.rLegId
rLeg = fullBody.rleg
rfoot = fullBody.rfoot
@@ -61,7 +62,7 @@ lfoot = fullBody.lfoot
# make sure this is 0
q_0 = fullBody.getCurrentConfig()
-zeroConf = [0, 0, 0, 0, 0, 0, 1.]
+zeroConf = [0, 0, 0, 0, 0, 0, 1.0]
q_0[0:7] = zeroConf
fullBody.setCurrentConfig(q_0)
@@ -93,24 +94,29 @@ def staticEq(positions, com):
E = zeros((3, sizeX))
for i, pos in enumerate(positions):
E[:2, i] = pos[:2]
- e = array([com[0], com[1], 1.])
+ e = array([com[0], com[1], 1.0])
E[2, :] = ones(sizeX)
- res = linprog(ones(sizeX),
- A_ub=None,
- b_ub=None,
- A_eq=E,
- b_eq=e,
- bounds=[(0., 1.) for _ in range(sizeX)],
- method='interior-point',
- callback=None,
- options={'presolve': True})
- return res['success']
-
-
-# returns true of one of the point is inside the convex hulls of the others. We do not want that
+ res = linprog(
+ ones(sizeX),
+ A_ub=None,
+ b_ub=None,
+ A_eq=E,
+ b_eq=e,
+ bounds=[(0.0, 1.0) for _ in range(sizeX)],
+ method="interior-point",
+ callback=None,
+ options={"presolve": True},
+ )
+ return res["success"]
+
+
def pointInsideHull(positions):
+ """
+ returns true of one of the point is inside the convex hulls of the others.
+ We do not want that
+ """
for i, pos in enumerate(positions):
- others = positions[:i] + positions[i + 1:]
+ others = positions[:i] + positions[i + 1 :]
if staticEq(others, pos):
return True
return False
@@ -129,7 +135,9 @@ def genFlat(init=False):
s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbIds)
succ = True
for effId, pos in zip(limbIds, positions):
- s, succ = state_alg.addNewContact(s, effId, pos, [0., 0., 1.], num_max_sample=0)
+ s, succ = state_alg.addNewContact(
+ s, effId, pos, [0.0, 0.0, 1.0], num_max_sample=0
+ )
if not succ:
break
@@ -137,11 +145,12 @@ def genFlat(init=False):
# poslf = fullBody.getJointPosition(lfoot)[:3]
# print ("limbsIds ", limbIds)
# s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbIds)
- # s, succ = state_alg.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample = 0)
+ # s, succ = state_alg.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample= 0)
# if succ:
- # s, succ = state_alg.addNewContact(s, lLegId, poslf, [0.,0.,1.], num_max_sample = 0)
+ # s, succ = state_alg.addNewContact(s, lLegId, poslf, [0.,0.,1.], num_max_sample= 0)
if succ:
- # ~ succ = fullBody.isConfigValid(q)[0] and norm (array(posrf[:2]) - array(poslf[:2]) ) >= 0.3
+ # ~ succ = fullBody.isConfigValid(q)[0]
+ # and norm (array(posrf[:2]) - array(poslf[:2]) ) >= 0.3
succ = fullBody.isConfigValid(q)[0]
# assert that in static equilibrium
@@ -171,21 +180,26 @@ def printFootPositionRelativeToOther(nbConfigs):
success += 1
addCom = True
for j, effectorName in enumerate(effectors):
- for otheridx, (oeffectorName, limbId) in enumerate(zip(effectors, limbIds)):
+ for otheridx, (oeffectorName, limbId) in enumerate(
+ zip(effectors, limbIds)
+ ):
if otheridx != j:
fullBody.setCurrentConfig(q)
pos_other = fullBody.getJointPosition(oeffectorName)
pos = fullBody.getJointPosition(effectorName)
p = array(pos_other[:3]) - array(pos[:3]).tolist()
# ~ qtr = q[:]
- # ~ qtr[:3] = [qtr[0] - pos_other[0], qtr[1] - pos_other[1], qtr[2] - pos_other[2]]
+ # ~ qtr[:3] = [qtr[0] - pos_other[0], qtr[1] - pos_other[1],
+ # ~ qtr[2] - pos_other[2]]
# ~ fullBody.setCurrentConfig(qtr)
# ~ qEffector = fullBody.getJointPosition(effectorName)
# check current joint pos is now zero
- # ~ q0 = Quaternion(qEffector[6], qEffector[3], qEffector[4], qEffector[5])
+ # ~ q0 = Quaternion(qEffector[6], qEffector[3], qEffector[4],
+ # qEffector[5])
# ~ rot = q0.matrix() # compute rotation matrix world -> local
- # ~ p = qEffector[0:3] # (0,0,0) coordinate expressed in effector fram
+ # (0,0,0) coordinate expressed in effector fram
+ # ~ p = qEffector[0:3]
# ~ rm = np.zeros((4, 4))
# ~ for k in range(0, 3):
# ~ for l in range(0, 3):
@@ -195,24 +209,34 @@ def printFootPositionRelativeToOther(nbConfigs):
# ~ rm[3, 3] = 1
# ~ invrm = np.linalg.inv(rm)
# ~ p = invrm.dot([0, 0, 0., 1])
- if (MAX_DIST_BETWEEN_FEET_Z > abs(p[2])):
- if (MIN_DIST_BETWEEN_FEET_Y <= abs(p[1])):
- if (MIN_DIST_BETWEEN_FEET_X <= abs(p[0])):
- # this is not what we want to do in theory but it works well in fact
+ if MAX_DIST_BETWEEN_FEET_Z > abs(p[2]):
+ if MIN_DIST_BETWEEN_FEET_Y <= abs(p[1]):
+ if MIN_DIST_BETWEEN_FEET_X <= abs(p[0]):
+ # this is not what we want to do in theory
+ # but it works well in fact
points[j][oeffectorName].append(p[:3])
else:
addCom = False
else:
addCom = False
else:
- print('rejecting ', effectorName, ' ', oeffectorName, p, abs(p[2]))
+ print(
+ "rejecting ",
+ effectorName,
+ " ",
+ oeffectorName,
+ p,
+ abs(p[2]),
+ )
# ~ print ('pos_other', pos_other)
# ~ print ('old_pos', old_pos)
addCom = False
v(q)
- # ~ if (j == 0 and p[1] > MIN_DIST_BETWEEN_FEET_Y and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
+ # ~ if (j == 0 and p[1] > MIN_DIST_BETWEEN_FEET_Y
+ # and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
# ~ points[j].append(p[:3])
- # ~ elif (j == 1 and p[1] < -MIN_DIST_BETWEEN_FEET_Y and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
+ # ~ elif (j == 1 and p[1] < -MIN_DIST_BETWEEN_FEET_Y
+ # and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
# ~ points[j].append(p[:3])
# ~ else:
# ~ addCom =
@@ -220,14 +244,15 @@ def printFootPositionRelativeToOther(nbConfigs):
fullBody.setCurrentConfig(q)
com = array(fullBody.getCenterOfMass())
- print('com ', com)
+ print("com ", com)
# ~ for x in range(0, 3):
# ~ q[x] = -com[x]
for j, effectorName in enumerate(effectors):
pos = fullBody.getJointPosition(effectorName)
rp = array(com) - array(pos[:3]).tolist()
# ~ qEffector = fullBody.getJointPosition(effectorName)
- # ~ q0 = Quaternion(qEffector[6], qEffector[3], qEffector[4], qEffector[5])
+ # ~ q0 = Quaternion(qEffector[6], qEffector[3], qEffector[4],
+ # qEffector[5])
# ~ rot = q0.matrix() # compute rotation matrix world -> local
# ~ p = qEffector[0:3] # (0,0,0) coordinate expressed in effector fram
# ~ rm = np.zeros((4, 4))
@@ -242,7 +267,7 @@ def printFootPositionRelativeToOther(nbConfigs):
# ~ # add offset
# ~ rp = array(p[:3] - offsets[j]).tolist()
- if (rp[2] < MIN_HEIGHT_COM):
+ if rp[2] < MIN_HEIGHT_COM:
addCom = False
print("reject min heught")
if addCom:
@@ -265,7 +290,9 @@ def printFootPositionRelativeToOther(nbConfigs):
# f1.close()
-s = rbprmstate.State(fullBody, q=fullBody.getCurrentConfig(), limbsIncontact=[fullBody.limbs_names[0]])
+s = rbprmstate.State(
+ fullBody, q=fullBody.getCurrentConfig(), limbsIncontact=[fullBody.limbs_names[0]]
+)
# printRootPosition(rLegId, rfoot, nbSamples)
# printRootPosition(lLegId, lfoot, nbSamples)
@@ -276,12 +303,20 @@ print("successes ", success)
print("fails ", fails)
# ~ for effector, comData, pointsData in zip(effectors, compoints, points):
-# ~ for effector, limbId, comData, pointsData in zip(effectors[:1],limbIds[1:], compoints[:1], points[:1]):
+# ~ for effector, limbId, comData, pointsData in zip(effectors[:1],limbIds[1:],
+# compoints[:1], points[:1]):
for effector, limbId, comData, pointsData in zip(effectors, limbIds, compoints, points):
hcom = ConvexHull(comData)
- hull_to_obj(hcom, comData, "anymal_COM_constraints_in_" + str(limbId) + "_effector_frame_quasi_static.obj")
+ hull_to_obj(
+ hcom,
+ comData,
+ "anymal_COM_constraints_in_" + str(limbId) + "_effector_frame_quasi_static.obj",
+ )
fig = plt.figure()
- fig.suptitle("anymal_COM_constraints_in_" + str(limbId) + "_effector_frame_quasi_static.obj", fontsize=16)
+ fig.suptitle(
+ "anymal_COM_constraints_in_" + str(limbId) + "_effector_frame_quasi_static.obj",
+ fontsize=16,
+ )
plot_hull(hcom, comData, array(comData), color="r", plot=False, fig=fig, ax=None)
fig = plt.figure()
@@ -292,7 +327,11 @@ for effector, limbId, comData, pointsData in zip(effectors, limbIds, compoints,
for (oEffector, pts) in pointsData.items():
# ~ ax = fig.add_subplot(axId, projection="3d")
hpts = ConvexHull(pts)
- hull_to_obj(hpts, pts, "anymal_" + str(oEffector) + "_constraints_in_" + str(limbId) + ".obj")
+ hull_to_obj(
+ hpts,
+ pts,
+ "anymal_" + str(oEffector) + "_constraints_in_" + str(limbId) + ".obj",
+ )
print("ax ", ax)
ax = plot_hull(hpts, pts, array(pts), color="b", plot=False, fig=fig, ax=ax)
print(
diff --git a/script/relative_foot_positions/run.sh b/script/relative_foot_positions/run.sh
index c75111a2db8f23af335baf238ba53de3296c343e..f1917c4d35568e1f6d064a1cd69f5c8b8f59e436 100644
--- a/script/relative_foot_positions/run.sh
+++ b/script/relative_foot_positions/run.sh
@@ -1,4 +1,4 @@
-#!/bin/bash
+#!/bin/bash
gepetto-gui &
hpp-rbprm-server &
diff --git a/setup.cfg b/setup.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..98698ff08783653bbcff9a976fefd01934bb6de6
--- /dev/null
+++ b/setup.cfg
@@ -0,0 +1,4 @@
+[flake8]
+exclude = cmake
+max-line-length = 88
+ignore = E226, E704, E24, E121, W504, E126, E123, W503, E203