Commit 8886fb32 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

flake8

parent 1a9c6905
Pipeline #16473 passed with stage
in 26 seconds
......@@ -17,15 +17,17 @@
# <http://www.gnu.org/licenses/>.
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody as Parent
from pinocchio import SE3, Quaternion
from pinocchio import SE3
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):
class Robot(Parent):
##
# Information to retrieve urdf and srdf files.
name = "anymal"
......@@ -34,10 +36,10 @@ class Robot (Parent):
rootJointType = "freeflyer"
urdfName = "anymal"
urdfSuffix = ""
#urdfSuffix="_ORI"
# urdfSuffix="_ORI"
srdfSuffix = ""
## Information about the names of thes joints defining the limbs of the robot
# Information about the names of thes joints defining the limbs of the robot
rLegId = 'RFleg'
rleg = 'RF_HAA'
rfoot = 'RF_ADAPTER_TO_FOOT'
......@@ -51,14 +53,27 @@ class Robot (Parent):
rarm = 'RH_HAA'
rhand = 'RH_ADAPTER_TO_FOOT'
referenceConfig_asymetric =[0.,0.,0.461, 0.,0.,0.,1., # FF
0.0, 0.611, -1.0452,
0.0, -0.853, 1.0847,
-0.0, 0.74, -1.08,
-0.0, -0.74, 1.08,
]
referenceConfig_asymetric = [
0.,
0.,
0.461,
0.,
0.,
0.,
1., # FF
0.0,
0.611,
-1.0452,
0.0,
-0.853,
1.0847,
-0.0,
0.74,
-1.08,
-0.0,
-0.74,
1.08,
]
"""
referenceConfig=[0,0,0.448,0,0,0,1,
0.079,0.78,-1.1,
......@@ -66,7 +81,7 @@ class Robot (Parent):
-0.079,0.78,-1.1,
-0.079,-0.78,1.1
]
"""
"""
"""
referenceConfig=[0,0,0.448,0,0,0,1,
0.095,0.76,-1.074,
......@@ -75,69 +90,90 @@ class Robot (Parent):
-0.095,-0.76,1.074
]
"""
referenceConfig=[0,0,0.47,0,0,0,1,
-0.12,0.724,-1.082,
-0.12,-0.724,1.082,
0.12,0.724,-1.082,
0.12,-0.724,1.082
]
postureWeights=[0,0,0,0,0,0, #FF
100.,1.,20.,
100.,1.,20.,
100.,1.,20.,
100.,1.,20.,]
referenceConfig = [
0, 0, 0.47, 0, 0, 0, 1, -0.12, 0.724, -1.082, -0.12, -0.724, 1.082, 0.12, 0.724, -1.082, 0.12, -0.724, 1.082
]
postureWeights = [
0,
0,
0,
0,
0,
0, # FF
100.,
1.,
20.,
100.,
1.,
20.,
100.,
1.,
20.,
100.,
1.,
20.,
]
DEFAULT_COM_HEIGHT = 0.445
# informations required to generate the limbs databases the limbs :
# informations required to generate the limbs databases the limbs :
nbSamples = 50000
octreeSize = 0.002
cType = "_3_DOF"
offset = [0.,0.,-0.005] # was 0.005
offset = [0., 0., -0.005] # was 0.005
rLegLimbOffset = [0.373, -0.264, -0.448]
lLegLimbOffset = [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
normal = [0, 0, 1]
legx = 0.02
legy = 0.02
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_")
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}
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
#limbs_names = [lLegId,rArmId,rLegId,lArmId] # default order to try to remove contacts at the beginning of the contact plan
limbs_names = [rArmId,rLegId,lArmId,lLegId]
dict_limb_rootJoint = {rLegId:rleg, lLegId:lleg, rArmId:rarm, lArmId:larm}
dict_limb_joint = {rLegId:rfoot, lLegId:lfoot, rArmId:rhand, lArmId:lhand}
dict_limb_color_traj = {rfoot:[0,1,0,1], lfoot:[1,0,0,1],rhand:[0,0,1,1],lhand:[0.9,0.5,0,1]}
# limbs_names = [rArmId,lLegId,lArmId,rLegId]
# reverse default order to try to remove contacts at the beginning of the contact plan
# limbs_names = [lLegId,rArmId,rLegId,lArmId]
# default order to try to remove contacts at the beginning of the contact plan
limbs_names = [rArmId, rLegId, lArmId, lLegId]
dict_limb_rootJoint = {rLegId: rleg, lLegId: lleg, rArmId: rarm, lArmId: larm}
dict_limb_joint = {rLegId: rfoot, lLegId: lfoot, rArmId: rhand, lArmId: lhand}
dict_limb_color_traj = {rfoot: [0, 1, 0, 1], lfoot: [1, 0, 0, 1], rhand: [0, 0, 1, 1], lhand: [0.9, 0.5, 0, 1]}
FOOT_SAFETY_SIZE = 0.01
# size of the contact surface (x,y)
dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
#dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
#various offset used by scripts
dict_size = {rfoot: [0.01, 0.01], lfoot: [0.01, 0.01], rhand: [0.01, 0.01], lhand: [0.01, 0.01]}
# dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
# various offset used by scripts
MRsole_offset = SE3.Identity()
MRsole_offset.translation = np.matrix(offset).T
MLsole_offset = MRsole_offset.copy()
MRhand_offset = MRsole_offset.copy()
MLhand_offset = MRsole_offset.copy()
dict_offset = {rfoot:MRsole_offset, lfoot:MLsole_offset, rhand:MRhand_offset, lhand:MLhand_offset}
dict_limb_offset= {rLegId:rLegLimbOffset, lLegId:lLegLimbOffset, rArmId:rArmLimbOffset, lArmId:lArmLimbOffset}
dict_normal = {rfoot:normal, lfoot:normal, rhand:normal, lhand:normal}
dict_offset = {rfoot: MRsole_offset, lfoot: MLsole_offset, rhand: MRhand_offset, lhand: MLhand_offset}
dict_limb_offset = {rLegId: rLegLimbOffset, lLegId: lLegLimbOffset, rArmId: rArmLimbOffset, lArmId: lArmLimbOffset}
dict_normal = {rfoot: normal, lfoot: normal, rhand: normal, lhand: normal}
# display transform :
MRsole_display = SE3.Identity()
MLsole_display = SE3.Identity()
MRhand_display = SE3.Identity()
MLhand_display = SE3.Identity()
dict_display_offset = {rfoot:MRsole_display, lfoot:MLsole_display, rhand:MRhand_display, lhand:MLhand_display}
dict_display_offset = {rfoot: MRsole_display, lfoot: MLsole_display, rhand: MRhand_display, lhand: MLhand_display}
kneeIds = {"LF":9,"LH":12,"RF":15,"RH":18}
kneeIds = {"LF": 9, "LH": 12, "RF": 15, "RH": 18}
def __init__(self, name=None, load=True, client=None, clientRbprm=None):
if name is not None:
......@@ -160,108 +196,109 @@ class Robot (Parent):
self.RH_HFE_bounds = self.getJointBounds('RH_HFE')
self.RH_KFE_bounds = self.getJointBounds('RH_KFE')
def loadAllLimbs(self,heuristic, analysis = None, nbSamples = nbSamples, octreeSize = octreeSize,disableEffectorCollision = False):
if isinstance(heuristic,str):#only one heuristic name given assign it to all the limbs
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 :
dict_heuristic.update({id: heuristic})
elif isinstance(heuristic, dict):
dict_heuristic = heuristic
else:
raise Exception("heuristic should be either a string or a map limbId:string")
#dict_heuristic = {self.rLegId:"static", self.lLegId:"static", self.rArmId:"fixedStep04", self.lArmId:"fixedStep04"}
# dict_heuristic = {self.rLegId:"static", self.lLegId:"static", self.rArmId:"fixedStep04",
# self.lArmId:"fixedStep04"}
for id in self.limbs_names:
print("add limb : ",id)
print("add limb : ", id)
eff = self.dict_limb_joint[id]
print("effector name = ",eff)
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.,
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",
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 :
if analysis:
self.runLimbSampleAnalysis(id, analysis, True)
def setSlightlyConstrainedJointsBounds(self):
self.setJointBounds('LF_HAA',[-1.,1.])
self.setJointBounds('LF_HFE',[-0.25,2.35])
self.setJointBounds('LF_KFE',[-2.35,0.])
self.setJointBounds('LF_HAA', [-1., 1.])
self.setJointBounds('LF_HFE', [-0.25, 2.35])
self.setJointBounds('LF_KFE', [-2.35, 0.])
self.setJointBounds('RF_HAA',[-1.,1.])
self.setJointBounds('RF_HFE',[-0.4,2.35])
self.setJointBounds('RF_KFE',[-2.35,0.])
self.setJointBounds('RF_HAA', [-1., 1.])
self.setJointBounds('RF_HFE', [-0.4, 2.35])
self.setJointBounds('RF_KFE', [-2.35, 0.])
self.setJointBounds('LH_HAA',[-1.,1.])
self.setJointBounds('LH_HFE',[-2.35,0.4])
self.setJointBounds('LH_KFE',[0.,2.35])
self.setJointBounds('RH_HAA',[-1.,1.])
self.setJointBounds('RH_HFE',[-2.35,0.25])
self.setJointBounds('RH_KFE',[0.,2.35])
self.setJointBounds('LH_HAA', [-1., 1.])
self.setJointBounds('LH_HFE', [-2.35, 0.4])
self.setJointBounds('LH_KFE', [0., 2.35])
self.setJointBounds('RH_HAA', [-1., 1.])
self.setJointBounds('RH_HFE', [-2.35, 0.25])
self.setJointBounds('RH_KFE', [0., 2.35])
def setConstrainedJointsBounds(self):
self.setJointBounds('LF_HAA',[-0.6,0.6])
self.setJointBounds('LF_HFE',[0.25,1.])
self.setJointBounds('LF_KFE',[-2.35,0.])
self.setJointBounds('RF_HAA',[-0.6,0.6])
self.setJointBounds('RF_HFE',[0.25,1.])
self.setJointBounds('RF_KFE',[-2.35,0.])
self.setJointBounds('LF_HAA', [-0.6, 0.6])
self.setJointBounds('LF_HFE', [0.25, 1.])
self.setJointBounds('LF_KFE', [-2.35, 0.])
self.setJointBounds('LH_HAA',[-0.6,0.6])
self.setJointBounds('LH_HFE',[-1.05,-0.45])
self.setJointBounds('LH_KFE',[0.,2.35])
self.setJointBounds('RF_HAA', [-0.6, 0.6])
self.setJointBounds('RF_HFE', [0.25, 1.])
self.setJointBounds('RF_KFE', [-2.35, 0.])
self.setJointBounds('RH_HAA',[-0.6,0.6])
self.setJointBounds('RH_HFE',[-1.05,-0.45])
self.setJointBounds('RH_KFE',[0.,2.35])
self.setJointBounds('LH_HAA', [-0.6, 0.6])
self.setJointBounds('LH_HFE', [-1.05, -0.45])
self.setJointBounds('LH_KFE', [0., 2.35])
self.setJointBounds('RH_HAA', [-0.6, 0.6])
self.setJointBounds('RH_HFE', [-1.05, -0.45])
self.setJointBounds('RH_KFE', [0., 2.35])
def setVeryConstrainedJointsBounds(self):
self.setJointBounds('LF_HAA',[-0.35,0.05])
self.setJointBounds('LF_HFE',[0.3,0.95])
self.setJointBounds('LF_KFE',[-2.35,0.])
self.setJointBounds('RF_HAA',[-0.05,0.35])
self.setJointBounds('RF_HFE',[0.3,0.95])
self.setJointBounds('RF_KFE',[-2.35,0.])
self.setJointBounds('LF_HAA', [-0.35, 0.05])
self.setJointBounds('LF_HFE', [0.3, 0.95])
self.setJointBounds('LF_KFE', [-2.35, 0.])
self.setJointBounds('LH_HAA',[-0.35,0.05])
self.setJointBounds('LH_HFE',[-1.,-0.5])
self.setJointBounds('LH_KFE',[0.,2.35])
self.setJointBounds('RF_HAA', [-0.05, 0.35])
self.setJointBounds('RF_HFE', [0.3, 0.95])
self.setJointBounds('RF_KFE', [-2.35, 0.])
self.setJointBounds('RH_HAA',[-0.05,0.35])
self.setJointBounds('RH_HFE',[-1.,-0.5])
self.setJointBounds('RH_KFE',[0.,2.35])
self.setJointBounds('LH_HAA', [-0.35, 0.05])
self.setJointBounds('LH_HFE', [-1., -0.5])
self.setJointBounds('LH_KFE', [0., 2.35])
self.setJointBounds('RH_HAA', [-0.05, 0.35])
self.setJointBounds('RH_HFE', [-1., -0.5])
self.setJointBounds('RH_KFE', [0., 2.35])
def resetJointsBounds(self):
self.setJointBounds('LF_HAA',self.LF_HAA_bounds)
self.setJointBounds('LF_HFE',self.LF_HFE_bounds)
self.setJointBounds('LF_KFE',self.LF_KFE_bounds)
self.setJointBounds('RF_HAA',self.RF_HAA_bounds)
self.setJointBounds('RF_HFE',self.RF_HFE_bounds)
self.setJointBounds('RF_KFE',self.RF_KFE_bounds)
self.setJointBounds('LH_HAA',self.LH_HAA_bounds)
self.setJointBounds('LH_HFE',self.LH_HFE_bounds)
self.setJointBounds('LH_KFE',self.LH_KFE_bounds)
self.setJointBounds('LF_HAA', self.LF_HAA_bounds)
self.setJointBounds('LF_HFE', self.LF_HFE_bounds)
self.setJointBounds('LF_KFE', self.LF_KFE_bounds)
self.setJointBounds('RH_HAA',self.RH_HAA_bounds)
self.setJointBounds('RH_HFE',self.RH_HFE_bounds)
self.setJointBounds('RH_KFE',self.RH_KFE_bounds)
self.setJointBounds('RF_HAA', self.RF_HAA_bounds)
self.setJointBounds('RF_HFE', self.RF_HFE_bounds)
self.setJointBounds('RF_KFE', self.RF_KFE_bounds)
self.setJointBounds('LH_HAA', self.LH_HAA_bounds)
self.setJointBounds('LH_HFE', self.LH_HFE_bounds)
self.setJointBounds('LH_KFE', self.LH_KFE_bounds)
self.setJointBounds('RH_HAA', self.RH_HAA_bounds)
self.setJointBounds('RH_HFE', self.RH_HFE_bounds)
self.setJointBounds('RH_KFE', self.RH_KFE_bounds)
......@@ -18,8 +18,8 @@
from hpp.corbaserver.rbprm.rbprmbuilder import Builder as Parent
class Robot (Parent):
##
class Robot(Parent):
# Information to retrieve urdf and srdf files.
rootJointType = 'freeflyer'
packageName = 'anymal-rbprm'
......@@ -27,7 +27,7 @@ class Robot (Parent):
# URDF file describing the trunk of the robot HyQ
urdfName = 'anymal_trunk'
# URDF files describing the reachable workspace of each limb of HyQ
urdfNameRom = ['anymal_RFleg_rom','anymal_LHleg_rom','anymal_LFleg_rom','anymal_RHleg_rom']
urdfNameRom = ['anymal_RFleg_rom', 'anymal_LHleg_rom', 'anymal_LFleg_rom', 'anymal_RHleg_rom']
urdfSuffix = ""
srdfSuffix = ""
name = urdfName
......@@ -35,32 +35,28 @@ 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_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]
ref_EE_rArm = [-0.373, -0.264, -0.448]
#ref_EE_lLeg = [0.3, 0.165 , -0.44]
#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}
# ref_EE_lLeg = [0.3, 0.165 , -0.44]
# 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:
self.name = name
Parent.__init__(self, self.name, self.rootJointType, load, client, None, clientRbprm)
self.setReferenceEndEffector('anymal_LFleg_rom',self.ref_EE_lLeg)
self.setReferenceEndEffector('anymal_RFleg_rom',self.ref_EE_rLeg)
self.setReferenceEndEffector('anymal_LHleg_rom',self.ref_EE_lArm)
self.setReferenceEndEffector('anymal_RHleg_rom',self.ref_EE_rArm)
self.setReferenceEndEffector('anymal_LFleg_rom', self.ref_EE_lLeg)
self.setReferenceEndEffector('anymal_RFleg_rom', self.ref_EE_rLeg)
self.setReferenceEndEffector('anymal_LHleg_rom', self.ref_EE_lArm)
self.setReferenceEndEffector('anymal_RHleg_rom', self.ref_EE_rArm)
......@@ -17,11 +17,11 @@
# <http://www.gnu.org/licenses/>.
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody as Parent
from pinocchio import SE3, Quaternion
from pinocchio import SE3
import numpy as np
class Robot (Parent):
##
class Robot(Parent):
# Information to retrieve urdf and srdf files.
packageName = "anymal_description"
......@@ -31,7 +31,7 @@ class Robot (Parent):
urdfSuffix = "_boxFeet"
srdfSuffix = ""
## Information about the names of thes joints defining the limbs of the robot
# Information about the names of thes joints defining the limbs of the robot
rLegId = 'RFleg'
rleg = 'RF_HAA'
rfoot = 'RF_CONTACT_3'
......@@ -45,81 +45,125 @@ class Robot (Parent):
rarm = 'RH_HAA'
rhand = 'RH_CONTACT_3'
referenceConfig = [
0.,
0.,
0.444,
0.,
0.,
0.,
1., # FF
0.04,
0.74,
-1.08,
0.34,
-0.04,
0.,
0.04,
-0.74,
1.08,
-0.34,
-0.04,
0.,
-0.04,
0.74,
-1.08,
0.34,
0.04,
0.,
-0.04,
-0.74,
1.08,
-0.34,
0.04,
0.
]
referenceConfig =[0.,0.,0.444, 0.,0.,0.,1., # FF
0.04, 0.74, -1.08,0.34,-0.04,0.,
0.04, -0.74, 1.08,-0.34,-0.04,0.,
-0.04, 0.74, -1.08,0.34,0.04, 0.,
-0.04, -0.74, 1.08,-0.34,0.04,0.
]
reference_weights=[100.,1.,1.,0.,0.,0.]
reference_weights = [100., 1., 1., 0., 0., 0.]
# informations required to generate the limbs databases the limbs :
# informations required to generate the limbs databases the limbs :
nbSamples = 50000
octreeSize = 0.01
cType = "_6_DOF"
offset = [0.,0.,0.]
offset = [0., 0., 0.]
rLegLimbOffset = [0.373, 0.264, 0.]
lLegLimbOffset = [0.373, -0.264,0.]
lLegLimbOffset = [0.373, -0.264, 0.]
rArmLimbOffset = [-0.373, 0.264, 0.]
lArmLimbOffset = [-0.373, -0.264, 0.]
normal = [0,0,1]
legx = 0.02; legy = 0.02
kinematicConstraintsPath="package://anymal-rbprm/com_inequalities/"
normal = [0, 0, 1]
legx = 0.02
legy = 0.02
kinematicConstraintsPath = "package://anymal-rbprm/com_inequalities/"
minDist = 0.2
minDist = 0.2
# 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
#limbs_names = [lLegId,rArmId,rLegId,lArmId] # default order to try to remove contacts at the beginning of the contact plan
limbs_names = [rArmId,lArmId,lLegId,rLegId]
dict_limb_rootJoint = {rLegId:rleg, lLegId:lleg, rArmId:rarm, lArmId:larm}
dict_limb_joint = {rLegId:rfoot, lLegId:lfoot, rArmId:rhand, lArmId:lhand}
dict_limb_color_traj = {rfoot:[0,1,0,1], lfoot:[1,0,0,1],rhand:[0,0,1,1],lhand:[0.9,0.5,0,1]}
# limbs_names = [rArmId,lLegId,lArmId,rLegId]
# reverse default order to try to remove contacts at the beginning of the contact plan
# limbs_names = [lLegId,rArmId,rLegId,lArmId]
# default order to try to remove contacts at the beginning of the contact plan
limbs_names = [rArmId, lArmId, lLegId, rLegId]
dict_limb_rootJoint = {rLegId: rleg, lLegId: lleg, rArmId: rarm, lArmId: larm}
dict_limb_joint = {rLegId: rfoot, lLegId: lfoot, rArmId: rhand, lArmId: lhand}
dict_limb_color_traj = {rfoot: [0, 1, 0, 1], lfoot: [1, 0, 0, 1], rhand: [0, 0, 1, 1], lhand: [0.9, 0.5, 0, 1]}
FOOT_SAFETY_SIZE = 0.01
# size of the contact surface (x,y)
dict_size={rfoot:[0.031 , 0.031], lfoot:[0.031 , 0.031],rhand:[0.031 , 0.031],lhand:[0.031 , 0.031]}
#dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
#various offset used by scripts
dict_size = {rfoot: [0.031, 0.031], lfoot: [0.031, 0.031], rhand: [0.031, 0.031], lhand: [0.031, 0.031]}
# dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
# various offset used by scripts
MRsole_offset = SE3.Identity()
MRsole_offset.translation = np.matrix(offset).T
MLsole_offset = MRsole_offset.copy()
MRhand_offset = MRsole_offset.copy()
MLhand_offset = MRsole_offset.copy()
dict_offset = {rfoot:MRsole_offset, lfoot:MLsole_offset, rhand:MRhand_offset, lhand:MLhand_offset}
dict_limb_offset= {rLegId:rLegLimbOffset, lLegId:lLegLimbOffset, rArmId:rArmLimbOffset, lArmId:lArmLimbOffset}
dict_normal = {rfoot:normal, lfoot:normal, rhand:normal, lhand:normal}
dict_offset = {rfoot: MRsole_offset, lfoot: MLsole_offset, rhand: MRhand_offset, lhand: MLhand_offset}
dict_limb_offset = {rLegId: rLegLimbOffset, lLegId: lLegLimbOffset, rArmId: rArmLimbOffset, lArmId: lArmLimbOffset}
dict_normal = {rfoot: normal, lfoot: normal, rhand: normal, lhand: normal}
# display transform :
MRsole_display = SE3.Identity()
MLsole_display = SE3.Identity()
MRhand_display = SE3.Identity()
MLhand_display = SE3.Identity()
dict_display_offset = {rfoot:MRsole_display, lfoot:MLsole_display, rhand:MRhand_display, lhand:MLhand_display}
dict_display_offset = {rfoot: MRsole_display, lfoot: MLsole_display, rhand: MRhand_display, lhand: MLhand_display}
def __init__ (self, name = None,load = True):
Parent.__init__ (self,load)
def __init__(self, name=None, load=True):
Parent.__init__(self, load)
if load:
self.loadFullBodyModel(self.urdfName, self.rootJointType, self.meshPackageName, self.packageName, self.urdfSuffix, self.