From 1b94902b3669b10ae2b5fbf4278c083b83af4d34 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@gmail.com>
Date: Mon, 27 Jul 2020 15:51:41 +0200
Subject: [PATCH] Initial commit
---
.gitmodules | 3 +
CMakeLists.txt | 34 ++
cmake | 1 +
...eg_effector_frame_quasi_static_reduced.obj | 66 ++++
...eg_effector_frame_quasi_static_reduced.obj | 64 ++++
...eg_effector_frame_quasi_static_reduced.obj | 62 ++++
...eg_effector_frame_quasi_static_reduced.obj | 61 ++++
data/meshes/FLleg_rom_reduced.stl | Bin 0 -> 1184 bytes
data/meshes/FRleg_rom_reduced.stl | Bin 0 -> 1184 bytes
data/meshes/HLleg_rom_reduced.stl | Bin 0 -> 1184 bytes
data/meshes/HRleg_rom_reduced.stl | Bin 0 -> 1184 bytes
data/meshes/solo_all_visual.dae | 304 +++++++++++++++++
data/meshes/solo_trunk.stl | Bin 0 -> 684 bytes
data/package.xml | 10 +
.../FL_FOOT_constraints_in_FRleg_reduced.obj | 64 ++++
.../FL_FOOT_constraints_in_HLleg_reduced.obj | 60 ++++
.../FL_FOOT_constraints_in_HRleg_reduced.obj | 62 ++++
.../FR_FOOT_constraints_in_FLleg_reduced.obj | 66 ++++
.../FR_FOOT_constraints_in_HLleg_reduced.obj | 64 ++++
.../FR_FOOT_constraints_in_HRleg_reduced.obj | 64 ++++
.../HL_FOOT_constraints_in_FLleg_reduced.obj | 62 ++++
.../HL_FOOT_constraints_in_FRleg_reduced.obj | 64 ++++
.../HL_FOOT_constraints_in_HRleg_reduced.obj | 64 ++++
.../HR_FOOT_constraints_in_FLleg_reduced.obj | 62 ++++
.../HR_FOOT_constraints_in_FRleg_reduced.obj | 66 ++++
.../HR_FOOT_constraints_in_HLleg_reduced.obj | 64 ++++
data/roms/FLleg_rom_reduced.obj | 59 ++++
data/roms/FRleg_rom_reduced.obj | 61 ++++
data/roms/HLleg_rom_reduced.obj | 59 ++++
data/roms/HRleg_rom_reduced.obj | 61 ++++
data/srdf/solo_LFleg_rom.srdf | 3 +
data/srdf/solo_LHleg_rom.srdf | 3 +
data/srdf/solo_RFleg_rom.srdf | 3 +
data/srdf/solo_RHleg_rom.srdf | 3 +
data/srdf/solo_trunk.srdf | 3 +
data/urdf/solo_LFleg_rom.urdf | 19 ++
data/urdf/solo_LHleg_rom.urdf | 19 ++
data/urdf/solo_RFleg_rom.urdf | 19 ++
data/urdf/solo_RHleg_rom.urdf | 19 ++
data/urdf/solo_trunk.urdf | 23 ++
script/constants_and_tools.py | 200 +++++++++++
script/gen_and_copy.sh | 10 +
script/generateROMs.py | 54 +++
script/obj_to_constraints.py | 156 +++++++++
script/plot_polytopes.py | 66 ++++
script/reduce.py | 67 ++++
script/relativeFootPositionQuasiFlat.py | 313 ++++++++++++++++++
script/run.sh | 8 +
solo_rbprm/__init__.py | 0
solo_rbprm/solo.py | 212 ++++++++++++
solo_rbprm/solo_abstract.py | 61 ++++
51 files changed, 2868 insertions(+)
create mode 100644 .gitmodules
create mode 100644 CMakeLists.txt
create mode 160000 cmake
create mode 100644 data/com_inequalities/feet_quasi_flat/COM_constraints_in_FLleg_effector_frame_quasi_static_reduced.obj
create mode 100644 data/com_inequalities/feet_quasi_flat/COM_constraints_in_FRleg_effector_frame_quasi_static_reduced.obj
create mode 100644 data/com_inequalities/feet_quasi_flat/COM_constraints_in_HLleg_effector_frame_quasi_static_reduced.obj
create mode 100644 data/com_inequalities/feet_quasi_flat/COM_constraints_in_HRleg_effector_frame_quasi_static_reduced.obj
create mode 100644 data/meshes/FLleg_rom_reduced.stl
create mode 100644 data/meshes/FRleg_rom_reduced.stl
create mode 100644 data/meshes/HLleg_rom_reduced.stl
create mode 100644 data/meshes/HRleg_rom_reduced.stl
create mode 100644 data/meshes/solo_all_visual.dae
create mode 100644 data/meshes/solo_trunk.stl
create mode 100644 data/package.xml
create mode 100644 data/relative_effector_positions/FL_FOOT_constraints_in_FRleg_reduced.obj
create mode 100644 data/relative_effector_positions/FL_FOOT_constraints_in_HLleg_reduced.obj
create mode 100644 data/relative_effector_positions/FL_FOOT_constraints_in_HRleg_reduced.obj
create mode 100644 data/relative_effector_positions/FR_FOOT_constraints_in_FLleg_reduced.obj
create mode 100644 data/relative_effector_positions/FR_FOOT_constraints_in_HLleg_reduced.obj
create mode 100644 data/relative_effector_positions/FR_FOOT_constraints_in_HRleg_reduced.obj
create mode 100644 data/relative_effector_positions/HL_FOOT_constraints_in_FLleg_reduced.obj
create mode 100644 data/relative_effector_positions/HL_FOOT_constraints_in_FRleg_reduced.obj
create mode 100644 data/relative_effector_positions/HL_FOOT_constraints_in_HRleg_reduced.obj
create mode 100644 data/relative_effector_positions/HR_FOOT_constraints_in_FLleg_reduced.obj
create mode 100644 data/relative_effector_positions/HR_FOOT_constraints_in_FRleg_reduced.obj
create mode 100644 data/relative_effector_positions/HR_FOOT_constraints_in_HLleg_reduced.obj
create mode 100644 data/roms/FLleg_rom_reduced.obj
create mode 100644 data/roms/FRleg_rom_reduced.obj
create mode 100644 data/roms/HLleg_rom_reduced.obj
create mode 100644 data/roms/HRleg_rom_reduced.obj
create mode 100644 data/srdf/solo_LFleg_rom.srdf
create mode 100644 data/srdf/solo_LHleg_rom.srdf
create mode 100644 data/srdf/solo_RFleg_rom.srdf
create mode 100644 data/srdf/solo_RHleg_rom.srdf
create mode 100644 data/srdf/solo_trunk.srdf
create mode 100644 data/urdf/solo_LFleg_rom.urdf
create mode 100644 data/urdf/solo_LHleg_rom.urdf
create mode 100644 data/urdf/solo_RFleg_rom.urdf
create mode 100644 data/urdf/solo_RHleg_rom.urdf
create mode 100644 data/urdf/solo_trunk.urdf
create mode 100644 script/constants_and_tools.py
create mode 100755 script/gen_and_copy.sh
create mode 100644 script/generateROMs.py
create mode 100644 script/obj_to_constraints.py
create mode 100644 script/plot_polytopes.py
create mode 100644 script/reduce.py
create mode 100644 script/relativeFootPositionQuasiFlat.py
create mode 100755 script/run.sh
create mode 100644 solo_rbprm/__init__.py
create mode 100644 solo_rbprm/solo.py
create mode 100644 solo_rbprm/solo_abstract.py
diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000..60c9164
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,3 @@
+[submodule "cmake"]
+ path = cmake
+ url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..d9daf08
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,34 @@
+CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
+
+SET(PROJECT_NAME solo-rbprm)
+SET(PROJECT_DESCRIPTION "Database for solo robot using hpp-rbprm")
+SET(PROJECT_URL "")
+SET(CUSTOM_HEADER_DIR "hpp/solo-rbprm")
+SET(CXX_DISABLE_WERROR true)
+SET(PROJECT_USE_CMAKE_EXPORT TRUE)
+
+INCLUDE(cmake/hpp.cmake)
+INCLUDE(cmake/python.cmake)
+
+COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
+PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
+
+FINDPYTHON()
+
+ADD_LIBRARY(${PROJECT_NAME} INTERFACE)
+INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
+
+install(DIRECTORY data/ DESTINATION share/${PROJECT_NAME} )
+
+STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
+
+SET(${PROJECT_NAME}_PYTHON_FILES
+ __init__.py
+ solo.py
+ solo_abstract.py
+ )
+
+FOREACH(SOURCE ${${PROJECT_NAME}_PYTHON_FILES})
+ PYTHON_INSTALL_ON_SITE(${PY_NAME} ${SOURCE})
+ENDFOREACH(SOURCE ${${PROJECT_NAME}_PYTHON_SOURCES})
+
diff --git a/cmake b/cmake
new file mode 160000
index 0000000..fb4c22c
--- /dev/null
+++ b/cmake
@@ -0,0 +1 @@
+Subproject commit fb4c22c319ec5320f9a85527eb1a4130954846f5
diff --git a/data/com_inequalities/feet_quasi_flat/COM_constraints_in_FLleg_effector_frame_quasi_static_reduced.obj b/data/com_inequalities/feet_quasi_flat/COM_constraints_in_FLleg_effector_frame_quasi_static_reduced.obj
new file mode 100644
index 0000000..8c21009
--- /dev/null
+++ b/data/com_inequalities/feet_quasi_flat/COM_constraints_in_FLleg_effector_frame_quasi_static_reduced.obj
@@ -0,0 +1,66 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o COM_constraints_in_FLleg_effector_frame_quasi_static_reduced_COM_constraints_in_FLleg_effector_frame_quasi_static
+v -0.316477 -0.217896 0.130774
+v -0.062497 -0.268232 0.207756
+v -0.229063 -0.067848 0.131301
+v 0.083333 -0.092516 0.137775
+v -0.321241 -0.064686 0.152163
+v -0.295703 -0.055264 0.231148
+v -0.275462 -0.269362 0.189248
+v -0.204602 -0.283913 0.232556
+v 0.032256 -0.232794 0.134727
+v -0.176763 0.008647 0.283672
+v -0.214952 -0.124655 0.292953
+v -0.108472 -0.159242 0.281608
+v 0.002746 -0.163319 0.227763
+v -0.010894 -0.022412 0.231722
+vn -0.4830 0.1977 0.8530
+vn 0.1173 0.0355 0.9925
+vn 0.4362 0.0169 0.8997
+vn -0.4914 0.8692 0.0552
+vn -0.0663 0.9007 -0.4294
+vn 0.0241 0.8901 -0.4551
+vn 0.3168 0.1173 0.9412
+vn 0.6371 -0.5069 0.5806
+vn 0.3876 -0.3995 0.8308
+vn -0.8533 -0.0980 0.5122
+vn -0.9352 -0.1511 0.3204
+vn 0.2021 -0.4895 0.8482
+vn -0.6983 -0.2005 0.6872
+vn 0.0218 0.0138 -0.9997
+vn 0.7238 0.0507 0.6881
+vn -0.0251 -0.7591 -0.6505
+vn 0.0772 0.8346 -0.5454
+vn -0.0159 -0.3555 0.9346
+vn -0.2147 0.1285 -0.9682
+vn 0.8163 -0.3078 0.4888
+vn 0.0303 -0.9141 -0.4043
+vn 0.0425 -0.9240 -0.3800
+vn -0.5407 -0.3288 0.7743
+vn 0.0112 -0.0030 -0.9999
+s off
+f 6//1 11//1 10//1
+f 11//2 12//2 10//2
+f 12//3 13//3 14//3
+f 6//4 10//4 5//4
+f 3//5 5//5 10//5
+f 3//6 10//6 14//6
+f 14//7 10//7 12//7
+f 13//8 2//8 9//8
+f 13//9 12//9 2//9
+f 1//10 7//10 5//10
+f 7//11 6//11 5//11
+f 12//12 8//12 2//12
+f 6//13 7//13 11//13
+f 4//14 9//14 3//14
+f 4//15 14//15 13//15
+f 9//16 7//16 1//16
+f 3//17 14//17 4//17
+f 11//18 8//18 12//18
+f 3//19 1//19 5//19
+f 13//20 9//20 4//20
+f 2//21 8//21 9//21
+f 8//22 7//22 9//22
+f 7//23 8//23 11//23
+f 3//24 9//24 1//24
diff --git a/data/com_inequalities/feet_quasi_flat/COM_constraints_in_FRleg_effector_frame_quasi_static_reduced.obj b/data/com_inequalities/feet_quasi_flat/COM_constraints_in_FRleg_effector_frame_quasi_static_reduced.obj
new file mode 100644
index 0000000..762df24
--- /dev/null
+++ b/data/com_inequalities/feet_quasi_flat/COM_constraints_in_FRleg_effector_frame_quasi_static_reduced.obj
@@ -0,0 +1,64 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o COM_constraints_in_FRleg_effector_frame_quasi_static_reduced_COM_constraints_in_FRleg_effector_frame_quasi_static
+v -0.307050 0.043850 0.200764
+v -0.311342 0.070704 0.131705
+v -0.145312 0.181695 0.280595
+v -0.219941 0.282608 0.222765
+v 0.051323 0.224854 0.130650
+v -0.295175 0.234256 0.131465
+v -0.308027 0.239673 0.162063
+v -0.220113 -0.001115 0.275464
+v -0.234932 0.127228 0.289454
+v -0.079066 0.285852 0.209297
+v 0.009082 0.167355 0.225026
+v 0.071028 0.080604 0.140326
+v -0.035532 0.016083 0.252023
+v -0.086167 0.106799 0.281511
+vn 0.6059 0.5295 0.5938
+vn -0.1553 0.4022 0.9023
+vn -0.2622 0.9253 -0.2740
+vn 0.1438 -0.2324 0.9619
+vn -0.8184 0.1075 0.5645
+vn 0.8650 0.1503 0.4787
+vn 0.7490 -0.1041 0.6544
+vn 0.0615 0.0608 0.9963
+vn -0.9978 0.0079 0.0651
+vn 0.3479 0.2857 0.8929
+vn 0.0388 -0.1039 0.9938
+vn -0.9125 0.0896 -0.3991
+vn -0.0024 -0.0012 -1.0000
+vn -0.0616 0.9021 -0.4270
+vn 0.0683 0.5336 0.8430
+vn -0.6890 -0.1567 0.7076
+vn 0.0205 0.8062 -0.5913
+vn 0.5083 0.0038 0.8612
+vn 0.3448 0.3709 0.8623
+vn 0.0241 -0.0636 -0.9977
+vn -0.6415 0.3540 0.6805
+vn -0.0081 -0.8966 -0.4428
+vn 0.0335 -0.8789 -0.4758
+s off
+f 5//1 10//1 11//1
+f 4//2 9//2 3//2
+f 6//3 7//3 4//3
+f 8//4 13//4 14//4
+f 1//5 9//5 7//5
+f 11//6 12//6 5//6
+f 13//7 12//7 11//7
+f 3//8 9//8 14//8
+f 1//9 7//9 2//9
+f 14//10 11//10 3//10
+f 9//11 8//11 14//11
+f 2//12 7//12 6//12
+f 2//13 6//13 5//13
+f 6//14 4//14 10//14
+f 3//15 10//15 4//15
+f 1//16 8//16 9//16
+f 5//17 6//17 10//17
+f 13//18 11//18 14//18
+f 11//19 10//19 3//19
+f 2//20 5//20 12//20
+f 7//21 9//21 4//21
+f 8//22 1//22 2//22 13//22
+f 2//23 12//23 13//23
diff --git a/data/com_inequalities/feet_quasi_flat/COM_constraints_in_HLleg_effector_frame_quasi_static_reduced.obj b/data/com_inequalities/feet_quasi_flat/COM_constraints_in_HLleg_effector_frame_quasi_static_reduced.obj
new file mode 100644
index 0000000..db6c52c
--- /dev/null
+++ b/data/com_inequalities/feet_quasi_flat/COM_constraints_in_HLleg_effector_frame_quasi_static_reduced.obj
@@ -0,0 +1,62 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o COM_constraints_in_HLleg_effector_frame_quasi_static_reduced_COM_constraints_in_HLleg_effector_frame_quasi_static
+v 0.082971 -0.002561 0.263059
+v -0.086952 -0.100086 0.134113
+v 0.302815 -0.254698 0.162356
+v -0.043057 -0.234236 0.137455
+v 0.200378 -0.141384 0.292106
+v 0.296580 -0.232550 0.133916
+v 0.296739 -0.078700 0.229223
+v 0.234698 -0.003512 0.267243
+v 0.178797 -0.281174 0.229137
+v 0.257410 -0.221207 0.235611
+v -0.034302 -0.094035 0.204093
+v 0.317969 -0.068172 0.133387
+v 0.050192 -0.262132 0.209690
+v 0.098528 -0.113704 0.289918
+vn -0.0025 -0.7892 -0.6141
+vn -0.6387 -0.2974 0.7097
+vn 0.5998 -0.1302 0.7895
+vn -0.0051 -0.0134 -0.9999
+vn 0.8048 0.5437 0.2380
+vn -0.5513 -0.2506 0.7958
+vn -0.1929 -0.3781 0.9055
+vn -0.0254 0.2315 0.9725
+vn 0.9447 -0.1239 -0.3036
+vn 0.4591 -0.6653 0.5887
+vn 0.7922 -0.1927 0.5791
+vn 0.5382 0.0180 0.8426
+vn -0.7666 -0.2360 0.5972
+vn -0.5245 0.1299 0.8415
+vn -0.0669 0.8362 -0.5443
+vn -0.0424 -0.9272 -0.3721
+vn 0.0174 0.9045 -0.4261
+vn 0.2564 -0.4296 0.8658
+vn 0.0254 0.1713 0.9849
+vn -0.1376 -0.4359 0.8894
+vn -0.6782 0.5725 0.4608
+vn 0.9763 -0.0465 0.2112
+s off
+f 4//1 6//1 3//1
+f 11//2 4//2 13//2
+f 10//3 7//3 5//3
+f 12//4 6//4 4//4 2//4
+f 7//5 12//5 8//5
+f 11//6 13//6 14//6
+f 13//7 9//7 5//7
+f 14//8 8//8 1//8
+f 6//9 12//9 3//9
+f 10//10 9//10 3//10
+f 10//11 3//11 7//11
+f 5//12 7//12 8//12
+f 11//13 2//13 4//13
+f 11//14 14//14 1//14
+f 12//15 2//15 1//15
+f 13//16 4//16 3//16 9//16
+f 8//17 12//17 1//17
+f 9//18 10//18 5//18
+f 5//19 8//19 14//19
+f 13//20 5//20 14//20
+f 2//21 11//21 1//21
+f 7//22 3//22 12//22
diff --git a/data/com_inequalities/feet_quasi_flat/COM_constraints_in_HRleg_effector_frame_quasi_static_reduced.obj b/data/com_inequalities/feet_quasi_flat/COM_constraints_in_HRleg_effector_frame_quasi_static_reduced.obj
new file mode 100644
index 0000000..955aadf
--- /dev/null
+++ b/data/com_inequalities/feet_quasi_flat/COM_constraints_in_HRleg_effector_frame_quasi_static_reduced.obj
@@ -0,0 +1,61 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o COM_constraints_in_HRleg_effector_frame_quasi_static_reduced_COM_constraints_in_HRleg_effector_frame_quasi_static
+v -0.028929 0.157222 0.202722
+v -0.045482 0.050207 0.190147
+v 0.315726 0.077646 0.130926
+v -0.077903 0.149396 0.132703
+v 0.209239 0.287616 0.221135
+v 0.201754 0.153537 0.293611
+v 0.077435 0.253044 0.227674
+v 0.074921 0.108906 0.281470
+v 0.156104 -0.003385 0.283788
+v 0.305996 0.249032 0.149423
+v 0.272282 0.158322 0.252016
+v -0.032803 0.235215 0.132513
+v 0.295415 0.030582 0.228830
+vn 0.0451 0.1097 -0.9929
+vn 0.0534 -0.9043 -0.4236
+vn -0.7697 0.4056 0.4930
+vn -0.6043 -0.0002 0.7968
+vn 0.5102 -0.0633 0.8577
+vn -0.4679 -0.3213 0.8233
+vn -0.0045 0.0001 -1.0000
+vn -0.2090 0.3451 0.9150
+vn -0.8211 0.0603 0.5676
+vn -0.0114 0.8741 -0.4857
+vn -0.6063 0.5152 0.6058
+vn -0.0818 0.4775 0.8748
+vn 0.4434 0.4069 0.7986
+vn -0.4762 0.3148 0.8211
+vn -0.0818 -0.0386 0.9959
+vn -0.2474 0.9631 0.1061
+vn -0.0991 -0.5228 -0.8467
+vn 0.6398 0.4592 0.6163
+vn 0.9755 0.0319 0.2177
+vn 0.9173 0.0960 0.3863
+vn -0.0116 -0.8784 -0.4778
+vn 0.3975 -0.1721 0.9013
+s off
+f 12//1 10//1 3//1
+f 3//2 13//2 9//2
+f 1//3 12//3 4//3
+f 2//4 8//4 1//4
+f 11//5 6//5 13//5
+f 2//6 9//6 8//6
+f 12//7 3//7 4//7
+f 7//8 8//8 6//8
+f 1//9 4//9 2//9
+f 10//10 12//10 5//10
+f 1//11 7//11 12//11
+f 6//12 5//12 7//12
+f 11//13 5//13 6//13
+f 1//14 8//14 7//14
+f 8//15 9//15 6//15
+f 7//16 5//16 12//16
+f 2//17 4//17 3//17
+f 10//18 5//18 11//18
+f 13//19 3//19 10//19
+f 11//20 13//20 10//20
+f 2//21 3//21 9//21
+f 13//22 6//22 9//22
diff --git a/data/meshes/FLleg_rom_reduced.stl b/data/meshes/FLleg_rom_reduced.stl
new file mode 100644
index 0000000000000000000000000000000000000000..7ee552c2e81e30b36e3f63e579f7ec7bf4b1c05c
GIT binary patch
literal 1184
zcmb7@T}YEr7{?F0SYC-?2-U0=VHudyCL7K7c_lM*S&&31+0yBzqp59fu#r(liYfWg
zheJn&mKPO@7ZsxKdk)zLO-4vdCBlp$t(jzB5~NbQe9mR>?K%JF`Tzd^^X7TZ);V34
zO10HlSEJr<x71czoN1YRbUO8}dbe4vNhSaFCB<%!Pb4vtJ}0-W7P5t$6SB?lNw}F%
zN}uXI!sVHG_~7WJU!FG!)_2!OH`%XILUMEONFIANOQ@KJh+{-nee(*Lirq92u|QEl
zvJ_nO)28kLxS{=<O&%1-6c-bh4eRsGY=?b`Q{cH+O^7YJSG;;YAipqaS+%<rD$b>W
zGQW$P%PW@hP76C5<Ji!aVsT1%BPUvo(B<!wu|wWf`=J{7w>691QfftfV%T^OL`M#c
zbd<_?5+Reb3aZR{3D3rD4Ce#xLU>r>gZSo4)YPblD?PJ<@%8-}dCE2u3x<!wW=(|R
zbg?Ub$DFhbCm#<=wM}IlIi}@z6wO}{XOD)n!_$cp=9riJVRXg|aSAQPOoS9C`>9G&
zh;LVpa2!*f=vb(~83tR76f+T`>1u$_!^7}BRK@TK?uK`zTK7XJ&{v7gxgo&Yu`4`@
zkTmO7u|f9-I-M$x9N)rfLORSJ$IdL5iRmT30H=r*n2C@zlbS^*TVZvBf#DO}Chm!l
z_d*K$+Ifmj^zCE#z2XLW#af#}@}`!HlifdobAnzaPK1!dw+StO)j|*WTnzV&_wngw
z9BidMLyNNik&APMIe5hr-7)6N=mnvDh2pGm3Ot8TZx-(cf<pcF1@6DYo%4PC19o*1
AZ~y=R
literal 0
HcmV?d00001
diff --git a/data/meshes/FRleg_rom_reduced.stl b/data/meshes/FRleg_rom_reduced.stl
new file mode 100644
index 0000000000000000000000000000000000000000..cdc71906b7db47c918e2d6047d44708f6240158d
GIT binary patch
literal 1184
zcmb7@T}YE*6vt0cM4BI91VfD%Ei)rqCJtin^N^?)t|C!1iz&@_i`d#?%Stn?v<sD%
zn2Cr<+RD#Ox)5aTJx{0<eiTxnmJvyTg!ExoAtCMZyq7)K&+nYy|2gN$y?W8*zGPJ-
zo7;6k%63?tiq(A}Bi&$-QazOxNxu*N>+B5QZsU=na<@@+)fS6TTTosupHy$W>&90r
z3F?Od9sX!nFvD{*)MB3q_4HOFfX~BuPs^IBu)U%waE6(Zmee<y{a9G_h>fJgsLQ`B
z7}U42;TKI<_pvNYJj_bPjl*;7Tda+n3wqfTMTaTFQR<cR6RIP-Q;n74u<rJ(x}H~r
z_Ab8)FuqtOE=Omg(Y&fLp|>avuyN`;UhBAp(??1Lb*Gt{8}G>G3@_$es=`)Ek{^}e
zQL$!-;nX+3x}R;Ev}3Y05~(eKyU!#W(~5<DD3VhXN^1$P*(ACK66CuF;|2AlFG_6z
z24@;WqYDnPb<a4Xb*cLpFJbq*t(Yz~gmc6cO+C|Pnfcjy;IuBChfXSBxTD)x|G6hj
zoNKo6uYV)Z=6S1`6eZ{@0L&GC39jZUyyE!^qd6!+djmub)fh|SPxHj}Eu0eceJBku
z-SiYsyl!XuKNXryq;<79lILsr$(&Bv7jVkdm)52I07fICajGLA*Wa(v>@M9BZI8m%
z4R-8(A1*vTF6cby4CpTa`0Z~C%}(0IuDq9wPL0xZdjQH)(s-<S6Z16~HCEJwW&+sW
y`BN>O8N<OPhoC03pVrF5_T#4qK3wc@3VMqs*AimnkjS|XA}(>W=0{Olo8up)Z7b6N
literal 0
HcmV?d00001
diff --git a/data/meshes/HLleg_rom_reduced.stl b/data/meshes/HLleg_rom_reduced.stl
new file mode 100644
index 0000000000000000000000000000000000000000..42dbf13f2fa0516cadbc6289e509522c56237e6e
GIT binary patch
literal 1184
zcmb7DTS${(7=EqrqJvaa7P2`H9Sn3os~j@F@5d-RT{sbDk+#kDr-HgU6U<#mc}hHF
zxmwdmLx*<KB5@V|@7GlbQYcY_iYN@qyI_?OCGGP4FZ*ua!*hDSgAE?H_pIVFobkG=
z4g0H<8kgeDG-a91hFx{_Wd<n&{_Aatb$3TL@PL1mhg02RIU%IJF1^f4vm3EEP=t^2
zo-<qX7#59NQTo}#-}bg6Ky`a?dL-T@=GwmUs*?w>T=7ji3x;uK^>S`}#k*V)tU!x7
zgnEyTWu`k~69e`6YFKKLb5@>bbl0m7z4*=4$rBYDQfz>!r4^RRup+ZxpLpxeNp>gS
z&P@{wD(zaEd>gk;TDiAtL<NYpL~|2&J7rH=l+y~Gpm+d2OM+NBr1Gv&E`rWJtdTbH
z6C)i+yC_?LCyN)@;^1{$KUXKGun^wN8)jwEICM<RvABY#?0Z`hQnmoPJs~Eodx(}o
zw@fRPA>9P<V4{{Eep#;Ck2eVFF3n>{p;28E_`wEe{V{iK=Ztdlm;2oNF<sCKRZbZK
zoQlr#!SN0ZCfNl|jpETw0JB+5;zie0^>ofEMzf{dsU`q>r%5&%FS3%m*%}PGSIcUn
zWgqWPxWQ|$XfKgYP!|C1=`-Xh<r+WsUJ`T{RZd+8xKvUnl6?|4rnG2srkb=mBIeKd
zXJ~*w>ns-Z8YmNbJpe7%?V?q$!&{BApk35m?L@?yCi)+5#_{evP3Dx9c47uAnPb}#
xaa7f5E|I259RcY1?GbPGN<4MUulb$y+O-&e7e=Qunk$91Z=TbfDJzNx@DJRj27mwn
literal 0
HcmV?d00001
diff --git a/data/meshes/HRleg_rom_reduced.stl b/data/meshes/HRleg_rom_reduced.stl
new file mode 100644
index 0000000000000000000000000000000000000000..de2afa816e75c39db531afca1f1dcbf22d8c5f5c
GIT binary patch
literal 1184
zcmb7@Z%9*77{*@;i-n;`g@wT-5M8laMp^2;*FTLuY35Qx%`VNA%++KzE5$#ciOSFd
zHPI;!W2~T}Xc^A?o;fWtg~&80qfm%M5+tq6`j8*r^Re^!Jn!>+p7$KXwNjh?in&O*
zV7FZoQmy98MP|D$Zhw5d5L@OrFX*-KUuVbq?YVO!Loxm3@-A<qBvf?av#-~O+TPb<
zOy+U*=*-8#y37`=+I$jgd%6=N)7%K~=6MzS9IC1oZ9wd9>BH|^>hW=J0(#4Ouu^{v
z_d0cIuI$0(v3u&Ku#M}A-_}(z*)lF||5PlSDuQBz!-yg5zFM{CwRre;x9W<F;M4#+
z>kcO74=8eb>JN#MP(rE(@TJ%#OHP#3ddI;Y=4sJU9)TLKmt&`s0CcmKq>&7*?EWLm
zbQWc&nE(o!gDBi>@}HWpavqa&P7Hq_;7a2y&JQ5R_ayOZio#rrKP5x>39RvSpvP6i
zrKKGLEDZY+{^}JuOZQ8nB$SZ001)&H_~+*pmfN(#dC;c$9I<`PqG8g?@{NpFEa}z4
zcTP-?C_$<QXgz&IPVUW?O4{}@P3c-vb=FpV<+by#glrQ)*<H*s3Od!7H>Z&HL(}p(
z8u~v-gA;Fu7EBj854uf$gQH_-WMfG+j=ospcqt+64Ir+ylI1PC{n^tZ(sn5cB?MU1
zPD{~&W(?>GnThm6si+@7n$3m7kH#^&{4Ar%se--*fG^uZSy$9ku_G*-Q&1}Y53aUr
y*lhh8R!kjWbQWc&-T-2VEEo7@@LT!@E(c}jcWzM*NjExIu_ra3Q&2ye3E&^*6)zzG
literal 0
HcmV?d00001
diff --git a/data/meshes/solo_all_visual.dae b/data/meshes/solo_all_visual.dae
new file mode 100644
index 0000000..874fd9b
--- /dev/null
+++ b/data/meshes/solo_all_visual.dae
@@ -0,0 +1,304 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+ <asset>
+ <contributor>
+ <author>Blender User</author>
+ <authoring_tool>Blender 2.77.0 commit date:2016-04-05, commit time:18:12, hash:abf6f08</authoring_tool>
+ </contributor>
+ <created>2020-07-27T15:10:28</created>
+ <modified>2020-07-27T15:10:28</modified>
+ <unit name="meter" meter="1"/>
+ <up_axis>Z_UP</up_axis>
+ </asset>
+ <library_images/>
+ <library_effects>
+ <effect id="rom-effect">
+ <profile_COMMON>
+ <technique sid="common">
+ <phong>
+ <emission>
+ <color sid="emission">0 0 0 1</color>
+ </emission>
+ <ambient>
+ <color sid="ambient">0 0 0 1</color>
+ </ambient>
+ <diffuse>
+ <color sid="diffuse">0 0.56 0 1</color>
+ </diffuse>
+ <specular>
+ <color sid="specular">0.5 0.5 0.5 1</color>
+ </specular>
+ <shininess>
+ <float sid="shininess">50</float>
+ </shininess>
+ <transparency>
+ <float sid="transparency">0.3</float>
+ </transparency>
+ <index_of_refraction>
+ <float sid="index_of_refraction">1</float>
+ </index_of_refraction>
+ </phong>
+ </technique>
+ </profile_COMMON>
+ </effect>
+ <effect id="trunk-effect">
+ <profile_COMMON>
+ <technique sid="common">
+ <phong>
+ <emission>
+ <color sid="emission">0 0 0 1</color>
+ </emission>
+ <ambient>
+ <color sid="ambient">0 0 0 1</color>
+ </ambient>
+ <diffuse>
+ <color sid="diffuse">0.64 0 0 1</color>
+ </diffuse>
+ <specular>
+ <color sid="specular">0.5 0.5 0.5 1</color>
+ </specular>
+ <shininess>
+ <float sid="shininess">50</float>
+ </shininess>
+ <index_of_refraction>
+ <float sid="index_of_refraction">1</float>
+ </index_of_refraction>
+ </phong>
+ </technique>
+ </profile_COMMON>
+ </effect>
+ </library_effects>
+ <library_materials>
+ <material id="rom-material" name="rom">
+ <instance_effect url="#rom-effect"/>
+ </material>
+ <material id="trunk-material" name="trunk">
+ <instance_effect url="#trunk-effect"/>
+ </material>
+ </library_materials>
+ <library_geometries>
+ <geometry id="HRleg_rom_reduced-mesh" name="HRleg rom reduced">
+ <mesh>
+ <source id="HRleg_rom_reduced-mesh-positions">
+ <float_array id="HRleg_rom_reduced-mesh-positions-array" count="39">-0.301713 -0.01525294 -0.25166 -0.04792797 -0.001684963 -0.277958 -0.202177 0.004749 -0.314965 -0.313588 -0.257896 -0.182246 -0.307523 -0.180849 -0.04236996 -0.324702 -0.08339297 -0.127987 0.09495997 -0.206503 -0.08483695 0.10114 -0.07597196 -0.142672 -0.165385 -0.14303 -0.329407 -0.275168 -0.1569139 -0.284248 -0.194398 -0.294239 -0.249621 -0.04319798 -0.275073 -0.229562 0.03057098 -0.13801 -0.236972</float_array>
+ <technique_common>
+ <accessor source="#HRleg_rom_reduced-mesh-positions-array" count="13" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="HRleg_rom_reduced-mesh-normals">
+ <float_array id="HRleg_rom_reduced-mesh-normals-array" count="66">-0.02384942 0.963427 0.2669075 -0.9912997 -0.09230971 0.09382897 0.1213016 0.397295 0.9096387 0.002708613 0.8777979 0.4790236 0.2365788 0.1526734 -0.9595423 0.01433587 0.6613581 0.7499333 -0.3799619 -0.004201054 -0.9249926 0.00551474 -0.8759989 0.4822816 0.001386642 0.8757475 0.4827677 -0.005604565 -0.878554 0.4776104 0.06768828 -0.9250836 0.3736829 0.7122278 0.2086115 -0.6702335 -0.9066984 -0.07439482 -0.4151672 0.4270288 -0.0258544 -0.9038683 0.8468288 -0.2484563 -0.4702665 -0.9826537 -0.004942297 -0.1853841 0.4163355 -0.2709972 -0.8678856 -0.3055364 -0.3978406 -0.8650841 -0.5471938 0.08892172 -0.8322691 -0.5374447 -0.4892951 -0.686836 0.7461737 -0.429118 -0.5090015 0.1751725 -0.4855386 -0.8564852</float_array>
+ <technique_common>
+ <accessor source="#HRleg_rom_reduced-mesh-normals-array" count="22" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="HRleg_rom_reduced-mesh-vertices">
+ <input semantic="POSITION" source="#HRleg_rom_reduced-mesh-positions"/>
+ </vertices>
+ <polylist material="rom-material" count="22">
+ <input semantic="VERTEX" source="#HRleg_rom_reduced-mesh-vertices" offset="0"/>
+ <input semantic="NORMAL" source="#HRleg_rom_reduced-mesh-normals" offset="1"/>
+ <vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+ <p>0 0 1 0 2 0 3 1 4 1 5 1 6 2 7 2 4 2 1 3 0 3 7 3 8 4 2 4 1 4 7 5 5 5 4 5 8 6 9 6 2 6 10 7 4 7 3 7 5 8 7 8 0 8 10 9 6 9 4 9 10 10 11 10 6 10 12 11 1 11 7 11 9 12 3 12 0 12 12 13 8 13 1 13 6 14 12 14 7 14 5 15 0 15 3 15 11 16 8 16 12 16 9 17 8 17 10 17 9 18 0 18 2 18 10 19 3 19 9 19 11 20 12 20 6 20 11 21 10 21 8 21</p>
+ </polylist>
+ </mesh>
+ </geometry>
+ <geometry id="HLleg_rom_reduced-mesh" name="HLleg rom reduced">
+ <mesh>
+ <source id="HLleg_rom_reduced-mesh-positions">
+ <float_array id="HLleg_rom_reduced-mesh-positions-array" count="39">-0.08059495 0.283919 -0.244348 -0.206632 0.09680497 -0.329127 -0.20379 0.279222 -0.264777 -0.1917549 -0.01534396 -0.318965 0.03101098 0.02678298 -0.227149 -0.322687 0.03843694 -0.218888 -0.28295 0.271873 -0.20937 0.08796596 0.218043 -0.111174 -0.330315 0.203812 -0.08034598 -0.287854 0.144716 -0.268832 -0.322732 0.07971495 -0.126905 0.101897 0.09097099 -0.1518819 -0.04280197 0.119917 -0.290616</float_array>
+ <technique_common>
+ <accessor source="#HLleg_rom_reduced-mesh-positions-array" count="13" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="HLleg_rom_reduced-mesh-normals">
+ <float_array id="HLleg_rom_reduced-mesh-normals-array" count="66">0.1424238 0.3273055 -0.9341235 -0.0187413 -0.8907263 0.4541535 -0.04845196 0.9968371 0.06299442 0.001998364 0.8973343 0.441347 0.02972644 0.8795486 0.4748798 -0.5555951 0.3696897 -0.7447441 0.06392234 -0.298095 0.9523935 0.08066385 -0.3458037 0.9348332 0.2360217 -0.05659317 -0.9700984 0.6854452 0.3396539 -0.6440498 0.4100599 -0.2689104 -0.8715149 -0.9968078 -0.07302242 0.03228127 0.6974392 0.06649625 -0.7135523 0.1769831 0.3047989 -0.9358283 0.6606243 0.2937448 -0.6908615 -0.9506311 0.1723446 -0.2580657 -0.4457385 0.3039626 -0.8419762 -0.6457598 -0.1534367 -0.7479649 -0.6379523 -0.146212 -0.7560681 0.05706667 -0.7855435 0.6161696 -0.9413233 0.1720765 -0.2903453 -0.02049446 -0.9121576 0.409327</float_array>
+ <technique_common>
+ <accessor source="#HLleg_rom_reduced-mesh-normals-array" count="22" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="HLleg_rom_reduced-mesh-vertices">
+ <input semantic="POSITION" source="#HLleg_rom_reduced-mesh-positions"/>
+ </vertices>
+ <polylist material="rom-material" count="22">
+ <input semantic="VERTEX" source="#HLleg_rom_reduced-mesh-vertices" offset="0"/>
+ <input semantic="NORMAL" source="#HLleg_rom_reduced-mesh-normals" offset="1"/>
+ <vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+ <p>0 0 1 0 2 0 3 1 4 1 5 1 2 2 6 2 0 2 7 3 0 3 8 3 8 4 0 4 6 4 9 5 6 5 2 5 10 6 11 6 7 6 7 7 8 7 10 7 3 8 1 8 12 8 12 9 0 9 11 9 4 10 3 10 12 10 5 11 10 11 8 11 4 12 12 12 11 12 0 13 12 13 1 13 11 14 0 14 7 14 6 15 5 15 8 15 2 16 1 16 9 16 1 17 3 17 9 17 3 18 5 18 9 18 10 19 4 19 11 19 5 20 6 20 9 20 10 21 5 21 4 21</p>
+ </polylist>
+ </mesh>
+ </geometry>
+ <geometry id="FRleg_rom_reduced-mesh" name="FRleg rom reduced">
+ <mesh>
+ <source id="FRleg_rom_reduced-mesh-positions">
+ <float_array id="FRleg_rom_reduced-mesh-positions-array" count="39">0.09110397 -0.09588599 -0.310215 -0.03830695 -0.150062 -0.223232 -0.01580196 -0.02385497 -0.244761 0.214533 -0.106999 -0.328528 0.319285 -0.034949 -0.221622 0.285528 -0.271047 -0.217638 0.317027 -0.149798 -0.03458797 -0.095416 -0.07532197 -0.1436769 -0.099864 -0.207302 -0.08477097 0.06910997 -0.277728 -0.243539 0.329058 -0.23024 -0.136128 0.210204 -0.286244 -0.258058 0.204224 0.019297 -0.316166</float_array>
+ <technique_common>
+ <accessor source="#FRleg_rom_reduced-mesh-positions-array" count="13" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="FRleg_rom_reduced-mesh-normals">
+ <float_array id="FRleg_rom_reduced-mesh-normals-array" count="66">-0.5439389 -0.04593318 -0.8378667 0.7478583 -0.1179516 -0.6532958 -0.1642708 0.4066467 0.8986955 -0.870495 -0.1758364 -0.4596957 -0.4635697 -0.2550696 -0.8485534 -0.338153 -0.7567505 0.5594474 0.04972022 -0.8365821 0.5455809 -0.7943963 0.03824764 -0.6061944 -0.01682072 -0.9251666 0.3791888 -0.006144046 0.8641067 0.5032712 0.9005647 -0.1357303 -0.4129899 -0.001734435 0.8916919 0.4526396 0.01542162 0.8519797 0.5233477 -0.346762 0.2945427 -0.8905059 0.9954549 0.0753256 0.058272 -0.1387127 0.08525234 -0.9866565 0.4972101 -0.3278297 -0.8033118 -0.01814931 -0.9209716 0.3892068 0.664648 0.1263315 -0.7363989 -0.116993 -0.3609265 -0.9252269 -0.1672304 -0.3215169 -0.9320198 -0.694088 -0.5019115 -0.5160687</float_array>
+ <technique_common>
+ <accessor source="#FRleg_rom_reduced-mesh-normals-array" count="22" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="FRleg_rom_reduced-mesh-vertices">
+ <input semantic="POSITION" source="#FRleg_rom_reduced-mesh-positions"/>
+ </vertices>
+ <polylist material="rom-material" count="22">
+ <input semantic="VERTEX" source="#FRleg_rom_reduced-mesh-vertices" offset="0"/>
+ <input semantic="NORMAL" source="#FRleg_rom_reduced-mesh-normals" offset="1"/>
+ <vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+ <p>0 0 1 0 2 0 3 1 4 1 5 1 6 2 7 2 8 2 1 3 8 3 7 3 1 4 0 4 9 4 5 5 10 5 6 5 8 6 5 6 6 6 7 7 2 7 1 7 5 8 9 8 11 8 2 9 4 9 12 9 5 10 4 10 10 10 7 11 4 11 2 11 7 12 6 12 4 12 0 13 2 13 12 13 10 14 4 14 6 14 3 15 0 15 12 15 5 16 11 16 3 16 9 17 5 17 8 17 3 18 12 18 4 18 11 19 9 19 3 19 9 20 0 20 3 20 1 21 9 21 8 21</p>
+ </polylist>
+ </mesh>
+ </geometry>
+ <geometry id="FLleg_rom_reduced-mesh" name="FLleg rom reduced">
+ <mesh>
+ <source id="FLleg_rom_reduced-mesh-positions">
+ <float_array id="FLleg_rom_reduced-mesh-positions-array" count="39">-0.05230796 0.112418 -0.214907 0.06060099 0.139494 -0.30165 0.06531697 -0.001127958 -0.285981 -0.090061 0.06101995 -0.166024 0.224517 -0.009599983 -0.304541 0.323414 0.03115296 -0.2115859 0.270427 0.169416 -0.283745 -0.102657 0.197655 -0.07890999 0.320505 0.187932 -0.05384796 0.199666 0.295606 -0.254929 0.180181 0.108203 -0.331652 0.317855 0.243987 -0.1757889 -0.00712496 0.262961 -0.197787</float_array>
+ <technique_common>
+ <accessor source="#FLleg_rom_reduced-mesh-positions-array" count="13" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="FLleg_rom_reduced-mesh-normals">
+ <float_array id="FLleg_rom_reduced-mesh-normals-array" count="66">-0.5890314 -0.1089764 -0.8007286 -0.01944124 -0.9079052 0.4187246 0.007479131 -0.883897 0.467622 0.7027804 -0.09838402 -0.7045711 0.05451005 -0.5332208 0.8442183 -0.05793052 -0.7086024 0.703226 -0.1277992 0.3871233 -0.9131281 -0.8676933 0.2078964 -0.4515389 0.4799566 -0.02120459 -0.877036 0.9995777 0.0276255 -0.009023368 -0.3029583 0.5106828 -0.8046239 0.2466476 0.3450997 -0.9055778 0.1181776 0.9032027 0.4126246 0.6226528 0.4887046 -0.6111231 -0.006847679 0.8787448 0.4772429 -0.6264494 0.2711251 -0.7307889 -0.6493813 -0.2163732 -0.7290312 -0.007967889 0.8795391 0.47576 -0.1256038 -0.2671538 -0.9554333 -0.2700667 -0.1155674 -0.9558808 -0.8172565 0.3011596 -0.4913196 0.8836761 0.09999394 -0.4572941</float_array>
+ <technique_common>
+ <accessor source="#FLleg_rom_reduced-mesh-normals-array" count="22" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="FLleg_rom_reduced-mesh-vertices">
+ <input semantic="POSITION" source="#FLleg_rom_reduced-mesh-positions"/>
+ </vertices>
+ <polylist material="rom-material" count="22">
+ <input semantic="VERTEX" source="#FLleg_rom_reduced-mesh-vertices" offset="0"/>
+ <input semantic="NORMAL" source="#FLleg_rom_reduced-mesh-normals" offset="1"/>
+ <vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+ <p>0 0 1 0 2 0 3 1 4 1 5 1 3 2 2 2 4 2 6 3 5 3 4 3 7 4 3 4 5 4 5 5 8 5 7 5 1 6 9 6 10 6 0 7 3 7 7 7 6 8 4 8 10 8 5 9 11 9 8 9 1 10 12 10 9 10 9 11 6 11 10 11 8 12 11 12 9 12 6 13 9 13 11 13 7 14 9 14 12 14 12 15 1 15 0 15 2 16 3 16 0 16 9 17 7 17 8 17 4 18 2 18 10 18 2 19 1 19 10 19 7 20 12 20 0 20 5 21 6 21 11 21</p>
+ </polylist>
+ </mesh>
+ </geometry>
+ <geometry id="Cube-mesh" name="Cube">
+ <mesh>
+ <source id="Cube-mesh-positions">
+ <float_array id="Cube-mesh-positions-array" count="24">-1 -1 -1 -1 -1 1 -1 1 -1 -1 1 1 1 -1 -1 1 -1 1 1 1 -1 1 1 1</float_array>
+ <technique_common>
+ <accessor source="#Cube-mesh-positions-array" count="8" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="Cube-mesh-normals">
+ <float_array id="Cube-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
+ <technique_common>
+ <accessor source="#Cube-mesh-normals-array" count="6" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="Cube-mesh-vertices">
+ <input semantic="POSITION" source="#Cube-mesh-positions"/>
+ </vertices>
+ <polylist material="trunk-material" count="12">
+ <input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
+ <input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
+ <vcount>3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+ <p>3 0 0 0 1 0 7 1 2 1 3 1 5 2 6 2 7 2 1 3 4 3 5 3 2 4 4 4 0 4 7 5 1 5 5 5 3 0 2 0 0 0 7 1 6 1 2 1 5 2 4 2 6 2 1 3 0 3 4 3 2 4 6 4 4 4 7 5 3 5 1 5</p>
+ </polylist>
+ </mesh>
+ </geometry>
+ </library_geometries>
+ <library_controllers/>
+ <library_visual_scenes>
+ <visual_scene id="Scene" name="Scene">
+ <node id="HRleg_rom_reduced" name="HRleg_rom_reduced" type="NODE">
+ <matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
+ <instance_geometry url="#HRleg_rom_reduced-mesh" name="HRleg_rom_reduced">
+ <bind_material>
+ <technique_common>
+ <instance_material symbol="rom-material" target="#rom-material"/>
+ </technique_common>
+ </bind_material>
+ </instance_geometry>
+ </node>
+ <node id="HLleg_rom_reduced" name="HLleg_rom_reduced" type="NODE">
+ <matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
+ <instance_geometry url="#HLleg_rom_reduced-mesh" name="HLleg_rom_reduced">
+ <bind_material>
+ <technique_common>
+ <instance_material symbol="rom-material" target="#rom-material"/>
+ </technique_common>
+ </bind_material>
+ </instance_geometry>
+ </node>
+ <node id="FRleg_rom_reduced" name="FRleg_rom_reduced" type="NODE">
+ <matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
+ <instance_geometry url="#FRleg_rom_reduced-mesh" name="FRleg_rom_reduced">
+ <bind_material>
+ <technique_common>
+ <instance_material symbol="rom-material" target="#rom-material"/>
+ </technique_common>
+ </bind_material>
+ </instance_geometry>
+ </node>
+ <node id="FLleg_rom_reduced" name="FLleg_rom_reduced" type="NODE">
+ <matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
+ <instance_geometry url="#FLleg_rom_reduced-mesh" name="FLleg_rom_reduced">
+ <bind_material>
+ <technique_common>
+ <instance_material symbol="rom-material" target="#rom-material"/>
+ </technique_common>
+ </bind_material>
+ </instance_geometry>
+ </node>
+ <node id="Cube" name="Cube" type="NODE">
+ <matrix sid="transform">0.25 0 0 0 0 0.17 0 0 0 0 0.07 -0.03798383 0 0 0 1</matrix>
+ <instance_geometry url="#Cube-mesh" name="Cube">
+ <bind_material>
+ <technique_common>
+ <instance_material symbol="trunk-material" target="#trunk-material"/>
+ </technique_common>
+ </bind_material>
+ </instance_geometry>
+ </node>
+ </visual_scene>
+ </library_visual_scenes>
+ <scene>
+ <instance_visual_scene url="#Scene"/>
+ </scene>
+</COLLADA>
\ No newline at end of file
diff --git a/data/meshes/solo_trunk.stl b/data/meshes/solo_trunk.stl
new file mode 100644
index 0000000000000000000000000000000000000000..b933c10f9f4e7a04ce2466727e5b4758e50c18c7
GIT binary patch
literal 684
zcmZ>*D9A4=Nlj5mE6UGRaLP%|OGz!#HPSOTSI{UfO;Rw>WFQIf09F70U%wxwiIJgU
zUzLcSU6L}hEtI`)q1xTOFg8dWq86yj9>fC5*rDmLL(%~@32X;gHIj~fP&>f7p*r9u
z!R-LM4B2+5eK1#nbbw8Q=zx$7`=L5uE&;1TaurxNiVk}${y=si)D8k+1+#A-$W<Ww
z;QqiCR$$*ExeBbCU|508g}D~R4s2lscL{R1q4*qISi$@O4>uGY*uu&VImJLy2>`S5
BubKb=
literal 0
HcmV?d00001
diff --git a/data/package.xml b/data/package.xml
new file mode 100644
index 0000000..d0ec24b
--- /dev/null
+++ b/data/package.xml
@@ -0,0 +1,10 @@
+<package>
+ <name>solo-rbprm</name>
+ <version>1.0.0</version>
+ <description>
+ Info Database for using rbprm with solo.
+ </description>
+ <author>Pierre Fernbach</author>
+ <maintainer email="hpp@laas.fr">Pierre Fernbach</maintainer>
+ <build_depend>hpp-rbprm</build_depend>
+</package>
diff --git a/data/relative_effector_positions/FL_FOOT_constraints_in_FRleg_reduced.obj b/data/relative_effector_positions/FL_FOOT_constraints_in_FRleg_reduced.obj
new file mode 100644
index 0000000..1f1a35e
--- /dev/null
+++ b/data/relative_effector_positions/FL_FOOT_constraints_in_FRleg_reduced.obj
@@ -0,0 +1,64 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o FL_FOOT_constraints_in_FRleg_reduced_FL_FOOT_constraints_in_FRleg
+v -0.171040 0.119532 -0.199045
+v 0.223429 0.419426 0.186108
+v -0.178392 0.039695 0.081983
+v -0.342642 0.337888 -0.128080
+v 0.267294 0.487051 -0.071359
+v -0.150085 0.529545 0.039502
+v -0.425628 0.245269 0.048618
+v 0.332678 0.254889 -0.140179
+v 0.215664 0.045960 -0.078767
+v 0.367146 0.151523 0.020702
+v 0.236935 0.140896 0.165757
+v 0.400582 0.378704 0.051495
+v -0.298795 0.332580 0.206120
+v -0.161455 0.452507 -0.195645
+vn 0.4048 -0.8637 0.3001
+vn 0.7368 -0.1961 0.6470
+vn -0.3604 0.0199 -0.9326
+vn 0.0499 -0.0704 0.9963
+vn 0.2077 -0.3811 -0.9009
+vn 0.7742 0.3702 -0.5134
+vn 0.6916 -0.5312 -0.4895
+vn -0.6636 0.7430 -0.0873
+vn 0.2370 0.3369 -0.9112
+vn 0.0139 0.9500 -0.3119
+vn -0.0967 -0.9568 -0.2743
+vn -0.0810 -0.4170 0.9053
+vn -0.6031 -0.7630 -0.2325
+vn 0.1699 0.9456 0.2773
+vn 0.6005 -0.0293 0.7991
+vn 0.9567 -0.1040 -0.2718
+vn 0.1557 -0.9253 0.3457
+vn -0.7027 0.6870 0.1850
+vn -0.7332 -0.3967 -0.5523
+vn 0.4225 0.8564 0.2969
+vn -0.5136 -0.5051 0.6936
+vn 0.1143 0.0069 -0.9934
+vn -0.0852 0.6802 0.7280
+s off
+f 9//1 10//1 11//1
+f 11//2 10//2 12//2
+f 1//3 4//3 14//3
+f 2//4 13//4 11//4
+f 9//5 1//5 8//5
+f 12//6 8//6 5//6
+f 10//7 9//7 8//7
+f 6//8 14//8 4//8 7//8
+f 8//9 14//9 5//9
+f 14//10 6//10 5//10
+f 9//11 3//11 1//11
+f 13//12 3//12 11//12
+f 3//13 7//13 1//13
+f 2//14 5//14 6//14
+f 11//15 12//15 2//15
+f 10//16 8//16 12//16
+f 9//17 11//17 3//17
+f 13//18 6//18 7//18
+f 4//19 1//19 7//19
+f 12//20 5//20 2//20
+f 3//21 13//21 7//21
+f 14//22 8//22 1//22
+f 2//23 6//23 13//23
diff --git a/data/relative_effector_positions/FL_FOOT_constraints_in_HLleg_reduced.obj b/data/relative_effector_positions/FL_FOOT_constraints_in_HLleg_reduced.obj
new file mode 100644
index 0000000..f3bef04
--- /dev/null
+++ b/data/relative_effector_positions/FL_FOOT_constraints_in_HLleg_reduced.obj
@@ -0,0 +1,60 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o FL_FOOT_constraints_in_HLleg_reduced_FL_FOOT_constraints_in_HLleg
+v 0.480988 -0.212224 -0.195284
+v -0.155163 -0.099746 -0.015107
+v 0.072604 -0.164107 -0.170244
+v 0.227535 -0.275296 -0.015546
+v -0.104979 0.145844 -0.059562
+v 0.576409 -0.047789 -0.198880
+v 0.582247 -0.204357 -0.019412
+v 0.618152 0.167346 0.014701
+v 0.624426 0.035127 0.137910
+v -0.054940 0.097910 0.160143
+v 0.508171 0.142299 0.201238
+v 0.273673 0.263577 0.123349
+v 0.546733 0.126255 -0.150209
+v 0.526963 -0.157682 0.131438
+vn -0.4065 0.8690 0.2822
+vn 0.8025 -0.4199 0.4240
+vn 0.9683 0.0543 -0.2439
+vn -0.1208 0.2634 -0.9571
+vn -0.9342 0.2388 0.2649
+vn -0.0528 -0.2294 0.9719
+vn -0.0979 0.3969 0.9126
+vn 0.7199 0.4911 0.4904
+vn -0.0405 0.8762 -0.4802
+vn 0.1656 0.9377 -0.3054
+vn 0.9884 -0.0972 -0.1170
+vn -0.5689 -0.0325 -0.8218
+vn -0.0597 0.0128 -0.9981
+vn 0.3244 -0.1950 0.9256
+vn 0.7779 -0.4608 -0.4273
+vn 0.1872 -0.9170 0.3523
+vn -0.4061 -0.8846 -0.2292
+vn -0.1316 -0.8627 -0.4883
+vn 0.1949 -0.9784 -0.0685
+vn -0.2200 -0.5468 0.8078
+vn 0.3478 0.8801 0.3232
+s off
+f 5//1 10//1 12//1
+f 9//2 14//2 7//2
+f 9//3 6//3 13//3 8//3
+f 13//4 6//4 3//4 5//4
+f 2//5 10//5 5//5
+f 10//6 14//6 11//6
+f 10//7 11//7 12//7
+f 9//8 8//8 11//8
+f 13//9 5//9 12//9
+f 8//10 13//10 12//10
+f 6//11 9//11 7//11
+f 3//12 2//12 5//12
+f 3//13 6//13 1//13
+f 14//14 9//14 11//14
+f 7//15 1//15 6//15
+f 4//16 7//16 14//16
+f 4//17 2//17 3//17
+f 4//18 3//18 1//18
+f 4//19 1//19 7//19
+f 10//20 2//20 4//20 14//20
+f 8//21 12//21 11//21
diff --git a/data/relative_effector_positions/FL_FOOT_constraints_in_HRleg_reduced.obj b/data/relative_effector_positions/FL_FOOT_constraints_in_HRleg_reduced.obj
new file mode 100644
index 0000000..6bec07a
--- /dev/null
+++ b/data/relative_effector_positions/FL_FOOT_constraints_in_HRleg_reduced.obj
@@ -0,0 +1,62 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o FL_FOOT_constraints_in_HRleg_reduced_FL_FOOT_constraints_in_HRleg
+v 0.604822 0.174421 -0.158848
+v 0.503193 0.464004 0.186880
+v 0.080992 0.034516 -0.055848
+v 0.575006 0.462746 -0.117402
+v 0.034648 0.445921 -0.106550
+v -0.149208 0.157734 0.034437
+v 0.246475 0.541169 -0.017302
+v -0.111691 0.262374 -0.128778
+v 0.559883 0.388869 -0.200330
+v -0.083610 0.451308 0.067019
+v 0.387887 0.016305 -0.068712
+v 0.516112 0.126099 0.189902
+v 0.620257 0.281047 0.113893
+v 0.142911 0.158234 0.191743
+vn 0.8596 0.4698 0.2009
+vn -0.3118 0.9191 -0.2409
+vn 0.0057 0.0092 0.9999
+vn -0.0599 -0.3499 -0.9349
+vn -0.0874 -0.4001 -0.9123
+vn -0.4741 0.0082 0.8804
+vn -0.5521 -0.6385 -0.5362
+vn -0.0365 0.7366 -0.6754
+vn -0.6619 0.5846 -0.4691
+vn -0.0338 -0.8906 0.4535
+vn -0.0659 -0.2028 -0.9770
+vn -0.0757 -0.9038 0.4213
+vn -0.1160 0.8721 0.4754
+vn 0.8529 -0.5009 0.1475
+vn -0.2585 -0.8367 0.4828
+vn 0.9816 0.1226 -0.1465
+vn 0.5609 0.0288 0.8274
+vn 0.2475 0.9674 0.0544
+vn -0.1990 0.2495 0.9477
+vn -0.9710 0.2257 -0.0785
+vn -0.1463 0.2331 -0.9614
+vn 0.5998 -0.7991 0.0419
+s off
+f 2//1 13//1 4//1
+f 10//2 7//2 5//2
+f 12//3 2//3 14//3
+f 3//4 8//4 11//4
+f 11//5 8//5 1//5
+f 6//6 14//6 10//6
+f 8//7 3//7 6//7
+f 7//8 4//8 9//8 5//8
+f 8//9 10//9 5//9
+f 11//10 14//10 3//10
+f 9//11 1//11 8//11
+f 14//12 11//12 12//12
+f 10//13 2//13 7//13
+f 1//14 13//14 12//14
+f 3//15 14//15 6//15
+f 9//16 4//16 13//16 1//16
+f 12//17 13//17 2//17
+f 2//18 4//18 7//18
+f 14//19 2//19 10//19
+f 6//20 10//20 8//20
+f 8//21 5//21 9//21
+f 11//22 1//22 12//22
diff --git a/data/relative_effector_positions/FR_FOOT_constraints_in_FLleg_reduced.obj b/data/relative_effector_positions/FR_FOOT_constraints_in_FLleg_reduced.obj
new file mode 100644
index 0000000..90cdb05
--- /dev/null
+++ b/data/relative_effector_positions/FR_FOOT_constraints_in_FLleg_reduced.obj
@@ -0,0 +1,66 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o FR_FOOT_constraints_in_FLleg_reduced_FR_FOOT_constraints_in_FLleg
+v -0.361445 -0.142162 -0.040293
+v -0.368738 -0.316657 -0.145286
+v 0.361953 -0.293405 -0.183682
+v -0.176371 -0.432102 -0.185302
+v 0.127738 -0.019193 -0.068692
+v -0.109363 -0.141074 -0.197207
+v 0.150353 -0.529386 -0.042090
+v 0.360730 -0.377320 0.056150
+v 0.395097 -0.220458 0.005795
+v 0.158752 -0.454046 0.196554
+v 0.219702 -0.143859 0.202719
+v -0.213865 -0.104953 0.154368
+v -0.350250 -0.465950 0.024739
+v -0.347636 -0.256134 0.126503
+vn 0.5994 -0.1335 0.7893
+vn -0.1543 -0.0454 0.9870
+vn -0.2002 0.0060 -0.9797
+vn -0.4679 0.4699 -0.7485
+vn 0.9683 -0.2372 -0.0781
+vn 0.4893 -0.8364 0.2468
+vn -0.1577 -0.9461 -0.2830
+vn -0.7126 0.5504 0.4351
+vn -0.0799 -0.9497 0.3026
+vn 0.5584 0.8093 0.1825
+vn 0.6221 0.6880 -0.3737
+vn -0.9974 -0.0207 0.0684
+vn -0.9954 -0.0154 0.0947
+vn 0.0144 -0.0442 -0.9989
+vn 0.1592 -0.6090 -0.7770
+vn 0.6412 -0.7253 -0.2505
+vn 0.0365 0.9128 0.4069
+vn -0.5140 -0.6715 -0.5337
+vn -0.1107 0.0020 0.9939
+vn 0.1899 0.5132 -0.8370
+vn -0.2458 0.8880 -0.3887
+vn -0.2438 0.9698 -0.0005
+vn 0.7546 0.0448 0.6547
+vn -0.2821 -0.4158 0.8646
+s off
+f 8//1 11//1 10//1
+f 14//2 10//2 12//2
+f 4//3 2//3 6//3
+f 2//4 1//4 6//4
+f 9//5 8//5 3//5
+f 7//6 8//6 10//6
+f 13//7 4//7 7//7
+f 14//8 12//8 1//8
+f 7//9 10//9 13//9
+f 11//10 9//10 5//10
+f 3//11 5//11 9//11
+f 1//12 13//12 14//12
+f 13//13 1//13 2//13
+f 4//14 6//14 3//14
+f 4//15 3//15 7//15
+f 7//16 3//16 8//16
+f 12//17 11//17 5//17
+f 13//18 2//18 4//18
+f 12//19 10//19 11//19
+f 6//20 5//20 3//20
+f 5//21 6//21 1//21
+f 1//22 12//22 5//22
+f 8//23 9//23 11//23
+f 14//24 13//24 10//24
diff --git a/data/relative_effector_positions/FR_FOOT_constraints_in_HLleg_reduced.obj b/data/relative_effector_positions/FR_FOOT_constraints_in_HLleg_reduced.obj
new file mode 100644
index 0000000..5c9939d
--- /dev/null
+++ b/data/relative_effector_positions/FR_FOOT_constraints_in_HLleg_reduced.obj
@@ -0,0 +1,64 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o FR_FOOT_constraints_in_HLleg_reduced_FR_FOOT_constraints_in_HLleg
+v -0.118686 -0.131954 -0.063627
+v 0.251003 -0.009364 0.037261
+v 0.088086 -0.467101 -0.163594
+v 0.012527 -0.202433 0.163562
+v 0.523292 -0.162003 0.194505
+v 0.620671 -0.129739 0.046048
+v 0.321361 -0.555352 0.007755
+v 0.059456 -0.469360 0.109201
+v 0.476971 -0.464887 0.173533
+v -0.173457 -0.377147 -0.022964
+v 0.510434 -0.402942 -0.196629
+v 0.593288 -0.441687 -0.091410
+v 0.567776 -0.158001 -0.189647
+v 0.610190 -0.358364 0.167606
+vn 0.4154 -0.9038 0.1032
+vn -0.0964 0.6256 0.7742
+vn 0.6169 -0.7601 0.2043
+vn 0.9907 -0.0282 -0.1334
+vn -0.1708 0.1834 -0.9681
+vn -0.0857 0.7744 -0.6269
+vn -0.3263 -0.9443 -0.0421
+vn -0.3908 0.7908 0.4710
+vn -0.4831 -0.0372 -0.8748
+vn 0.0842 -0.8346 -0.5444
+vn 0.3084 0.9338 -0.1812
+vn 0.2789 0.8840 0.3751
+vn -0.7960 0.2678 0.5428
+vn -0.1462 -0.2220 0.9640
+vn 0.1126 -0.0858 0.9899
+vn -0.5521 -0.2585 0.7927
+vn -0.0725 -0.8453 0.5293
+vn 0.7471 -0.1565 -0.6460
+vn -0.0852 0.0483 -0.9952
+vn -0.3460 -0.9372 -0.0441
+vn 0.7783 0.2666 0.5685
+vn 0.1903 -0.8632 -0.4677
+vn -0.0556 -0.0605 0.9966
+s off
+f 7//1 12//1 9//1
+f 2//2 4//2 5//2
+f 9//3 12//3 14//3
+f 14//4 12//4 13//4 6//4
+f 13//5 3//5 1//5
+f 2//6 13//6 1//6
+f 7//7 8//7 3//7
+f 4//8 2//8 1//8
+f 3//9 10//9 1//9
+f 11//10 7//10 3//10
+f 13//11 2//11 6//11
+f 5//12 6//12 2//12
+f 4//13 1//13 10//13
+f 8//14 9//14 4//14
+f 9//15 14//15 5//15
+f 4//16 10//16 8//16
+f 7//17 9//17 8//17
+f 12//18 11//18 13//18
+f 13//19 11//19 3//19
+f 3//20 8//20 10//20
+f 14//21 6//21 5//21
+f 12//22 7//22 11//22
+f 9//23 5//23 4//23
diff --git a/data/relative_effector_positions/FR_FOOT_constraints_in_HRleg_reduced.obj b/data/relative_effector_positions/FR_FOOT_constraints_in_HRleg_reduced.obj
new file mode 100644
index 0000000..030ec0d
--- /dev/null
+++ b/data/relative_effector_positions/FR_FOOT_constraints_in_HRleg_reduced.obj
@@ -0,0 +1,64 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o FR_FOOT_constraints_in_HRleg_reduced_FR_FOOT_constraints_in_HRleg
+v 0.336018 -0.274837 0.067000
+v 0.356553 -0.231466 0.181387
+v -0.072389 0.080492 -0.136546
+v 0.549002 -0.114650 -0.161487
+v -0.115511 -0.151284 -0.019166
+v 0.599394 0.083251 0.125979
+v 0.620764 -0.144933 0.139686
+v 0.459850 0.108171 0.184659
+v 0.195274 0.213512 -0.158022
+v -0.066363 -0.088048 0.132598
+v 0.634273 0.135517 -0.028822
+v -0.027016 0.098358 0.083169
+v 0.325079 0.286856 -0.036513
+v 0.542228 0.144182 -0.194812
+vn -0.2009 0.2905 0.9356
+vn -0.0918 -0.7731 -0.6276
+vn -0.1069 0.0229 0.9940
+vn 0.3384 -0.8984 0.2799
+vn -0.3328 -0.8273 0.4525
+vn 0.0979 0.8023 -0.5888
+vn 0.1757 -0.0629 0.9824
+vn 0.3106 0.8772 0.3662
+vn 0.8730 -0.0397 -0.4862
+vn 0.9498 -0.1934 -0.2458
+vn 0.3999 0.0922 0.9119
+vn -0.3112 -0.8687 0.3853
+vn -0.4481 0.8920 -0.0597
+vn -0.4654 0.8847 0.0242
+vn -0.9477 0.2604 0.1843
+vn 0.4478 -0.8727 -0.1945
+vn 0.3898 0.8179 0.4232
+vn -0.0981 0.0368 -0.9945
+vn -0.1678 -0.4204 -0.8917
+vn -0.0803 -0.1294 -0.9883
+vn 0.4351 0.8789 -0.1954
+vn -0.1555 0.7199 0.6764
+vn 0.9619 0.1052 0.2523
+s off
+f 12//1 10//1 8//1
+f 1//2 5//2 4//2
+f 10//3 2//3 8//3
+f 7//4 2//4 1//4
+f 2//5 10//5 5//5
+f 14//6 9//6 13//6
+f 8//7 2//7 7//7
+f 6//8 11//8 8//8
+f 4//9 14//9 11//9
+f 4//10 11//10 7//10
+f 7//11 6//11 8//11
+f 1//12 2//12 5//12
+f 9//13 3//13 13//13
+f 3//14 12//14 13//14
+f 5//15 10//15 12//15 3//15
+f 4//16 7//16 1//16
+f 8//17 11//17 13//17
+f 3//18 9//18 14//18
+f 4//19 5//19 3//19
+f 14//20 4//20 3//20
+f 14//21 13//21 11//21
+f 12//22 8//22 13//22
+f 6//23 7//23 11//23
diff --git a/data/relative_effector_positions/HL_FOOT_constraints_in_FLleg_reduced.obj b/data/relative_effector_positions/HL_FOOT_constraints_in_FLleg_reduced.obj
new file mode 100644
index 0000000..723d826
--- /dev/null
+++ b/data/relative_effector_positions/HL_FOOT_constraints_in_FLleg_reduced.obj
@@ -0,0 +1,62 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o HL_FOOT_constraints_in_FLleg_reduced_HL_FOOT_constraints_in_FLleg
+v -0.480921 0.212104 0.195380
+v -0.196758 0.267013 0.016001
+v -0.064965 -0.034918 -0.183991
+v -0.592833 0.058132 0.198084
+v -0.072604 0.164107 0.170244
+v 0.161718 -0.075309 -0.017559
+v 0.018492 -0.167339 0.089857
+v 0.163115 0.095958 0.025017
+v -0.506155 -0.144048 -0.201272
+v -0.538798 0.208281 -0.092064
+v -0.628611 -0.160570 -0.019601
+v -0.549505 -0.126323 0.149938
+v -0.611097 -0.024336 -0.149826
+v -0.262740 -0.261787 -0.125725
+vn 0.4100 0.8759 0.2544
+vn -0.1450 0.2807 -0.9488
+vn 0.5089 -0.0682 0.8581
+vn -0.9532 0.3016 -0.0214
+vn -0.7361 -0.4158 -0.5340
+vn -0.7981 0.5828 0.1529
+vn -0.3355 -0.8906 -0.3071
+vn -0.8344 -0.3142 0.4528
+vn 0.2242 0.6043 -0.7646
+vn -0.0350 0.2929 -0.9555
+vn -0.9894 0.1442 0.0178
+vn 0.1218 -0.3452 -0.9306
+vn -0.1813 -0.9441 0.2753
+vn -0.0206 -0.9056 0.4236
+vn 0.0585 -0.0250 0.9980
+vn -0.1761 0.9841 0.0224
+vn 0.6028 0.1879 -0.7755
+vn 0.4336 -0.8832 -0.1786
+vn 0.6631 -0.1857 0.7251
+vn 0.4259 -0.5548 -0.7147
+vn 0.0915 -0.2182 0.9716
+vn 0.1315 0.8702 0.4747
+s off
+f 8//1 2//1 5//1
+f 13//2 10//2 9//2
+f 7//3 8//3 5//3
+f 13//4 4//4 10//4
+f 9//5 11//5 13//5
+f 1//6 10//6 4//6
+f 11//7 9//7 14//7
+f 12//8 4//8 11//8
+f 2//9 8//9 3//9 10//9
+f 9//10 10//10 3//10
+f 4//11 13//11 11//11
+f 14//12 9//12 3//12
+f 14//13 12//13 11//13
+f 12//14 14//14 7//14
+f 4//15 5//15 1//15
+f 10//16 1//16 2//16
+f 6//17 3//17 8//17
+f 14//18 6//18 7//18
+f 6//19 8//19 7//19
+f 6//20 14//20 3//20
+f 5//21 4//21 12//21 7//21
+f 5//22 2//22 1//22
diff --git a/data/relative_effector_positions/HL_FOOT_constraints_in_FRleg_reduced.obj b/data/relative_effector_positions/HL_FOOT_constraints_in_FRleg_reduced.obj
new file mode 100644
index 0000000..0dc0a05
--- /dev/null
+++ b/data/relative_effector_positions/HL_FOOT_constraints_in_FRleg_reduced.obj
@@ -0,0 +1,64 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o HL_FOOT_constraints_in_FRleg_reduced_HL_FOOT_constraints_in_FRleg
+v 0.127881 0.131701 0.054062
+v -0.567651 0.157877 0.189624
+v -0.620327 0.129438 -0.046167
+v -0.239183 0.556938 0.041137
+v -0.610668 0.357731 -0.167598
+v -0.511388 0.398822 0.197323
+v -0.591808 0.443913 0.090276
+v -0.067157 0.174166 -0.170458
+v -0.254057 0.008942 -0.037113
+v 0.170041 0.381834 0.026965
+v 0.024282 0.423994 -0.107677
+v -0.476971 0.464887 -0.173533
+v -0.063764 0.411577 0.175449
+v -0.523423 0.161921 -0.194263
+vn -0.7778 -0.2692 -0.5680
+vn 0.0567 -0.6651 -0.7446
+vn 0.3699 0.8307 0.4160
+vn 0.3699 -0.7999 -0.4726
+vn -0.9902 0.0306 0.1364
+vn 0.0500 -0.0436 0.9978
+vn 0.1725 -0.2901 0.9413
+vn -0.1120 0.0849 -0.9901
+vn 0.3861 0.9130 -0.1321
+vn 0.6365 -0.1883 -0.7480
+vn 0.1432 0.1916 -0.9710
+vn -0.3123 0.9480 -0.0606
+vn 0.0949 -0.7668 0.6349
+vn -0.3113 -0.9327 0.1820
+vn -0.2810 -0.8824 -0.3774
+vn -0.1992 0.8406 0.5037
+vn 0.0158 0.6888 0.7248
+vn 0.5362 0.0011 0.8441
+vn -0.7569 0.1565 0.6345
+vn 0.7409 -0.1045 -0.6634
+vn 0.1366 0.8466 -0.5144
+vn -0.6173 0.7586 -0.2084
+vn 0.0504 0.0605 -0.9969
+s off
+f 3//1 5//1 14//1
+f 9//2 14//2 8//2
+f 13//3 10//3 4//3
+f 1//4 9//4 8//4
+f 5//5 3//5 2//5 7//5
+f 6//6 2//6 13//6
+f 2//7 1//7 13//7
+f 12//8 14//8 5//8
+f 11//9 4//9 10//9
+f 1//10 11//10 10//10
+f 8//11 12//11 11//11
+f 12//12 7//12 4//12
+f 2//13 9//13 1//13
+f 9//14 2//14 3//14
+f 14//15 9//15 3//15
+f 4//16 7//16 6//16
+f 6//17 13//17 4//17
+f 1//18 10//18 13//18
+f 2//19 6//19 7//19
+f 1//20 8//20 11//20
+f 12//21 4//21 11//21
+f 5//22 7//22 12//22
+f 14//23 12//23 8//23
diff --git a/data/relative_effector_positions/HL_FOOT_constraints_in_HRleg_reduced.obj b/data/relative_effector_positions/HL_FOOT_constraints_in_HRleg_reduced.obj
new file mode 100644
index 0000000..00aff44
--- /dev/null
+++ b/data/relative_effector_positions/HL_FOOT_constraints_in_HRleg_reduced.obj
@@ -0,0 +1,64 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o HL_FOOT_constraints_in_HRleg_reduced_HL_FOOT_constraints_in_HRleg
+v -0.193981 0.448561 0.178830
+v -0.260178 0.225080 0.198226
+v 0.329258 0.203197 0.156630
+v 0.241370 0.152259 -0.184556
+v -0.063535 0.014903 0.073354
+v -0.270107 0.508918 -0.056197
+v -0.382289 0.352310 0.069291
+v 0.204609 0.393077 -0.191105
+v -0.325684 0.369187 -0.164860
+v 0.230539 0.497326 0.100747
+v 0.378803 0.326149 0.102255
+v 0.384562 0.308685 -0.056513
+v -0.370618 0.135055 -0.092667
+v 0.308707 0.458379 -0.075501
+vn -0.1357 -0.2681 -0.9538
+vn 0.3105 0.2770 0.9093
+vn -0.5853 0.7192 0.3743
+vn 0.9357 -0.3453 0.0719
+vn 0.7754 -0.6224 -0.1068
+vn -0.8612 0.4777 -0.1738
+vn -0.0614 0.9618 0.2669
+vn 0.8928 0.4502 -0.0171
+vn -0.0667 0.6289 -0.7746
+vn 0.4279 -0.9035 0.0247
+vn -0.0479 -0.0345 -0.9983
+vn -0.8285 -0.3627 0.4268
+vn -0.9673 0.1161 -0.2255
+vn 0.0726 0.0649 0.9952
+vn 0.7418 0.6441 0.1867
+vn 0.0440 -0.4795 0.8764
+vn -0.5201 -0.7400 0.4265
+vn -0.0538 -0.8536 -0.5182
+vn 0.6328 0.1326 -0.7629
+vn 0.0563 0.8468 -0.5290
+vn 0.1500 0.2329 0.9609
+vn 0.0795 0.9802 -0.1813
+vn -0.5765 0.2386 0.7815
+s off
+f 13//1 9//1 4//1
+f 3//2 11//2 10//2
+f 7//3 1//3 6//3
+f 12//4 11//4 3//4
+f 12//5 3//5 4//5
+f 6//6 9//6 7//6
+f 6//7 1//7 10//7
+f 12//8 14//8 11//8
+f 8//9 9//9 6//9
+f 3//10 5//10 4//10
+f 9//11 8//11 4//11
+f 13//12 2//12 7//12
+f 13//13 7//13 9//13
+f 3//14 1//14 2//14
+f 14//15 10//15 11//15
+f 5//16 3//16 2//16
+f 13//17 5//17 2//17
+f 4//18 5//18 13//18
+f 8//19 14//19 12//19 4//19
+f 14//20 8//20 6//20
+f 3//21 10//21 1//21
+f 10//22 14//22 6//22
+f 1//23 7//23 2//23
diff --git a/data/relative_effector_positions/HR_FOOT_constraints_in_FLleg_reduced.obj b/data/relative_effector_positions/HR_FOOT_constraints_in_FLleg_reduced.obj
new file mode 100644
index 0000000..762db2d
--- /dev/null
+++ b/data/relative_effector_positions/HR_FOOT_constraints_in_FLleg_reduced.obj
@@ -0,0 +1,62 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o HR_FOOT_constraints_in_FLleg_reduced_HR_FOOT_constraints_in_FLleg
+v -0.502383 -0.463015 -0.187297
+v -0.059029 -0.038734 0.052491
+v 0.147233 -0.158657 -0.036964
+v 0.081273 -0.452738 -0.067537
+v -0.605835 -0.176899 0.160575
+v -0.577916 -0.463602 0.114101
+v -0.244381 -0.541328 0.019105
+v -0.034995 -0.444257 0.105319
+v -0.518056 -0.126189 -0.190021
+v -0.560071 -0.388300 0.200249
+v -0.618340 -0.275052 -0.116519
+v 0.112753 -0.257048 0.128376
+v -0.384186 -0.014209 0.066997
+v -0.142911 -0.158234 -0.191743
+vn -0.2435 -0.9679 -0.0629
+vn 0.4706 -0.0138 -0.8822
+vn 0.9719 -0.2251 0.0687
+vn 0.6620 -0.5808 0.4738
+vn -0.9833 -0.1182 0.1386
+vn 0.0371 -0.7306 0.6818
+vn 0.3169 -0.9127 0.2580
+vn 0.0748 0.8993 -0.4308
+vn -0.6009 0.7986 -0.0349
+vn 0.0861 0.4061 0.9098
+vn -0.8652 -0.4518 -0.2177
+vn -0.5588 -0.0327 -0.8287
+vn 0.0470 0.8908 -0.4520
+vn -0.8519 0.5046 -0.1403
+vn 0.1123 -0.8744 -0.4721
+vn 0.0696 0.3759 0.9240
+vn 0.1989 -0.2484 -0.9480
+vn 0.5870 0.6360 0.5009
+vn 0.2657 0.8268 -0.4958
+vn 0.1485 -0.2355 0.9605
+vn -0.0053 -0.0083 -1.0000
+vn 0.0659 0.1978 0.9780
+s off
+f 6//1 1//1 7//1
+f 4//2 14//2 3//2
+f 12//3 4//3 3//3
+f 12//4 8//4 4//4
+f 10//5 5//5 11//5 6//5
+f 10//6 6//6 7//6 8//6
+f 8//7 7//7 4//7
+f 13//8 14//8 9//8
+f 9//9 5//9 13//9
+f 5//10 12//10 13//10
+f 1//11 6//11 11//11
+f 11//12 9//12 1//12
+f 14//13 13//13 2//13
+f 5//14 9//14 11//14
+f 7//15 1//15 4//15
+f 12//16 2//16 13//16
+f 1//17 14//17 4//17
+f 12//18 3//18 2//18
+f 3//19 14//19 2//19
+f 12//20 10//20 8//20
+f 1//21 9//21 14//21
+f 10//22 12//22 5//22
diff --git a/data/relative_effector_positions/HR_FOOT_constraints_in_FRleg_reduced.obj b/data/relative_effector_positions/HR_FOOT_constraints_in_FRleg_reduced.obj
new file mode 100644
index 0000000..57b9a27
--- /dev/null
+++ b/data/relative_effector_positions/HR_FOOT_constraints_in_FRleg_reduced.obj
@@ -0,0 +1,66 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o HR_FOOT_constraints_in_FRleg_reduced_HR_FOOT_constraints_in_FRleg
+v -0.050171 -0.022323 0.181764
+v -0.548975 0.114384 0.161746
+v 0.063245 0.165782 0.042340
+v 0.115068 -0.091682 0.055652
+v -0.542195 -0.144266 0.194715
+v 0.029879 -0.098073 -0.082224
+v -0.237990 0.271534 -0.030248
+v -0.612983 0.170138 -0.133841
+v -0.331200 0.213073 -0.201270
+v -0.519755 -0.108385 -0.178524
+v -0.635709 -0.134683 0.026639
+v -0.196455 -0.214043 0.156540
+v -0.325079 -0.286856 0.036513
+v 0.128040 0.092318 -0.072427
+vn -0.4366 -0.8787 0.1928
+vn 0.6416 0.1678 0.7484
+vn 0.9025 0.2013 0.3807
+vn 0.3976 -0.9090 0.1253
+vn 0.1161 0.5453 0.8302
+vn 0.3450 0.8145 -0.4665
+vn -0.8722 0.0393 0.4875
+vn -0.9494 0.1988 0.2431
+vn 0.7931 -0.3846 -0.4722
+vn 0.2499 -0.0792 -0.9650
+vn 0.4040 -0.8907 -0.2083
+vn -0.2427 0.0739 -0.9673
+vn 0.1732 -0.1702 -0.9701
+vn -0.0052 0.1263 0.9920
+vn -0.1979 0.9555 -0.2188
+vn -0.4054 -0.8494 -0.3380
+vn 0.3829 0.8609 -0.3349
+vn -0.8415 -0.2012 -0.5014
+vn 0.0713 -0.1834 0.9805
+vn 0.0781 0.7057 0.7041
+vn 0.1362 -0.6982 -0.7028
+vn -0.0967 -0.8017 0.5899
+vn 0.4266 -0.4302 0.7956
+vn -0.3149 0.9179 0.2413
+s off
+f 13//1 5//1 11//1
+f 1//2 4//2 3//2
+f 4//3 14//3 3//3
+f 12//4 13//4 4//4
+f 2//5 1//5 3//5
+f 14//6 9//6 7//6
+f 11//7 5//7 2//7
+f 11//8 2//8 8//8
+f 6//9 14//9 4//9
+f 6//10 9//10 14//10
+f 13//11 6//11 4//11
+f 8//12 9//12 10//12
+f 6//13 10//13 9//13
+f 2//14 5//14 1//14
+f 7//15 9//15 8//15
+f 11//16 10//16 13//16
+f 3//17 14//17 7//17
+f 10//18 11//18 8//18
+f 5//19 12//19 1//19
+f 2//20 3//20 7//20
+f 10//21 6//21 13//21
+f 13//22 12//22 5//22
+f 4//23 1//23 12//23
+f 2//24 7//24 8//24
diff --git a/data/relative_effector_positions/HR_FOOT_constraints_in_HLleg_reduced.obj b/data/relative_effector_positions/HR_FOOT_constraints_in_HLleg_reduced.obj
new file mode 100644
index 0000000..52c151d
--- /dev/null
+++ b/data/relative_effector_positions/HR_FOOT_constraints_in_HLleg_reduced.obj
@@ -0,0 +1,64 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o HR_FOOT_constraints_in_HLleg_reduced_HR_FOOT_constraints_in_HLleg
+v -0.328369 -0.201840 -0.156535
+v 0.244054 -0.183722 -0.193492
+v 0.382536 -0.352429 -0.068799
+v 0.326481 -0.368497 0.164055
+v 0.100900 -0.016325 -0.047299
+v -0.380160 -0.327748 -0.097522
+v -0.383770 -0.311173 0.058917
+v -0.308719 -0.458379 0.075492
+v 0.374546 -0.143399 0.090642
+v -0.203736 -0.392663 0.191809
+v 0.186055 -0.461932 -0.176114
+v -0.230595 -0.497317 -0.100741
+v 0.271657 -0.507177 0.058437
+v -0.239150 -0.152539 0.184786
+vn -0.8918 -0.4516 0.0273
+vn 0.8272 0.3604 -0.4311
+vn -0.6267 -0.1319 0.7681
+vn -0.0557 -0.8469 0.5288
+vn 0.1415 0.2795 0.9497
+vn 0.0459 -0.9776 -0.2053
+vn -0.9327 0.3559 -0.0592
+vn -0.3898 0.9204 -0.0310
+vn -0.7775 0.6185 0.1139
+vn -0.0771 -0.9802 0.1824
+vn 0.5958 -0.7198 -0.3563
+vn 0.8628 -0.4743 0.1750
+vn 0.0781 0.8056 0.5873
+vn -0.0700 0.6215 -0.7803
+vn 0.0692 -0.6216 0.7803
+vn 0.5418 -0.1644 -0.8242
+vn 0.9656 -0.1334 0.2232
+vn -0.0628 -0.0492 -0.9968
+vn -0.7388 -0.6482 -0.1843
+vn -0.1540 -0.2323 -0.9604
+vn 0.0506 0.0367 0.9980
+vn -0.3385 -0.2816 -0.8978
+vn 0.5349 0.7670 -0.3545
+s off
+f 8//1 7//1 6//1
+f 3//2 2//2 9//2
+f 7//3 8//3 10//3 14//3
+f 8//4 13//4 10//4
+f 14//5 4//5 9//5
+f 12//6 11//6 13//6
+f 6//7 7//7 1//7
+f 1//8 14//8 5//8
+f 7//9 14//9 1//9
+f 8//10 12//10 13//10
+f 13//11 11//11 3//11
+f 3//12 4//12 13//12
+f 9//13 5//13 14//13
+f 2//14 1//14 5//14
+f 10//15 13//15 4//15
+f 11//16 2//16 3//16
+f 3//17 9//17 4//17
+f 2//18 11//18 1//18
+f 8//19 6//19 12//19
+f 12//20 1//20 11//20
+f 4//21 14//21 10//21
+f 12//22 6//22 1//22
+f 9//23 2//23 5//23
diff --git a/data/roms/FLleg_rom_reduced.obj b/data/roms/FLleg_rom_reduced.obj
new file mode 100644
index 0000000..bcf2dda
--- /dev/null
+++ b/data/roms/FLleg_rom_reduced.obj
@@ -0,0 +1,59 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o FLleg_rom_reduced_FLleg_rom
+v 0.180181 0.108203 -0.331652
+v -0.102657 0.197655 -0.078910
+v 0.320505 0.187932 -0.053848
+v 0.224517 -0.009600 -0.304541
+v 0.060601 0.139494 -0.301650
+v -0.090061 0.061020 -0.166024
+v 0.065317 -0.001128 -0.285981
+v 0.323414 0.031153 -0.211586
+v 0.317855 0.243987 -0.175789
+v 0.270427 0.169416 -0.283745
+v -0.007125 0.262961 -0.197787
+v 0.199666 0.295606 -0.254929
+v -0.052308 0.112418 -0.214907
+vn -0.5890 -0.1090 -0.8007
+vn -0.0194 -0.9079 0.4187
+vn 0.0075 -0.8839 0.4676
+vn 0.7028 -0.0984 -0.7046
+vn -0.0115 -0.6410 0.7674
+vn -0.1278 0.3871 -0.9131
+vn -0.8677 0.2079 -0.4515
+vn 0.4800 -0.0212 -0.8770
+vn 0.9996 0.0276 -0.0090
+vn -0.3030 0.5107 -0.8046
+vn 0.2466 0.3451 -0.9056
+vn 0.1182 0.9032 0.4126
+vn 0.6227 0.4887 -0.6111
+vn -0.0068 0.8787 0.4772
+vn -0.6264 0.2711 -0.7308
+vn -0.6494 -0.2164 -0.7290
+vn -0.0080 0.8795 0.4758
+vn -0.1256 -0.2672 -0.9554
+vn -0.2701 -0.1156 -0.9559
+vn -0.8173 0.3012 -0.4913
+vn 0.8837 0.1000 -0.4573
+s off
+f 13//1 5//1 7//1
+f 6//2 4//2 8//2
+f 6//3 7//3 4//3
+f 10//4 8//4 4//4
+f 2//5 6//5 8//5 3//5
+f 5//6 12//6 1//6
+f 13//7 6//7 2//7
+f 10//8 4//8 1//8
+f 8//9 9//9 3//9
+f 5//10 11//10 12//10
+f 12//11 10//11 1//11
+f 3//12 9//12 12//12
+f 10//13 12//13 9//13
+f 2//14 12//14 11//14
+f 11//15 5//15 13//15
+f 7//16 6//16 13//16
+f 12//17 2//17 3//17
+f 4//18 7//18 1//18
+f 7//19 5//19 1//19
+f 2//20 11//20 13//20
+f 8//21 10//21 9//21
diff --git a/data/roms/FRleg_rom_reduced.obj b/data/roms/FRleg_rom_reduced.obj
new file mode 100644
index 0000000..02704bb
--- /dev/null
+++ b/data/roms/FRleg_rom_reduced.obj
@@ -0,0 +1,61 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o FRleg_rom_reduced_FRleg_rom
+v 0.317027 -0.149798 -0.034588
+v 0.319285 -0.034949 -0.221622
+v 0.329058 -0.230240 -0.136128
+v 0.210204 -0.286244 -0.258058
+v -0.038307 -0.150062 -0.223232
+v 0.285528 -0.271047 -0.217638
+v 0.069110 -0.277728 -0.243539
+v -0.015802 -0.023855 -0.244761
+v 0.214533 -0.106999 -0.328528
+v -0.095416 -0.075322 -0.143677
+v -0.099864 -0.207302 -0.084771
+v 0.091104 -0.095886 -0.310215
+v 0.204224 0.019297 -0.316166
+vn -0.5439 -0.0459 -0.8379
+vn 0.7479 -0.1180 -0.6533
+vn -0.1643 0.4066 0.8987
+vn -0.8705 -0.1758 -0.4597
+vn -0.4636 -0.2551 -0.8486
+vn -0.3382 -0.7568 0.5594
+vn 0.0497 -0.8366 0.5456
+vn -0.7944 0.0382 -0.6062
+vn -0.0168 -0.9252 0.3792
+vn -0.0061 0.8641 0.5033
+vn 0.9006 -0.1357 -0.4130
+vn -0.0017 0.8917 0.4526
+vn 0.0154 0.8520 0.5233
+vn -0.3468 0.2945 -0.8905
+vn 0.9955 0.0753 0.0583
+vn -0.1387 0.0852 -0.9867
+vn 0.4972 -0.3278 -0.8033
+vn -0.0181 -0.9210 0.3892
+vn 0.6646 0.1263 -0.7364
+vn -0.1170 -0.3609 -0.9252
+vn -0.1672 -0.3215 -0.9320
+vn -0.6941 -0.5019 -0.5161
+s off
+f 12//1 5//1 8//1
+f 9//2 2//2 6//2
+f 1//3 10//3 11//3
+f 5//4 11//4 10//4
+f 5//5 12//5 7//5
+f 6//6 3//6 1//6
+f 11//7 6//7 1//7
+f 10//8 8//8 5//8
+f 6//9 7//9 4//9
+f 8//10 2//10 13//10
+f 6//11 2//11 3//11
+f 10//12 2//12 8//12
+f 10//13 1//13 2//13
+f 12//14 8//14 13//14
+f 3//15 2//15 1//15
+f 9//16 12//16 13//16
+f 6//17 4//17 9//17
+f 7//18 6//18 11//18
+f 9//19 13//19 2//19
+f 4//20 7//20 9//20
+f 7//21 12//21 9//21
+f 5//22 7//22 11//22
diff --git a/data/roms/HLleg_rom_reduced.obj b/data/roms/HLleg_rom_reduced.obj
new file mode 100644
index 0000000..ce49ea0
--- /dev/null
+++ b/data/roms/HLleg_rom_reduced.obj
@@ -0,0 +1,59 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o HLleg_rom_reduced_HLleg_rom
+v 0.101897 0.090971 -0.151882
+v -0.330315 0.203812 -0.080346
+v -0.282950 0.271873 -0.209370
+v -0.287854 0.144716 -0.268832
+v -0.322732 0.079715 -0.126905
+v -0.322687 0.038437 -0.218888
+v -0.206632 0.096805 -0.329127
+v -0.042802 0.119917 -0.290616
+v 0.031011 0.026783 -0.227149
+v -0.080595 0.283919 -0.244348
+v 0.087966 0.218043 -0.111174
+v -0.191755 -0.015344 -0.318965
+v -0.203790 0.279222 -0.264777
+vn 0.1424 0.3273 -0.9341
+vn -0.0187 -0.8907 0.4542
+vn -0.0485 0.9968 0.0630
+vn 0.0020 0.8973 0.4413
+vn 0.0297 0.8795 0.4749
+vn -0.5556 0.3697 -0.7447
+vn 0.0722 -0.3218 0.9440
+vn 0.2360 -0.0566 -0.9701
+vn 0.6854 0.3397 -0.6441
+vn 0.4101 -0.2689 -0.8715
+vn -0.9968 -0.0730 0.0323
+vn 0.6974 0.0665 -0.7136
+vn 0.1770 0.3048 -0.9358
+vn 0.6606 0.2937 -0.6909
+vn -0.9506 0.1723 -0.2581
+vn -0.4457 0.3040 -0.8420
+vn -0.6458 -0.1534 -0.7480
+vn -0.6380 -0.1462 -0.7561
+vn 0.0571 -0.7855 0.6162
+vn -0.9413 0.1721 -0.2903
+vn -0.0205 -0.9122 0.4093
+s off
+f 10//1 7//1 13//1
+f 12//2 9//2 6//2
+f 13//3 3//3 10//3
+f 11//4 10//4 2//4
+f 2//5 10//5 3//5
+f 4//6 3//6 13//6
+f 5//7 1//7 11//7 2//7
+f 12//8 7//8 8//8
+f 8//9 10//9 1//9
+f 9//10 12//10 8//10
+f 6//11 5//11 2//11
+f 9//12 8//12 1//12
+f 10//13 8//13 7//13
+f 1//14 10//14 11//14
+f 3//15 6//15 2//15
+f 13//16 7//16 4//16
+f 7//17 12//17 4//17
+f 12//18 6//18 4//18
+f 5//19 9//19 1//19
+f 6//20 3//20 4//20
+f 5//21 6//21 9//21
diff --git a/data/roms/HRleg_rom_reduced.obj b/data/roms/HRleg_rom_reduced.obj
new file mode 100644
index 0000000..ce2536d
--- /dev/null
+++ b/data/roms/HRleg_rom_reduced.obj
@@ -0,0 +1,61 @@
+# Blender v2.82 (sub 7) OBJ File: ''
+# www.blender.org
+o HRleg_rom_reduced_HRleg_rom
+v 0.094960 -0.206503 -0.084837
+v -0.307523 -0.180849 -0.042370
+v -0.313588 -0.257896 -0.182246
+v -0.043198 -0.275073 -0.229562
+v -0.324702 -0.083393 -0.127987
+v -0.301713 -0.015253 -0.251660
+v -0.194398 -0.294239 -0.249621
+v -0.202177 0.004749 -0.314965
+v 0.101140 -0.075972 -0.142672
+v 0.030571 -0.138010 -0.236972
+v -0.275168 -0.156914 -0.284248
+v -0.165385 -0.143030 -0.329407
+v -0.047928 -0.001685 -0.277958
+vn -0.0238 0.9634 0.2669
+vn -0.9913 -0.0923 0.0938
+vn 0.1213 0.3973 0.9096
+vn 0.0027 0.8778 0.4790
+vn 0.2366 0.1527 -0.9595
+vn 0.0143 0.6614 0.7499
+vn -0.3800 -0.0042 -0.9250
+vn 0.0055 -0.8760 0.4823
+vn 0.0014 0.8758 0.4828
+vn -0.0056 -0.8786 0.4776
+vn 0.0677 -0.9251 0.3737
+vn 0.7122 0.2086 -0.6702
+vn -0.9067 -0.0744 -0.4152
+vn 0.4270 -0.0259 -0.9039
+vn 0.8468 -0.2485 -0.4703
+vn -0.9827 -0.0049 -0.1854
+vn 0.4163 -0.2710 -0.8679
+vn -0.3055 -0.3978 -0.8651
+vn -0.5472 0.0889 -0.8323
+vn -0.5374 -0.4893 -0.6868
+vn 0.7462 -0.4291 -0.5090
+vn 0.1752 -0.4855 -0.8565
+s off
+f 6//1 13//1 8//1
+f 3//2 2//2 5//2
+f 1//3 9//3 2//3
+f 13//4 6//4 9//4
+f 12//5 8//5 13//5
+f 9//6 5//6 2//6
+f 12//7 11//7 8//7
+f 7//8 2//8 3//8
+f 5//9 9//9 6//9
+f 7//10 1//10 2//10
+f 7//11 4//11 1//11
+f 10//12 13//12 9//12
+f 11//13 3//13 6//13
+f 10//14 12//14 13//14
+f 1//15 10//15 9//15
+f 5//16 6//16 3//16
+f 4//17 12//17 10//17
+f 11//18 12//18 7//18
+f 11//19 6//19 8//19
+f 7//20 3//20 11//20
+f 4//21 10//21 1//21
+f 4//22 7//22 12//22
diff --git a/data/srdf/solo_LFleg_rom.srdf b/data/srdf/solo_LFleg_rom.srdf
new file mode 100644
index 0000000..bed0eda
--- /dev/null
+++ b/data/srdf/solo_LFleg_rom.srdf
@@ -0,0 +1,3 @@
+<robot name="solo_LFleg_rom">
+
+</robot>
diff --git a/data/srdf/solo_LHleg_rom.srdf b/data/srdf/solo_LHleg_rom.srdf
new file mode 100644
index 0000000..9bf27fb
--- /dev/null
+++ b/data/srdf/solo_LHleg_rom.srdf
@@ -0,0 +1,3 @@
+<robot name="solo_LHleg_rom">
+
+</robot>
diff --git a/data/srdf/solo_RFleg_rom.srdf b/data/srdf/solo_RFleg_rom.srdf
new file mode 100644
index 0000000..2d03988
--- /dev/null
+++ b/data/srdf/solo_RFleg_rom.srdf
@@ -0,0 +1,3 @@
+<robot name="solo_RFleg_rom">
+
+</robot>
diff --git a/data/srdf/solo_RHleg_rom.srdf b/data/srdf/solo_RHleg_rom.srdf
new file mode 100644
index 0000000..3bbcb16
--- /dev/null
+++ b/data/srdf/solo_RHleg_rom.srdf
@@ -0,0 +1,3 @@
+<robot name="solo_RHleg_rom">
+
+</robot>
diff --git a/data/srdf/solo_trunk.srdf b/data/srdf/solo_trunk.srdf
new file mode 100644
index 0000000..9a076eb
--- /dev/null
+++ b/data/srdf/solo_trunk.srdf
@@ -0,0 +1,3 @@
+<robot name="solo_trunk">
+
+</robot>
diff --git a/data/urdf/solo_LFleg_rom.urdf b/data/urdf/solo_LFleg_rom.urdf
new file mode 100644
index 0000000..43e80ef
--- /dev/null
+++ b/data/urdf/solo_LFleg_rom.urdf
@@ -0,0 +1,19 @@
+<robot name="solo_LFleg_rom">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://solo-rbprm/meshes/FLleg_rom_reduced.stl"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://solo-rbprm/meshes/FLleg_rom_reduced.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/data/urdf/solo_LHleg_rom.urdf b/data/urdf/solo_LHleg_rom.urdf
new file mode 100644
index 0000000..a893924
--- /dev/null
+++ b/data/urdf/solo_LHleg_rom.urdf
@@ -0,0 +1,19 @@
+<robot name="solo_LHleg_rom">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://solo-rbprm/meshes/HLleg_rom_reduced.stl"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://solo-rbprm/meshes/HLleg_rom_reduced.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/data/urdf/solo_RFleg_rom.urdf b/data/urdf/solo_RFleg_rom.urdf
new file mode 100644
index 0000000..405b9d9
--- /dev/null
+++ b/data/urdf/solo_RFleg_rom.urdf
@@ -0,0 +1,19 @@
+<robot name="solo_RFleg_rom">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://solo-rbprm/meshes/FRleg_rom_reduced.stl"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://solo-rbprm/meshes/FRleg_rom_reduced.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/data/urdf/solo_RHleg_rom.urdf b/data/urdf/solo_RHleg_rom.urdf
new file mode 100644
index 0000000..135b314
--- /dev/null
+++ b/data/urdf/solo_RHleg_rom.urdf
@@ -0,0 +1,19 @@
+<robot name="solo_RHleg_rom">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://solo-rbprm/meshes/HRleg_rom_reduced.stl"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://solo-rbprm/meshes/HRleg_rom_reduced.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/data/urdf/solo_trunk.urdf b/data/urdf/solo_trunk.urdf
new file mode 100644
index 0000000..493f555
--- /dev/null
+++ b/data/urdf/solo_trunk.urdf
@@ -0,0 +1,23 @@
+<robot name="solo_trunk">
+ <link name="base_link">
+ <inertial>
+ <mass value="30.47"/>
+ <inertia ixx="1." ixy="0.0" ixz="0.0" iyy="1." iyz="0.0" izz="1."/>
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://solo-rbprm/meshes/solo_trunk.stl"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 -0.05" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://solo-rbprm/meshes/solo_trunk.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/script/constants_and_tools.py b/script/constants_and_tools.py
new file mode 100644
index 0000000..65c4f26
--- /dev/null
+++ b/script/constants_and_tools.py
@@ -0,0 +1,200 @@
+import numpy as np
+#~ 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_centroidal_dynamics import *
+#~ from hpp_spline import *e
+from numpy import array, asmatrix, matrix, zeros, ones
+from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate
+from numpy.linalg import norm
+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
+from random import random as rd
+from random import randint as rdi
+from numpy import squeeze, asarray
+
+
+Id = matrix([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
+z = array([0.,0.,1.])
+zero3 = zeros(3)
+
+
+def generators(A,b, Aeq = None, beq = None):
+ m = np.hstack([b,-A])
+ matcdd = cdd.Matrix(m); matcdd.rep_type = cdd.RepType.INEQUALITY
+
+ if Aeq is not None:
+ meq = np.hstack([beq,-Aeq])
+ matcdd.extend(meq.tolist(), True)
+
+ H = cdd.Polyhedron(matcdd)
+ g = H.get_generators()
+
+ return [array(g[el][1:]) for el in range(g.row_size)], H
+
+def filter(pts):
+ hull = ConvexHull(pts, qhull_options='Q12')
+ return [pts[i] for i in hull.vertices.tolist()]
+
+def ineq(pts, canonicalize = False):
+ apts = array(pts)
+ m = np.hstack([ones((apts.shape[0],1)),apts])
+ matcdd = cdd.Matrix(m); matcdd.rep_type = cdd.RepType.GENERATOR
+ H = cdd.Polyhedron(matcdd)
+ bmA = H.get_inequalities()
+ if canonicalize:
+ bmA.canonicalize()
+ Ares = zeros((bmA.row_size,bmA.col_size-1))
+ bres = zeros(bmA.row_size )
+ for i in range(bmA.row_size):
+ l = array(bmA[i])
+ Ares[i,:] = -l[1:]
+ bres[i] = l[0]
+ return Ares, bres
+
+def ineqQHull(hull):
+ A = hull.equations[:,:-1]
+ b = -hull.equations[:,-1]
+ return A,b
+
+
+def canon(A,b):
+ m = np.hstack([b,-A])
+ matcdd = cdd.Matrix(m); matcdd.rep_type = 1
+ H = cdd.Polyhedron(matcdd)
+ bmA = H.get_inequalities()
+ #~ bmA.canonicalize()
+ Ares = zeros((bmA.row_size,bmA.col_size-1))
+ bres = zeros((bmA.row_size,1 ))
+ for i in range(bmA.row_size):
+ #~ print "line ", array(bmA[i])
+ #~ print "A ", A[i][:]
+ #~ print "b ", b[i]
+ l = array(bmA[i])
+ Ares[i,:] = -l[1:]
+ bres[i] = l[0]
+ #~ print "Ares ",Ares[i,:]
+ #~ print "bres ",bres[i]
+ return Ares, bres
+
+def genPolytope(A,b):
+ pts, H = generators(A,b)
+ apts = array(pts)
+ if len(apts) > 0:
+ hull = ConvexHull(apts)
+ return hull, pts, apts, H
+ return None, None, None, None
+
+def convex_hull_ineq(pts):
+ return None
+
+
+ m = cData.contactPhase_.getMass()
+ #~ #get 6D polytope
+ (H, h) = ineqFromCdata(cData)
+ #project to the space where aceleration is 0
+ D = zeros((6,3))
+ D[3:,:] = m * gX
+
+ d = zeros((6,))
+ d[:3] = -m * g
+
+ A = H.dot(D)
+ b = h.reshape((-1,)) - H.dot(d)
+ #add kinematic polytope
+ (K,k) = (cData.Kin_[0], cData.Kin_[1].reshape(-1,))
+
+ resA = vstack([A, K])
+ resb = concatenate([b, k]).reshape((-1,1))
+
+ #DEBUG
+ allpts = generators(resA,resb)[0]
+ 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())
+ 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()))
+ assert not error, str (len(allpts))
+
+ return (resA, resb)
+ #~ return (A, b)
+ #~ return (vstack([A, K]), None)
+
+def default_transform_from_pos_normal(pos, normal):
+ #~ print "pos ", pos
+ #~ print "normal ", normal
+ f = array([0.,0.,1.])
+ t = array(normal)
+ v = np.cross(f, t)
+ c = np.dot(f, t)
+ if c > 0.99:
+ rot = identity(3)
+ else:
+ u = v/norm(v)
+ h = (1. - c)/(1. - 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. ] ] )
+
+import os
+
+def continuous(h, initpts):
+ dic = {}
+ pts = []
+ for i, pt in enumerate(h.vertices.tolist()):
+ pts += [initpts[pt]]
+ dic[pt] = i
+ faces = []
+ for f in h.simplices:
+ faces += [[dic[idx]+1 for idx in f ]]
+ return pts, faces
+
+def hull_to_obj(h,pts,name):
+ pts, faces = continuous(h, pts)
+ f = open(name, "w")
+ #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')
+ for pt in faces:
+ #~ print "???"
+ 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(v,f,fname)
+#~ % v is a Nx3 matrix of vertex coordinates.
+#~ % f is a Mx3 matrix of vertex indices.
+#~ % fname is the filename to save the obj file.
+
+#~ fid = fopen(name,'w');
+
+#~ for i=1:size(v,1)
+#~ fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
+#~ end
+
+#~ fprintf(fid,'g foo\n');
+
+#~ for i=1:size(f,1);
+#~ fprintf(fid,'f %d %d %d\n',f(i,1),f(i,2),f(i,3));
+#~ end
+#~ fprintf(fid,'g\n');
+
+#~ fclose(fid);
diff --git a/script/gen_and_copy.sh b/script/gen_and_copy.sh
new file mode 100755
index 0000000..9036356
--- /dev/null
+++ b/script/gen_and_copy.sh
@@ -0,0 +1,10 @@
+#!/bin/bash
+
+cd $DEVEL_DIR/src/solo-rbprm/script;
+starthpp relativeFootPositionQuasiFlat.py
+starthpp generateROMS.py
+/local/pfernbac/blender-2.82a-linux64/blender --background --python reduce.py
+
+#for f in ./output/*reduced.obj; do mv "$f" "${f%.obj}.obj"; done
+#cp ./output/* $DEVEL_DIR/src/solo-rbprm/data/
+#cd -
diff --git a/script/generateROMs.py b/script/generateROMs.py
new file mode 100644
index 0000000..36d7384
--- /dev/null
+++ b/script/generateROMs.py
@@ -0,0 +1,54 @@
+from solo_rbprm.solo import Robot
+from hpp.gepetto import Viewer
+from hpp.corbaserver.problem_solver import ProblemSolver
+from numpy import array, ones, zeros
+
+
+fullBody = Robot ()
+
+nbSamples = 100000
+
+ps = ProblemSolver( fullBody )
+
+r = Viewer (ps)
+
+rootName = 'base_joint_xyz'
+
+
+q_0 = fullBody.referenceConfig
+
+r(q_0)
+fullBody.setJointBounds ("root_joint", [-20,20, -20, 20, -20, 20])
+fullBody.setConstrainedJointsBounds()
+dict_heuristic = {fullBody.rLegId:"static", fullBody.lLegId:"static", fullBody.rArmId:"fixedStep04", fullBody.lArmId:"fixedStep04"}
+fullBody.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=nbSamples)
+
+def getEffPosition(limbId, nbSamples):
+ positions = []
+ limit = nbSamples-1
+ for i in range(0,limit):
+ q = fullBody.getSamplePosition(limbId,i)
+ positions += [q[:3]]
+ return positions
+
+
+from scipy.spatial import ConvexHull
+from constants_and_tools import hull_to_obj
+from scipy.optimize import linprog
+from plot_polytopes import plot_hull
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+
+for limbId in fullBody.limbs_names:
+ positions = getEffPosition(limbId, nbSamples)
+ rom_hull = ConvexHull(positions)
+ hull_to_obj(rom_hull, positions, str(limbId)+"_rom.obj")
+ fig = plt.figure()
+ fig.suptitle(str(limbId)+"_rom.obj", fontsize=16)
+ plot_hull(rom_hull, positions, array(positions), color = "r", plot = False, fig = fig, ax = None)
+
+ fig = plt.figure()
+ fig.suptitle(str(limbId), fontsize=16)
+ ax = None
+ plt.show(block = False)
+
diff --git a/script/obj_to_constraints.py b/script/obj_to_constraints.py
new file mode 100644
index 0000000..f7d7997
--- /dev/null
+++ b/script/obj_to_constraints.py
@@ -0,0 +1,156 @@
+#do the loading of the obj file
+import numpy as np
+from collections import namedtuple
+ObjectData = namedtuple("ObjectData", "V T N F")
+Inequalities = namedtuple("Inequality", "A b N V")
+
+def toFloat(stringArray):
+ res= np.zeros(len(stringArray))
+ for i in range(0,len(stringArray)):
+ res[i] = float(stringArray[i])
+ return res
+
+def load_obj(filename) :
+ V = [] #vertex
+ T = [] #texcoords
+ N = [] #normals
+ F = [] #face indexies
+
+ fh = open(filename)
+ for line in fh :
+ if line[0] == '#' : continue
+
+ line = line.strip().split(' ')
+ if line[0] == 'v' : #vertex
+ V.append(toFloat(line[1:]))
+ elif line[0] == 'vt' : #tex-coord
+ T.append(line[1:])
+ elif line[0] == 'vn' : #normal vector
+ N.append(toFloat(line[1:]))
+ elif line[0] == 'f' : #face
+ face = line[1:]
+ for i in range(0, len(face)) :
+ 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])):
+ if j!=1:
+ face[i][j] = int(face[i][j]) - 1
+ F.append(face)
+
+ return ObjectData(V, T, N, F)
+
+def inequality(v, n):
+ #the plan has for equation ax + by + cz = d, with a b c coordinates of the normal
+ #inequality is then ax + by +cz -d <= 0
+ # last var is v because we need it
+ return [n[0], n[1], n[2], np.array(v).dot(np.array(n))]
+
+def as_inequalities(obj):
+ #for each face, find first three points and deduce plane
+ #inequality is given by normal
+ A= np.empty([len(obj.F), 3])
+ b = np.empty(len(obj.F))
+ V = np.ones([len(obj.F), 4])
+ N = np.empty([len(obj.F), 3])
+ for f in range(0, len(obj.F)):
+ face = obj.F[f]
+ v = obj.V[face[0][0]]
+ # assume normals are in obj
+ n = obj.N[face[0][2]]
+ ineq = inequality(v,n)
+ A[f,:] = ineq[0:3]
+ b[f] = ineq[3]
+ V[f,0:3] = v
+ N[f,:] = n
+ return Inequalities(A,b, N, V)
+
+def is_inside(inequalities, pt):
+ return ((inequalities.A.dot(pt) - inequalities.b) < 0).all()
+
+#~ def rotate_inequalities_q():
+
+# TODO this is naive, should be a way to simply update d
+def rotate_inequalities(ineq, transform):
+ #for each face, find first three points and deduce plane
+ #inequality is given by normal
+ A = np.empty([len(ineq.A), 3])
+ b = np.empty(len(ineq.b))
+ V = np.ones([len(ineq.V), 4])
+ N = np.ones([len(ineq.N), 3])
+ for i in range(0, len(b)):
+ v = transform.dot(ineq.V[i,:])
+ n = transform[0:3,0:3].dot(ineq.N[i,0:3])
+ ine = inequality(v[0:3],n[0:3])
+ A[i,:] = ine[0:3]
+ b[i] = ine[3]
+ V[i,:] = v
+ N[i,:] = n
+ return Inequalities(A,b, N, V)
+
+from pickle import dump
+def ineq_to_file(ineq, filename):
+ f1=open(filename, 'w+')
+ res = { 'A' : ineq.A, 'b' : ineq.b, 'N' : ineq.N, 'V' : ineq.V}
+ dump(res, f1)
+ f1.close()
+
+from pickle import load
+def ineq_from_file(filename):
+ f1=open(filename, 'r')
+ res = load(f1)
+ return Inequalities(res['A'], res['b'],res['N'],res['V'])
+
+def test_inequality():
+ n = np.array([0,-1,0])
+ v = np.array([0,1,1])
+ if inequality(v,n) != [0,-1,0,-1]:
+ print("error in test_inequality")
+ else:
+ print("test_inequality successful")
+
+def __gen_data():
+ 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]]
+ return obj, ineq, ok_points, not_ok_points
+
+def test_belonging():
+ data = __gen_data()
+ ineq = data[1]
+ 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"
+ for p in not_ok_points:
+ 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. ]])
+
+ 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"
+ for p in not_ok_points:
+ assert (not is_inside(ineq, p)), "point " + str(p) + " should NOT be inside object"
+ print("test_rotate_inequalities successful")
+
+
+def load_obj_and_save_ineq(in_name, out_name):
+ obj = load_obj(in_name)
+ ineq = as_inequalities(obj)
+ 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')
diff --git a/script/plot_polytopes.py b/script/plot_polytopes.py
new file mode 100644
index 0000000..114d6e3
--- /dev/null
+++ b/script/plot_polytopes.py
@@ -0,0 +1,66 @@
+import numpy as np
+from hpp_centroidal_dynamics import *
+from curves import *
+from numpy import array, asmatrix, matrix, zeros, ones
+from numpy import array, dot, vstack, hstack, asmatrix, identity, cross
+from numpy.linalg import norm
+
+from scipy.spatial import ConvexHull
+from hpp_bezier_com_traj import *
+#~ from qp import solve_lp
+
+import eigenpy
+# ~ import cdd
+from curves import bezier3
+from random import random as rd
+from random import randint as rdi
+from numpy import squeeze, asarray
+
+eigenpy.switchToNumpyArray()
+
+
+
+from constants_and_tools import *
+
+
+
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+
+
+def plot_hull_in_subplot(hull, pts, apts, ax, color = "r", just_pts = False):
+ # Plot defining corner points
+ ax.plot(apts.T[0], apts.T[1], apts.T[2], "ko")
+ if not just_pts:
+ for s in hull.simplices:
+ s = np.append(s, s[0]) # Here we cycle back to the first coordinate
+ 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):
+ if fig is None:
+ fig = plt.figure()
+ if ax is None:
+ print ("ax is none")
+ ax = fig.add_subplot(111, projection="3d")
+ plot_hull_in_subplot(hull, pts, array(pts), ax, color, just_pts)
+ if plot:
+ print (" PLOT" )
+ plt.show(block=False)
+ return ax
+
+def plot_polytope_H_rep(A_in,b_in, color = "r", just_pts = False):
+ hull, pts, apts, cd = genPolytope(A_in,b_in)
+ plot_hull(hull, pts, apts, color, just_pts)
+
+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')
+ plot_hull(hull, pts, apts, color, just_pts)
+
+def plot_polytope_CDD_PolyHeron(H, color = "r", just_pts = False):
+ g = H.get_generators()
+ pts = [array(g[el][1:]) for el in range(g.row_size)]
+ plot_polytope_V_rep(pts, color, just_pts)
+
diff --git a/script/reduce.py b/script/reduce.py
new file mode 100644
index 0000000..9267b7b
--- /dev/null
+++ b/script/reduce.py
@@ -0,0 +1,67 @@
+import bpy
+
+
+# this script is tested with blender 2.82
+# WARNING !! this script will erase your scene
+#
+
+#change those parameters according to your needs
+TARGET_NUM_FACES = 24.
+FOLDER_PATH = "/local/dev_hpp/src/solo-rbprm/script/"
+OUTPUT_PATH = FOLDER_PATH+"output/"
+
+EXTENSION = ".obj"
+
+def decimate(obj):
+ nFaces = len(obj.data.polygons)
+ heuristic_ratio = TARGET_NUM_FACES / float(nFaces)
+ bpy.ops.mesh.decimate(ratio=heuristic_ratio)
+ return
+
+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)
+
+ obj = bpy.data.objects[-1]
+
+ #api change in 2.82
+ #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.select_mode(type="VERT")
+ bpy.ops.mesh.select_all(action = 'SELECT')
+ bpy.ops.mesh.convex_hull()
+ decimate(obj)
+
+ #to export first extract filename
+ idx = file.index(EXTENSION)
+ obj.name = file[:idx] + "_reduced"
+ bpy.ops.export_scene.obj(filepath=OUTPUT_PATH+obj.name+EXTENSION, check_existing=True, filter_glob="*.obj;*.mtl",
+ use_selection=True, use_animation=False,
+ use_mesh_modifiers=True, use_edges=True,
+ use_smooth_groups=False, use_smooth_groups_bitflags=False,
+ use_normals=True, use_uvs=True, use_materials=False,
+ use_triangles=False, use_nurbs=False,
+ use_vertex_groups=False, use_blen_objects=True,
+ group_by_object=False, group_by_material=False, keep_vertex_order=False,
+ # ~ global_scale=1.0, path_mode='AUTO', axis_forward='X', axis_up='Z')
+ global_scale=1.0, path_mode='AUTO')
+
+ #delete all objects
+ bpy.ops.object.delete()
+
+
+bpy.ops.object.select_all(action='SELECT')
+bpy.ops.object.delete()
+
+import glob, os
+os.chdir(FOLDER_PATH)
+directory = os.path.dirname(OUTPUT_PATH)
+if not os.path.exists(directory):
+ os.makedirs(directory)
+
+#clear the scene !
+
+for file in glob.glob("*.obj"):
+ load_obj(file)
diff --git a/script/relativeFootPositionQuasiFlat.py b/script/relativeFootPositionQuasiFlat.py
new file mode 100644
index 0000000..79446fc
--- /dev/null
+++ b/script/relativeFootPositionQuasiFlat.py
@@ -0,0 +1,313 @@
+from hpp.gepetto import Viewer
+from hpp.gepetto import ViewerFactory
+from numpy import array, zeros, ones
+from numpy.linalg import norm
+
+from solo_rbprm.solo import Robot
+from hpp.corbaserver.rbprm.tools.display_tools import *
+
+#from plot_polytopes import *
+from pinocchio import Quaternion
+
+NUM_SAMPLES = 6000
+IT_DISPLAY_PROGRESS = NUM_SAMPLES / 10
+MIN_DIST_BETWEEN_FEET_Y = 0.02
+MIN_DIST_BETWEEN_FEET_X = 0.02
+MAX_DIST_BETWEEN_FEET_X = 0.2
+MAX_DIST_BETWEEN_FEET_Z = 0.2
+MIN_HEIGHT_COM = 0.13
+# 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.01
+
+
+fullBody = Robot ()
+
+
+
+fullBody.setConstrainedJointsBounds()
+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.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=12)
+
+#~ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.corbaserver import ProblemSolver
+nbSamples = 1
+
+ps = ProblemSolver( fullBody )
+vf = ViewerFactory (ps)
+v = vf.createViewer()
+rootName = 'root_joint'
+
+zero = [0.,0.,0.]
+rLegId = fullBody.rLegId
+rLeg = fullBody.rleg
+rfoot = fullBody.rfoot
+rLegOffset = fullBody.offset[:]
+lLegOffset = fullBody.offset[:]
+rArmOffset = fullBody.offset[:]
+lArmOffset = fullBody.offset[:]
+
+lLegId = fullBody.lLegId
+lLeg = fullBody.lleg
+lfoot = fullBody.lfoot
+
+#make sure this is 0
+q_0 = fullBody.getCurrentConfig ()
+zeroConf = [0,0,0, 0, 0, 0, 1.]
+q_0[0:7] = zeroConf
+fullBody.setCurrentConfig (q_0)
+
+effectors = [fullBody.rfoot, fullBody.lfoot, fullBody.rhand, fullBody.lhand,]
+limbIds = [fullBody.rLegId, fullBody.lLegId, fullBody.rArmId, fullBody.lArmId]
+offsets = [array(rLegOffset), array(lLegOffset), array(rArmOffset), array(lArmOffset)]
+
+import numpy as np
+
+compoints = [[] for _ in effectors]
+#~ compoints = [[[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]],[[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]]]
+points = [ {} for _ in effectors]
+for i, eff in enumerate(effectors):
+ for j, otherEff in enumerate(effectors):
+ if i != j:
+ points[i][otherEff] = []
+
+
+success = 0
+fails = 0
+from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper
+from scipy.spatial import ConvexHull
+from constants_and_tools import hull_to_obj
+from scipy.optimize import linprog
+
+#static eq is com is convex combination of pos (projected)
+def staticEq(positions, com):
+ sizeX = len(positions)
+ E = zeros((3,sizeX))
+ for i, pos in enumerate(positions):
+ E[:2,i] = pos[:2]
+ e = array([com[0], com[1], 1.])
+ E[2,:] = ones(sizeX)
+ try:
+ 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']
+ except:
+ return False
+
+
+#returns true of one of the point is inside the convex hulls of the others. We do not want that
+def pointInsideHull(positions):
+ for i, pos in enumerate(positions):
+ others = positions[:i] + positions[i+1:]
+ if staticEq(others, pos):
+ return True
+ return False
+
+def genFlat(init = False):
+ q = fullBody.shootRandomConfig()
+ if init:
+ q = fullBody.referenceConfig[::]
+ q[0:7] = zeroConf
+ fullBody.setCurrentConfig(q)
+ #~ v(q)
+
+ positions = [fullBody.getJointPosition(foot)[:3] for foot in effectors]
+
+ s = State(fullBody, q = q, limbsIncontact = limbIds)
+ succ = True
+ for effId, pos in zip(limbIds,positions):
+ s, succ = StateHelper.addNewContact(s, effId, pos, [0.,0.,1.], num_max_sample = 0)
+ if not succ:
+ break
+
+ # ~ posrf = fullBody.getJointPosition(rfoot)[:3]
+ # ~ poslf = fullBody.getJointPosition(lfoot)[:3]
+ # ~ print ("limbsIds ", limbIds)
+ # ~ s = State(fullBody, q = q, limbsIncontact = limbIds)
+ # ~ s, succ = StateHelper.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample = 0)
+ # ~ if succ:
+ # ~ s, succ = StateHelper.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]
+
+ #assert that in static equilibrium
+ if succ:
+ fullBody.setCurrentConfig(q)
+ succ = staticEq(positions, fullBody.getCenterOfMass())
+ if not succ:
+ v(q)
+ if succ:
+ succ = not pointInsideHull(positions)
+ if not succ:
+ print ("************* contacts crossing", not succ)
+ v(q)
+ #~ if succ and norm (array(posrf[:2]) - array(poslf[:2]) ) <= 0.1:
+ # ~ if succ and norm (array(posrf) - array(poslf) ) <= 0.1:
+ v(s.q())
+ return s.q(), succ, s, positions
+
+
+def printFootPositionRelativeToOther(nbConfigs):
+ for i in range(0, nbConfigs):
+ if i > 0 and not i % IT_DISPLAY_PROGRESS:
+ print(int((i * 100) / nbConfigs), " % done")
+ q, succ, s, pos = genFlat(i==0)
+ if succ:
+ global success
+ success += 1
+ addCom = True
+ for j, effectorName in enumerate(effectors):
+ 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]]
+ # ~ fullBody.setCurrentConfig(qtr)
+ # ~ qEffector = fullBody.getJointPosition(effectorName)
+
+ # check current joint pos is now zero
+ # ~ 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))
+ # ~ for k in range(0, 3):
+ # ~ for l in range(0, 3):
+ # ~ rm[k, l] = rot[k, l]
+ # ~ for m in range(0, 3):
+ # ~ rm[m, 3] = qEffector[m]
+ # ~ 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
+ points[j][oeffectorName].append(p[:3])
+ else:
+ addCom = False
+ else:
+ addCom = False
+ else:
+ 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):
+ # ~ 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):
+ # ~ points[j].append(p[:3])
+ # ~ else:
+ # ~ addCom =
+ # now compute coms
+
+ fullBody.setCurrentConfig(q)
+ com = array(fullBody.getCenterOfMass())
+ 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])
+ # ~ 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))
+ # ~ for k in range(0, 3):
+ # ~ for l in range(0, 3):
+ # ~ rm[k, l] = rot[k, l]
+ # ~ for m in range(0, 3):
+ # ~ rm[m, 3] = qEffector[m]
+ # ~ rm[3, 3] = 1
+ # ~ invrm = np.linalg.inv(rm)
+ # ~ p = invrm.dot([0, 0, 0, 1])
+ # ~ # add offset
+ # ~ rp = array(p[:3] - offsets[j]).tolist()
+
+ if (rp[2] < MIN_HEIGHT_COM):
+ addCom = False
+ print ("reject min heught")
+ if addCom:
+ compoints[j].append(rp)
+ # ~ if j == 1:
+ # ~ if rp[1] < MARGIN_FEET_SIDE:
+ # ~ compoints[j].append(rp)
+ # ~ else:
+ # ~ if rp[1] > -MARGIN_FEET_SIDE:
+ # ~ compoints[j].append(rp)
+
+
+ else:
+ global fails
+ fails += 1
+ # print(fullBody.isConfigValid(q)[1])
+ # for j in range(0,len(limbIds)):
+ # f1=open('./'+str(limbIds[j])+'_com.erom', 'w+')
+ # for p in points[j]:
+ # f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n")
+ # f1.close()
+
+
+s = State(fullBody, q = fullBody.referenceConfig, limbsIncontact = [fullBody.limbs_names[0]])
+v(s.q())
+
+#~ printRootPosition(rLegId, rfoot, nbSamples)
+#~ printRootPosition(lLegId, lfoot, nbSamples)
+#~ printRootPosition(rarmId, rHand, nbSamples)
+#~ printRootPosition(larmId, lHand, nbSamples)
+printFootPositionRelativeToOther(6000)
+print ("successes ", success )
+print ("fails ", fails )
+
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from plot_polytopes import plot_hull
+
+# ~ 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,limbIds, compoints, points):
+ #comData_array = np.array(comData)
+ #print("com data shape : ", comData_array.shape)
+ hcom = ConvexHull(comData)
+ hull_to_obj(hcom,comData,"COM_constraints_in_"+str(limbId)+"_effector_frame_quasi_static.obj")
+ fig = plt.figure()
+ fig.suptitle("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()
+ fig.suptitle(str(limbId), fontsize=16)
+ # ~ axes = [221,222,223,224]
+ ax = None
+ # ~ for (oEffector, pts), axId in zip(pointsData.items(), axes):
+ for (oEffector, pts) in pointsData.items():
+ # ~ ax = fig.add_subplot(axId, projection="3d")
+ hpts = ConvexHull(pts)
+ hull_to_obj(hpts,pts,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("effector ", limbId, )
+ print("oEffector ", oEffector, )
+ plt.show(block = False)
+
+
+# ~ hcomRF = ConvexHull(compoints[0])
+# ~ hcomLF = ConvexHull(compoints[1])
+# ~ hull_to_obj(hcomRF,compoints[0],"anymal_COM_constraints_in_RF_effector_frame.obj")
+# ~ hull_to_obj(hcomLF,compoints[1],"anymal_COM_constraints_in_LF_effector_frame.obj")
+
+# ~ hptsRF = ConvexHull(points[0])
+# ~ hptsLF = ConvexHull(points[1])
+# ~ hull_to_obj(hptsRF,points[0],"anymal_LF_constraints_in_RF.obj")
+# ~ hull_to_obj(hptsLF,points[1],"anymal_RF_constraints_in_LF.obj")
+
+
+# ~ for k in range(2):
+ # ~ hcom = ConvexHull(compoints[k])
+ # ~ plot_hull(hcom, compoints[k], array(compoints[k]))
+
+ # ~ hpts = ConvexHull(points[k])
+ # ~ plot_hull(hpts, points[k], array(points[k]), color = "b", plot = k == 1 and True)
diff --git a/script/run.sh b/script/run.sh
new file mode 100755
index 0000000..c75111a
--- /dev/null
+++ b/script/run.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+
+gepetto-gui &
+hpp-rbprm-server &
+ipython3 -i --no-confirm-exit ./$1
+
+pkill -f 'gepetto-gui'
+pkill -f 'hpp-rbprm-server'
diff --git a/solo_rbprm/__init__.py b/solo_rbprm/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/solo_rbprm/solo.py b/solo_rbprm/solo.py
new file mode 100644
index 0000000..8bcc356
--- /dev/null
+++ b/solo_rbprm/solo.py
@@ -0,0 +1,212 @@
+#!/usr/bin/env python
+# Copyright (c) 2020 CNRS
+# Author: Pierre Fernbach
+#
+# This file is part of solo-rbprm.
+# solo-rbprm is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation, either version
+# 3 of the License, or (at your option) any later version.
+#
+# hpp_tutorial is distributed in the hope that it will be
+# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+# General Lesser Public License for more details. You should have
+# received a copy of the GNU Lesser General Public License along with
+# hpp_tutorial. If not, see
+# <http://www.gnu.org/licenses/>.
+
+
+
+import os
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody as Parent
+from pinocchio import SE3, Quaternion
+import numpy as np
+
+
+class Robot (Parent):
+ ##
+ # Information to retrieve urdf and srdf files.
+ name = "solo"
+ packageName = "solo_description"
+ meshPackageName = "solo_description"
+ rootJointType = "freeflyer"
+ urdfName = "solo"
+ urdfSuffix = "12"
+ srdfSuffix = ""
+
+ ## Information about the names of thes joints defining the limbs of the robot
+ rLegId = 'FRleg'
+ rleg = 'FR_HAA'
+ rfoot = 'FR_FOOT'
+ lLegId = 'FLleg'
+ lleg = 'FL_HAA'
+ lfoot = 'FL_FOOT'
+ lArmId = 'HLleg'
+ larm = 'HL_HAA'
+ lhand = 'HL_FOOT'
+ rArmId = 'HRleg'
+ rarm = 'HR_HAA'
+ rhand = 'HR_FOOT'
+
+ referenceConfig = [0., 0., 0.235, 0., 0., 0., 1., # freeflyer
+ 0., 0.8, -1.6, # FL
+ 0., 0.8, -1.6, # FR
+ 0., -0.8, 1.6, # HL
+ 0., -0.8, 1.6] # HR
+
+ DEFAULT_COM_HEIGHT = 0.211
+
+ # informations required to generate the limbs databases the limbs :
+ nbSamples = 50000 # Number of sampled configuration for each limb in the database
+ octreeSize = 0.002 # Resolution of the octree leaf (as a cube, in meter)
+ cType = "_3_DOF" # 6_dof constraints the orientation of the contacts, 3_dof only constraint the position
+ offset = [0.,0.,-0.005] # Offset applied to the contact position wrt to the frame position
+
+ # Position on the first joint of each limb (in the root frame) in the 'reference' configration,
+ # Used by some heuristic when sorting candidates contacts positions
+ rLegLimbOffset = [0.1946, 0.0875, 0.]
+ lLegLimbOffset = [0.1946, -0.0875,0.]
+ rArmLimbOffset = [-0.1946, 0.0875, 0.]
+ lArmLimbOffset = [-0.1946, -0.0875, 0.]
+
+ normal = [0,0,1] # Contact normal, in the effector frame
+ legx = 0.01; legy = 0.01 # Contact patch size, cannot be 0 even for contact points
+
+ kinematicConstraintsPath="package://solo-rbprm/com_inequalities/"
+ # Path to constraints files used by SL1M:
+ kinematic_constraints_path = os.environ["INSTALL_HPP_DIR"] + "/share/solo-rbprm/com_inequalities/feet_quasi_flat/"
+ relative_feet_constraints_path = os.environ["INSTALL_HPP_DIR"] + "/share/solo-rbprm/relative_effector_positions/"
+ minDist = 0.2 # Minimal height of the CoM wrt to the contact height
+
+ # Data used by mlp
+ limbs_names = [rArmId,rLegId,lArmId,lLegId] # List of effector used to create contact
+ 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]}
+ FOOT_SAFETY_SIZE = 0.01
+ # size of the contact surface (x,y)
+ dict_size={rfoot:[legx , legy], lfoot:[legx , legy],rhand:[legx , legy],lhand:[legx , legy]}
+ #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_normal = {rfoot:normal, lfoot:normal, rhand:normal, lhand:normal}
+ # Effector position in the reference configuration, in the root frame
+ ref_EE_lLeg = np.array([0.1946, 0.14695, -0.223])
+ ref_EE_rLeg = np.array([0.1946, -0.14695, -0.223])
+ ref_EE_lArm = np.array([-0.1946, 0.14695, -0.223])
+ ref_EE_rArm = np.array([-0.1946, -0.14695, -0.223])
+ dict_ref_effector_from_root = {rLegId:ref_EE_rLeg,
+ lLegId:ref_EE_lLeg,
+ rArmId:ref_EE_rArm,
+ lArmId:ref_EE_lArm}
+ # 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}
+
+ kneeIds = {"LF":9,"LH":12,"RF":15,"HR":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)
+ # save original bounds of the urdf for futur reset
+ self.FL_HAA_bounds = self.getJointBounds('FL_HAA')
+ self.FL_HFE_bounds = self.getJointBounds('FL_HFE')
+ self.FL_KFE_bounds = self.getJointBounds('FL_KFE')
+
+ self.FR_HAA_bounds = self.getJointBounds('FR_HAA')
+ self.FR_HFE_bounds = self.getJointBounds('FR_HFE')
+ self.FR_KFE_bounds = self.getJointBounds('FR_KFE')
+
+ self.HL_HAA_bounds = self.getJointBounds('HL_HAA')
+ self.HL_HFE_bounds = self.getJointBounds('HL_HFE')
+ self.HL_KFE_bounds = self.getJointBounds('HL_KFE')
+
+ self.HR_HAA_bounds = self.getJointBounds('HR_HAA')
+ self.HR_HFE_bounds = self.getJointBounds('HR_HFE')
+ self.HR_KFE_bounds = self.getJointBounds('HR_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")
+ 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)
+ if analysis :
+ self.runLimbSampleAnalysis(id, analysis, True)
+
+
+ def setConstrainedJointsBounds(self):
+ self.setJointBounds('FL_HAA',[-0.5,0.5])
+ self.setJointBounds('FL_HFE',[0.2,1.4])
+ self.setJointBounds('FL_KFE',[-2.3,-0.4])
+
+ self.setJointBounds('FR_HAA',[-0.5,0.5])
+ self.setJointBounds('FR_HFE',[0.2,1.4])
+ self.setJointBounds('FR_KFE',[-2.3,-0.4])
+
+ self.setJointBounds('HL_HAA',[-0.5,0.5])
+ self.setJointBounds('HL_HFE',[-1.4,-0.2])
+ self.setJointBounds('HL_KFE',[0.4,2.3])
+
+ self.setJointBounds('HR_HAA',[-0.5,0.5])
+ self.setJointBounds('HR_HFE',[-1.4,-0.2])
+ self.setJointBounds('HR_KFE',[0.4,2.3])
+
+ def setConstrainedShoulder(self, max_amplitude = 0.05):
+ self.setJointBounds('FL_HAA',[-max_amplitude,max_amplitude])
+ self.setJointBounds('FR_HAA',[-max_amplitude,max_amplitude])
+ self.setJointBounds('HL_HAA',[-max_amplitude,max_amplitude])
+ self.setJointBounds('HR_HAA',[-max_amplitude,max_amplitude])
+
+
+ def resetJointsBounds(self):
+ self.setJointBounds('FL_HAA',self.FL_HAA_bounds)
+ self.setJointBounds('FL_HFE',self.FL_HFE_bounds)
+ self.setJointBounds('FL_KFE',self.FL_KFE_bounds)
+
+ self.setJointBounds('FR_HAA',self.FR_HAA_bounds)
+ self.setJointBounds('FR_HFE',self.FR_HFE_bounds)
+ self.setJointBounds('FR_KFE',self.FR_KFE_bounds)
+
+ self.setJointBounds('HL_HAA',self.HL_HAA_bounds)
+ self.setJointBounds('HL_HFE',self.HL_HFE_bounds)
+ self.setJointBounds('HL_KFE',self.HL_KFE_bounds)
+
+ self.setJointBounds('HR_HAA',self.HR_HAA_bounds)
+ self.setJointBounds('HR_HFE',self.HR_HFE_bounds)
+ self.setJointBounds('HR_KFE',self.HR_KFE_bounds)
+
diff --git a/solo_rbprm/solo_abstract.py b/solo_rbprm/solo_abstract.py
new file mode 100644
index 0000000..21e1f78
--- /dev/null
+++ b/solo_rbprm/solo_abstract.py
@@ -0,0 +1,61 @@
+#!/usr/bin/env python
+# Copyright (c) 2020 CNRS
+# Author: Pierre Fernbach
+#
+# This file is part of solo-rbprm.
+# solo-rbprm is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation, either version
+# 3 of the License, or (at your option) any later version.
+#
+# hpp_tutorial is distributed in the hope that it will be
+# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+# General Lesser Public License for more details. You should have
+# received a copy of the GNU Lesser General Public License along with
+# hpp_tutorial. If not, see
+# <http://www.gnu.org/licenses/>.
+
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder as Parent
+
+
+class Robot (Parent):
+
+ ##
+ # Information to retrieve urdf and srdf files.
+ rootJointType = 'freeflyer'
+ packageName = 'solo-rbprm'
+ meshPackageName = 'solo-rbprm'
+ # URDF file describing the trunk of the robot solo
+ urdfName = 'solo_trunk'
+ # URDF files describing the reachable workspace of each limb of solo
+ urdfNameRom = ['solo_RFleg_rom','solo_LHleg_rom','solo_LFleg_rom','solo_RHleg_rom']
+ urdfSuffix = ""
+ srdfSuffix = ""
+ name = urdfName
+
+ ref_height = 0.465
+
+ rLegId = 'solo_RFleg_rom'
+ lLegId = 'solo_LFleg_rom'
+ rArmId = 'solo_RHleg_rom'
+ lArmId = 'solo_LHleg_rom'
+
+ # Effector position in the reference configuration, in the root frame
+ ref_EE_lLeg =[0.1946, 0.14695, -0.223]
+ ref_EE_rLeg = [0.1946, -0.14695, -0.223]
+ ref_EE_lArm = [-0.1946, 0.14695, -0.223]
+ ref_EE_rArm = [-0.1946, -0.14695, -0.223]
+ 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('solo_LFleg_rom',self.ref_EE_lLeg)
+ self.setReferenceEndEffector('solo_RFleg_rom',self.ref_EE_rLeg)
+ self.setReferenceEndEffector('solo_LHleg_rom',self.ref_EE_lArm)
+ self.setReferenceEndEffector('solo_RHleg_rom',self.ref_EE_rArm)
--
GitLab