Unverified Commit 932640db authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #4 from stonneau/devel

Devel
parents 1e5be76a 08d2948b
Pipeline #16202 passed with stage
in 38 seconds
......@@ -19,16 +19,21 @@
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody as Parent
from pinocchio import SE3, Quaternion
import numpy as np
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.
name = "anymal"
packageName = "anymal_data"
meshPackageName = "anymal_data"
packageName = "example-robot-data/robots/anymal_b_simple_description"
meshPackageName = "example-robot-data/robots/anymal_b_simple_description"
rootJointType = "freeflyer"
urdfName = "anymal"
urdfSuffix = "_small_collision_feet"
urdfSuffix = ""
#urdfSuffix="_ORI"
srdfSuffix = ""
......@@ -90,15 +95,20 @@ class Robot (Parent):
cType = "_3_DOF"
offset = [0.,0.,-0.005] # was 0.005
rLegLimbOffset = [0.373, 0.264, 0.]
lLegLimbOffset = [0.373, -0.264,0.]
rArmLimbOffset = [-0.373, 0.264, 0.]
lArmLimbOffset = [-0.373, -0.264, 0.]
rLegLimbOffset = [0.373, -0.264, -0.448]
lLegLimbOffset = [0.373, 0.264,-0.448]
rArmLimbOffset = [-0.373, -0.264, -0.448]
lArmLimbOffset = [-0.373, 0.264, -0.448]
normal = [0,0,1]
legx = 0.02; legy = 0.02
kinematicConstraintsPath="package://anymal-rbprm/com_inequalities/"
import anymal_rbprm
kinematic_constraints_path = str(prefix(anymal_rbprm) / "share/anymal-rbprm/com_inequalities/feet_quasi_flat/anymal_")
relative_feet_constraints_path = str(prefix(anymal_rbprm) / "share/anymal-rbprm/relative_effector_positions/anymal_")
minDist = 0.2
dict_ref_effector_from_root = {rLegId:rLegLimbOffset, lLegId:lLegLimbOffset, rArmId:rArmLimbOffset, lArmId:lArmLimbOffset}
# data used by scripts :,,,
#limbs_names = [rArmId,lLegId,lArmId,rLegId] # reverse default order to try to remove contacts at the beginning of the contact plan
......
......@@ -35,6 +35,13 @@ class Robot (Parent):
ref_height = 0.465
# TODO
rLegId = 'anymal_RFleg_rom'
lLegId = 'anymal_LFleg_rom'
rArmId = 'anymal_RHleg_rom'
lArmId = 'anymal_LHleg_rom'
ref_EE_lLeg =[0.373, 0.264, -0.448]
ref_EE_rLeg = [0.373, -0.264, -0.448]
ref_EE_lArm = [-0.373, 0.264, -0.448]
......@@ -43,6 +50,11 @@ class Robot (Parent):
#ref_EE_rLeg = [0.3, -0.165 , -0.44]
#ref_EE_lArm = [-0.3, 0.165 , -0.44]
#ref_EE_rArm = [-0.3, -0.165 , -0.44]
dict_ref_effector_from_root = {rLegId:ref_EE_rLeg,
lLegId:ref_EE_lLeg,
rArmId:ref_EE_rArm,
lArmId:ref_EE_lArm}
def __init__(self, name=None, load=True, client=None, clientRbprm=None):
if name is not None:
......
# Blender v2.79 (sub 0) OBJ File: ''
# www.blender.org
o anymal_COM_constraints_in_LFleg_effector_frame_quasi_static.000
v -0.112253 0.229012 0.548757
v -0.242575 -0.017352 0.652694
v 0.037196 -0.150480 0.541696
v -0.786524 -0.032667 0.242410
v 0.074540 0.121345 0.468167
v 0.247888 -0.369828 0.320184
v -0.621373 -0.593873 0.244774
v -0.631777 -0.151785 0.507676
v -0.122686 -0.561291 0.464687
v -0.329887 -0.684072 0.360032
v 0.108413 -0.606330 0.230614
v -0.571541 0.206401 0.498955
v -0.349583 -0.535586 0.511327
v -0.367727 -0.248774 0.615472
vn 0.4258 0.1511 0.8921
vn 0.1807 0.3416 -0.9223
vn -0.8805 -0.2574 0.3981
vn 0.4711 0.1694 0.8656
vn 0.2815 -0.8518 0.4419
vn -0.3736 0.0853 0.9236
vn -0.4431 -0.6680 0.5979
vn 0.0499 0.6525 -0.7561
vn -0.8226 0.1517 0.5480
vn -0.1182 0.4384 0.8910
vn 0.0592 0.7051 -0.7066
vn 0.2419 -0.2689 0.9323
vn -0.5860 -0.4243 0.6903
vn -0.0196 -0.0100 -0.9998
vn 0.4892 -0.3409 0.8028
vn -0.0251 -0.8171 -0.5759
vn 0.2269 -0.2731 0.9348
vn 0.1543 -0.3286 0.9318
vn -0.3629 0.0465 0.9307
vn 0.1584 0.2666 -0.9507
vn 0.5353 -0.5564 0.6355
vn 0.7572 0.0716 0.6493
vn -0.4580 -0.3289 0.8259
vn 0.0643 -0.7080 0.7033
s 1
f 1//1 2//1 3//1
f 4//2 5//2 6//2
f 7//3 8//3 4//3
f 1//4 3//4 5//4
f 9//5 10//5 11//5
f 8//6 2//6 12//6
f 10//7 13//7 7//7
f 1//8 5//8 12//8
f 4//9 8//9 12//9
f 12//10 2//10 1//10
f 4//11 12//11 5//11
f 9//12 3//12 2//12
f 8//13 7//13 13//13
f 11//14 7//14 4//14
f 6//15 3//15 9//15
f 11//16 10//16 7//16
f 9//17 2//17 14//17
f 13//18 9//18 14//18
f 8//19 14//19 2//19
f 11//20 4//20 6//20
f 9//21 11//21 6//21
f 6//22 5//22 3//22
f 13//23 14//23 8//23
f 10//24 9//24 13//24
# Blender v2.79 (sub 0) OBJ File: ''
# www.blender.org
o anymal_COM_constraints_in_LHleg_effector_frame_quasi_static_red
v -0.144949 -0.598410 0.235430
v -0.204657 -0.360342 0.367913
v -0.243073 -0.034617 0.380573
v 0.678971 -0.428919 0.253884
v 0.679043 0.023227 0.338045
v 0.144216 -0.002342 0.611740
v 0.409746 0.186441 0.529248
v 0.015804 0.198781 0.521639
v -0.033457 -0.216911 0.538812
v 0.478680 -0.657006 0.292545
v 0.178314 -0.668441 0.413171
v 0.524994 -0.378630 0.481118
v 0.556038 0.035167 0.505368
v 0.313398 -0.186966 0.602859
vn -0.9633 -0.1040 -0.2473
vn -0.0156 0.1830 -0.9830
vn -0.0049 0.4062 0.9138
vn -0.4712 0.0921 0.8772
vn 0.0365 0.7849 -0.6185
vn -0.0669 -0.9638 -0.2580
vn 0.4377 -0.0853 0.8951
vn -0.0597 0.2391 -0.9692
vn 0.3255 -0.5668 0.7568
vn 0.8119 -0.1069 0.5739
vn -0.5434 0.0988 0.8337
vn -0.4961 -0.5139 0.6998
vn 0.2764 0.1167 0.9539
vn 0.7989 -0.0947 0.5940
vn -0.6564 -0.1064 0.7469
vn -0.1442 -0.3275 0.9338
vn 0.2024 0.1389 0.9694
vn 0.1573 -0.3999 0.9030
vn -0.4358 -0.4251 0.7933
vn 0.6750 0.5810 0.4548
vn -0.0736 0.5744 -0.8153
vn -0.1618 -0.1948 0.9674
vn 0.6670 -0.4907 0.5606
vn 0.0679 -0.2244 -0.9721
s 1
f 1//1 2//1 3//1
f 4//2 1//2 5//2
f 6//3 7//3 8//3
f 8//4 9//4 6//4
f 5//5 8//5 7//5
f 10//6 11//6 1//6
f 12//7 13//7 14//7
f 5//8 1//8 3//8
f 12//9 11//9 10//9
f 4//10 5//10 12//10
f 9//11 8//11 3//11
f 1//12 11//12 2//12
f 14//13 13//13 7//13
f 12//14 5//14 13//14
f 9//15 3//15 2//15
f 11//16 14//16 9//16
f 14//17 7//17 6//17
f 12//18 14//18 11//18
f 2//19 11//19 9//19
f 13//20 5//20 7//20
f 3//21 8//21 5//21
f 9//22 14//22 6//22
f 10//23 4//23 12//23
f 4//24 10//24 1//24
# Blender v2.79 (sub 0) OBJ File: ''
# www.blender.org
o anymal_COM_constraints_in_RFleg_effector_frame_quasi_static_red
v -0.708207 0.112251 0.394356
v -0.435213 0.204652 0.622306
v -0.561954 0.621084 0.346834
v -0.619029 0.561871 0.247282
v -0.745337 0.287197 0.238623
v 0.193794 -0.064310 0.446920
v -0.727515 -0.040907 0.280840
v 0.252900 0.458251 0.268038
v 0.152133 0.300993 0.469989
v 0.027558 0.667570 0.315130
v -0.151250 0.109629 0.643488
v -0.180620 -0.241292 0.593868
v -0.176723 0.538777 0.500221
v -0.519770 -0.178641 0.537358
vn -0.6692 0.2574 0.6971
vn -0.8784 0.3955 0.2684
vn 0.1588 -0.3358 -0.9284
vn 0.8704 0.0685 0.4876
vn 0.5804 0.4763 0.6605
vn -0.0945 0.8785 -0.4684
vn 0.0261 0.0195 -0.9995
vn 0.4258 -0.1615 0.8903
vn 0.0556 0.2758 -0.9596
vn 0.0359 0.3184 0.9473
vn -0.0370 0.8389 0.5430
vn 0.4115 0.4698 0.7810
vn -0.1168 -0.1295 0.9847
vn 0.0986 -0.7509 -0.6530
vn -0.2314 0.4868 0.8423
vn 0.0506 -0.1248 -0.9909
vn -0.8140 -0.2761 0.5110
vn -0.8130 0.2813 0.5098
vn -0.1927 -0.1716 0.9661
vn -0.1000 -0.9081 -0.4066
vn 0.4956 0.0017 0.8685
vn 0.3119 0.3175 0.8955
vn -0.9788 -0.0270 0.2030
vn -0.6344 -0.0312 0.7724
s 1
f 1//1 2//1 3//1
f 3//2 4//2 5//2
f 6//3 7//3 8//3
f 9//4 6//4 8//4
f 9//5 8//5 10//5
f 4//6 3//6 10//6
f 5//7 4//7 8//7
f 11//8 12//8 6//8
f 8//9 4//9 10//9
f 11//10 13//10 2//10
f 3//11 13//11 10//11
f 9//12 10//12 13//12
f 12//13 11//13 2//13
f 12//14 7//14 6//14
f 2//15 13//15 3//15
f 7//16 5//16 8//16
f 7//17 14//17 1//17
f 1//18 3//18 5//18
f 14//19 12//19 2//19
f 14//20 7//20 12//20
f 11//21 6//21 9//21
f 11//22 9//22 13//22
f 1//23 5//23 7//23
f 14//24 2//24 1//24
# Blender v2.79 (sub 0) OBJ File: ''
# www.blender.org
o anymal_COM_constraints_in_RHleg_effector_frame_quasi_static_red
v 0.081476 0.676873 0.347654
v 0.249408 0.411231 0.578270
v 0.418617 0.649977 0.299790
v 0.043638 0.335388 0.565325
v -0.207435 0.567709 0.229938
v -0.244168 0.135925 0.392615
v -0.024435 -0.176507 0.519371
v 0.627132 0.414601 0.357305
v 0.666132 -0.039350 0.307122
v 0.620540 0.472678 0.241095
v 0.109712 -0.219722 0.549810
v 0.328442 -0.053091 0.608139
v 0.505740 -0.148221 0.468804
v 0.484568 0.063958 0.562682
vn 0.1547 0.7016 0.6955
vn -0.4826 0.5082 0.7133
vn -0.5097 -0.0094 0.8603
vn 0.4187 0.5479 0.7242
vn 0.9961 0.0870 -0.0130
vn -0.0013 -0.1280 -0.9918
vn -0.0721 -0.7142 -0.6962
vn 0.2630 -0.6076 0.7495
vn 0.0549 0.3696 -0.9276
vn 0.8164 0.0063 0.5775
vn 0.6773 0.6728 0.2978
vn -0.1967 -0.3970 -0.8965
vn 0.4758 -0.3158 0.8210
vn -0.0295 0.7680 -0.6397
vn -0.6401 0.3180 0.6994
vn -0.2258 -0.0541 0.9727
vn 0.0456 -0.8490 -0.5264
vn -0.0811 0.0502 0.9954
vn 0.2096 0.0983 0.9728
vn 0.0343 -0.3549 -0.9343
vn -0.2496 0.5400 0.8038
vn 0.4815 0.2889 0.8275
vn 0.7594 -0.1984 0.6197
s 1
f 1//1 2//1 3//1
f 4//2 1//2 5//2
f 6//3 7//3 4//3
f 8//4 3//4 2//4
f 8//5 9//5 10//5
f 10//6 9//6 5//6
f 11//7 7//7 9//7
f 12//8 11//8 13//8
f 3//9 10//9 5//9
f 14//10 9//10 8//10
f 3//11 8//11 10//11
f 9//12 7//12 5//12
f 13//13 14//13 12//13
f 3//14 5//14 1//14
f 5//15 6//15 4//15
f 7//16 11//16 12//16 4//16
f 13//17 11//17 9//17
f 4//18 12//18 2//18
f 14//19 2//19 12//19
f 7//20 6//20 5//20
f 4//21 2//21 1//21
f 2//22 14//22 8//22
f 13//23 9//23 14//23
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