Unverified Commit 008d63dd authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #16 from pFernbach/devel

Update relative to SL1M API
parents 114e7284 b049042b
Pipeline #15034 passed with stage
in 1 minute and 4 seconds
Subproject commit 3a52692a40839b10f38352c1b06ccfebc0b53f36
Subproject commit 094834088ad9de32f1abdc2d56d29bca2190772c
......@@ -6,54 +6,54 @@ v 0.309961 0.800311 0.006626
v -0.042532 0.728959 -0.277092
v -0.204739 0.785187 -0.194369
v 0.163816 0.900633 0.274600
v -0.326193 0.293722 -0.450179
v -0.290104 0.190606 -0.475631
v -0.323395 0.201466 -0.372144
v -0.318240 0.296456 0.500298
v -0.326193 0.303722 -0.450179
v -0.290104 0.200606 -0.475631
v -0.323395 0.211466 -0.372144
v -0.318240 0.306456 0.500298
v -0.323902 0.873649 0.563283
v -0.325758 0.185694 0.251725
v -0.325758 0.195694 0.251725
v -0.296636 0.639755 -0.382398
v -0.276093 0.440533 -0.522028
v 0.170417 0.368876 -0.564590
v -0.046029 0.344797 -0.559881
v 0.302679 0.226204 -0.313885
v 0.316714 0.222618 -0.245047
v 0.321054 0.184809 0.036949
v 0.259631 0.241383 -0.495101
v 0.302679 0.236204 -0.313885
v 0.316714 0.232618 -0.245047
v 0.321054 0.194809 0.036949
v 0.259631 0.251383 -0.495101
v 0.322192 0.412583 -0.387172
v 0.327981 0.188394 0.097360
v 0.327981 0.198394 0.097360
v 0.323458 0.825942 0.104246
v 0.216139 0.884025 0.328076
v 0.099770 0.877321 0.471264
v 0.114758 0.794634 0.610727
v 0.302292 0.711536 0.539728
v -0.313732 0.182866 -0.057566
v -0.313732 0.192866 -0.057566
v 0.099285 0.647921 0.617583
v -0.145435 0.558858 0.614339
v 0.276530 0.223744 0.567556
v -0.272490 0.188464 0.392388
v 0.276530 0.233744 0.567556
v -0.272490 0.198464 0.392388
vn 0.1013 0.9386 -0.3299
vn -0.9274 -0.2576 -0.2713
vn -0.9992 -0.0138 0.0364
vn -0.9267 0.1469 -0.3460
vn -0.9992 -0.0140 0.0365
vn -0.9230 0.1525 -0.3533
vn -0.0500 0.2610 -0.9640
vn 0.3956 -0.9095 -0.1280
vn 0.0320 -0.4610 -0.8868
vn 0.9976 -0.0576 -0.0386
vn 0.0354 -0.4887 -0.8717
vn 0.9974 -0.0611 -0.0389
vn 0.1803 -0.9757 -0.1246
vn 0.9999 0.0072 -0.0086
vn 0.9999 0.0073 -0.0087
vn 0.4870 0.7673 0.4173
vn 0.0487 -0.9964 -0.0694
vn -0.0769 -0.2322 0.9696
vn -0.0766 -0.2388 0.9680
vn 0.8872 -0.4553 -0.0747
vn -0.9991 0.0374 -0.0191
vn -0.9991 0.0381 -0.0192
vn -0.7402 -0.6056 0.2922
vn 0.9968 -0.0174 0.0784
vn 0.0153 -0.4980 -0.8671
vn 0.8792 -0.0212 -0.4759
vn 0.9617 -0.1815 -0.2055
vn 0.9968 -0.0177 0.0784
vn 0.0187 -0.5278 -0.8492
vn 0.8792 -0.0226 -0.4758
vn 0.9597 -0.1916 -0.2056
vn -0.0137 -0.9711 0.2385
vn -0.4356 -0.1019 0.8944
vn -0.4381 -0.1035 0.8930
vn 0.0545 0.6203 -0.7825
vn 0.1010 0.8605 0.4993
vn 0.0182 0.9922 0.1235
......@@ -66,9 +66,9 @@ vn 0.9557 0.2241 -0.1910
vn 0.3634 0.9292 -0.0669
vn 0.7557 0.6235 0.2005
vn 0.1554 0.9500 -0.2709
vn 0.3669 0.0337 0.9297
vn -0.3359 -0.1537 -0.9293
vn 0.3001 0.0129 0.9538
vn 0.3671 0.0343 0.9295
vn -0.3396 -0.1594 -0.9270
vn 0.2996 0.0130 0.9540
vn -0.4217 -0.9058 -0.0406
vn 0.1437 0.9762 0.1625
vn -0.0750 0.0980 0.9924
......
......@@ -2,60 +2,60 @@
# www.blender.org
mtllib talos_RF_constraints_in_LF_quasi_flat_REDUCED.mtl
o foo
v -0.315717 -0.189447 -0.075045
v -0.315717 -0.199447 -0.075045
v -0.316097 -0.477041 -0.457372
v -0.318636 -0.258711 0.500236
v -0.318636 -0.268711 0.500236
v -0.060798 -0.389734 -0.673594
v -0.104032 -0.585113 -0.573917
v -0.175744 -0.538260 -0.608216
v -0.255646 -0.354604 -0.581284
v 0.278307 -0.256619 0.437545
v 0.283981 -0.185612 0.387452
v -0.225588 -0.184966 0.500152
v 0.157596 -0.248020 -0.581029
v 0.278307 -0.266619 0.437545
v 0.283981 -0.195612 0.387452
v -0.225588 -0.194966 0.500152
v 0.157596 -0.258020 -0.581029
v 0.221555 -0.573481 -0.408854
v 0.318993 -0.286385 0.384315
v 0.324409 -0.195788 0.061247
v 0.318993 -0.296385 0.384315
v 0.324409 -0.205788 0.061247
v -0.310169 -0.346221 -0.509742
v -0.294903 -0.205896 -0.378054
v 0.196852 -0.180381 -0.489950
v 0.292010 -0.181846 -0.317769
v -0.294903 -0.215896 -0.378054
v 0.196852 -0.190381 -0.489950
v 0.292010 -0.191846 -0.317769
v 0.304342 -0.576528 -0.284838
v 0.299100 -0.832487 0.310836
v 0.315113 -0.259115 -0.378870
v 0.315113 -0.269115 -0.378870
v -0.273470 -0.850701 0.210493
v -0.313807 -0.838394 0.222548
v 0.317610 -0.762291 0.513917
v -0.311802 -0.618950 0.538435
v 0.299116 -0.250973 -0.458221
v 0.299116 -0.260973 -0.458221
v -0.007460 -0.760086 -0.268260
v -0.231538 -0.917320 0.371109
v 0.150214 -0.888616 0.173444
v 0.074152 -0.912322 0.346961
v -0.285990 -0.840699 0.459188
vn -1.0000 0.0070 -0.0042
vn -1.0000 0.0072 -0.0042
vn -0.2683 -0.2065 -0.9409
vn 0.1800 0.5574 0.8105
vn 0.4816 -0.3904 -0.7846
vn 0.4762 -0.3980 -0.7841
vn 0.9403 0.3234 0.1064
vn -0.7665 -0.2288 -0.6001
vn -0.1873 0.7915 -0.5818
vn -0.1784 0.8057 -0.5649
vn 0.3087 0.9511 0.0086
vn 0.4828 -0.8061 -0.3421
vn 0.9398 0.3348 -0.0680
vn -0.9991 0.0351 -0.0254
vn -0.9991 0.0354 -0.0247
vn -0.6098 -0.7076 -0.3570
vn 0.9998 0.0018 0.0173
vn 0.8238 0.1467 0.5476
vn -0.0812 0.1036 0.9913
vn 0.8730 -0.2182 -0.4361
vn 0.6292 -0.2633 -0.7313
vn 0.8241 0.1495 0.5463
vn -0.0835 0.1064 0.9908
vn 0.8715 -0.2250 -0.4357
vn 0.6276 -0.2711 -0.7298
vn -0.6185 0.7805 0.0908
vn 0.8319 0.2740 0.4826
vn -0.0650 0.9962 -0.0585
vn -0.9808 0.1791 -0.0771
vn -0.6184 0.5725 -0.5384
vn -0.1857 0.7218 -0.6668
vn -0.9781 0.1931 -0.0777
vn -0.5990 0.6028 -0.5271
vn -0.1730 0.7420 -0.6476
vn 0.7023 0.6522 -0.2854
vn -0.0075 1.0000 0.0014
vn -0.2527 -0.9153 -0.3137
......@@ -66,8 +66,8 @@ vn 0.0615 -0.7853 0.6160
vn 0.0056 1.0000 0.0054
vn 0.0359 -0.7429 0.6684
vn -0.9813 -0.1549 0.1138
vn -0.9997 -0.0170 0.0182
vn -1.0000 -0.0079 -0.0008
vn -0.9997 -0.0175 0.0185
vn -1.0000 -0.0080 -0.0009
vn -0.6872 -0.7264 -0.0054
vn -0.3749 -0.8503 -0.3693
vn -0.3798 -0.8495 -0.3663
......@@ -78,9 +78,9 @@ vn 0.5840 -0.7417 -0.3299
vn 0.3605 -0.8912 0.2752
vn -0.0409 -0.3405 0.9394
vn 0.9907 -0.1277 -0.0462
vn 0.9988 -0.0417 -0.0263
vn 0.9995 -0.0259 -0.0174
vn 0.0979 0.1157 0.9884
vn 0.9987 -0.0431 -0.0266
vn 0.9995 -0.0263 -0.0173
vn 0.0984 0.1182 0.9881
usemtl None
s 1
f 1//1 2//1 3//1
......
......@@ -3,10 +3,14 @@
# Author: Pierre Fernbach
import numpy as np
import os
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody as Parent
from pinocchio import SE3
from pathlib import Path
def prefix(module):
"""$prefix/lib/pythonX.Y/site-packages/$module/__init__.py: extract prefix from module"""
return Path(module.__file__).parent.parent.parent.parent.parent
class Robot(Parent):
# Information to retrieve urdf and srdf files.
......@@ -40,7 +44,7 @@ class Robot(Parent):
referenceConfig = [
0.0,
0.0,
1.0225,
1.01977,
0.0,
0.0,
0.0,
......@@ -82,7 +86,7 @@ class Robot(Parent):
referenceConfig_elbowsUp = [
0.0,
0.0,
1.0225,
1.01977,
0.0,
0.0,
0.0,
......@@ -124,7 +128,7 @@ class Robot(Parent):
referenceConfig_legsApart = [
0.0,
0.0,
1.0225,
1.01977,
0.0,
0.0,
0.0,
......@@ -166,7 +170,7 @@ class Robot(Parent):
referenceConfig_armsFront = [
0.0,
0.0,
1.0225,
1.01977,
0,
0.0,
0.0,
......@@ -358,12 +362,20 @@ class Robot(Parent):
lArmx = 0.005
lArmy = 0.005
# Paths to constraints files:
kinematicConstraintsPath = "package://talos-rbprm/com_inequalities/"
rLegKinematicConstraints = kinematicConstraintsPath + rleg + "_com_constraints.obj"
lLegKinematicConstraints = kinematicConstraintsPath + lleg + "_com_constraints.obj"
rArmKinematicConstraints = kinematicConstraintsPath + rarm + "_com_constraints.obj"
lArmKinematicConstraints = kinematicConstraintsPath + larm + "_com_constraints.obj"
minDist = 0.4
# Constraints used by SL1M:
filekin_right = prefix(talos_rbprm) / "share/talos-rbprm/com_inequalities/feet_quasi_flat/COM_constraints_in_RF_effector_frame_REDUCED.obj"
filekin_left = prefix(talos_rbprm) / "share/talos-rbprm/com_inequalities/feet_quasi_flat/COM_constraints_in_LF_effector_frame_REDUCED.obj"
file_rf_in_lf = prefix(talos_rbprm) / "share/talos-rbprm/relative_effector_positions/RF_constraints_in_LF_quasi_flat_REDUCED.obj"
file_lf_in_rf = prefix(talos_rbprm) / "share/talos-rbprm/relative_effector_positions/LF_constraints_in_RF_quasi_flat_REDUCED.obj"
kinematic_constraints_path = prefix(talos_rbprm) / "share/talos-rbprm/com_inequalities/feet_quasi_flat/"
relative_feet_constraints_path = prefix(talos_rbprm) / "share/talos-rbprm/relative_effector_positions/"
# data used by scripts :
limbs_names = [rLegId, lLegId, rArmId, lArmId]
dict_limb_rootJoint = {rLegId: rleg, lLegId: lleg, rArmId: rarm, lArmId: larm}
......@@ -376,16 +388,23 @@ class Robot(Parent):
# various offset used by scripts :
MRsole_offset = SE3.Identity()
MRsole_offset.translation = np.matrix(rLegOffset).T
MRsole_offset.translation = np.array(rLegOffset)
MLsole_offset = SE3.Identity()
MLsole_offset.translation = np.matrix(lLegOffset).T
MLsole_offset.translation = np.array(lLegOffset)
MRhand_offset = SE3.Identity()
MRhand_offset.translation = np.matrix(rArmOffset).T
MRhand_offset.translation = np.array(rArmOffset)
MLhand_offset = SE3.Identity()
MLhand_offset.translation = np.matrix(lArmOffset).T
MLhand_offset.translation = np.array(lArmOffset)
dict_offset = {rfoot: MRsole_offset, lfoot: MLsole_offset, rhand: MRhand_offset, lhand: MLhand_offset}
dict_normal = {rfoot: rLegNormal, lfoot: lLegNormal, rhand: rArmNormal, lhand: lArmNormal}
ref_EE_lLeg = np.array([0, 0.0848172440888579, -1.019272022956703])
ref_EE_rLeg = np.array([0, -0.0848172440888579, -1.019272022956703])
ref_EE_lArm = np.array([0.13028765672452458, 0.44360498616312666, -0.2881211563246389])
ref_EE_rArm = np.array([0.13028765672452458, -0.44360498616312666, -0.2881211563246389])
dict_ref_effector_from_root = {rLegId:ref_EE_rLeg, # Effector position in the reference configuration, in the root frame
lLegId:ref_EE_lLeg,
rArmId:ref_EE_rArm,
lArmId:ref_EE_lArm}
# display transform :
# MRsole_display = MRsole_offset.copy()
......@@ -417,7 +436,7 @@ class Robot(Parent):
self.joint6R_bounds_prev = self.getJointBounds('leg_right_6_joint')
self.joint2R_bounds_prev = self.getJointBounds('leg_right_2_joint')
def loadAllLimbs(self, heuristic, analysis=None, nbSamples=nbSamples, octreeSize=octreeSize):
def loadAllLimbs(self, heuristic, analysis=None, nbSamples=nbSamples, octreeSize=octreeSize, disableEffectorCollision=False):
for id in self.limbs_names:
eff = self.dict_limb_joint[id]
self.addLimb(id,
......@@ -433,7 +452,8 @@ class Robot(Parent):
self.cType,
kinematicConstraintsPath=self.kinematicConstraintsPath + self.dict_limb_rootJoint[id] +
"_com_constraints.obj",
kinematicConstraintsMin=self.minDist)
kinematicConstraintsMin=self.minDist,
disableEffectorCollision=disableEffectorCollision)
if analysis:
self.runLimbSampleAnalysis(id, analysis, True)
......
......@@ -25,15 +25,18 @@ class Robot(Parent):
legX = 0.1
legY = 0.06
ref_height = 1.0225
ref_height = 1.01977
# reference position of the end effector position for each ROM
ref_EE_lLeg = [-0.008846952891378526, 0.0848172440888579, -1.0225]
ref_EE_lLeg = [-0.008846952891378526, 0.0848172440888579, -1.01977]
ref_EE_lLeg[0] = 0. # assure symetry of dynamic constraints on flat ground
ref_EE_rLeg = [-0.008846952891378526, -0.0848172440888579, -1.0225]
ref_EE_rLeg = [-0.008846952891378526, -0.0848172440888579, -1.01977]
ref_EE_rLeg[0] = 0.
ref_EE_lArm = [0.13028765672452458, 0.44360498616312666, -0.2881211563246389]
ref_EE_rArm = [0.13028765672452458, -0.44360498616312666, -0.2881211563246389]
dict_ref_effector_from_root = {rLegId:ref_EE_rLeg, # Effector position in the reference configuration, in the root frame
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:
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment