Skip to content
GitLab
Commit eca08ff6 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

add methods to set constrained joints bounds for contact generation

parent 9951b509
Hide whitespace changes
Inline Side-by-side
src/hpp/corbaserver/rbprm/anymal/robot.py
View file @ eca08ff6
......@@ -24,42 +24,46 @@ class Robot (Parent):
##
# Information to retrieve urdf and srdf files.
packageName = "anymal_description"
meshPackageName = "anymal_description"
packageName = "anymal_data"
meshPackageName = "anymal_data"
rootJointType = "freeflyer"
urdfName = "anymal_reachability"
urdfSuffix = "_boxFeet"
urdfName = "anymal"
urdfSuffix = ""
srdfSuffix = ""
## Information about the names of thes joints defining the limbs of the robot
rLegId = 'RFleg'
rleg = 'RF_HAA'
rfoot = 'RF_KFE'
rfoot = 'RF_ADAPTER_TO_FOOT'
lLegId = 'LFleg'
lleg = 'LF_HAA'
lfoot = 'LF_KFE'
lfoot = 'LF_ADAPTER_TO_FOOT'
lArmId = 'LHleg'
larm = 'LH_HAA'
lhand = 'LH_KFE'
lhand = 'LH_ADAPTER_TO_FOOT'
rArmId = 'RHleg'
rarm = 'RH_HAA'
rhand = 'RH_KFE'
rhand = 'RH_ADAPTER_TO_FOOT'
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.47, 0.,0.,0.,1., # FF
0.04, 0.74, -1.08,
0.04, -0.74, 1.08,
-0.04, 0.74, -1.08,
-0.04, -0.74, 1.08,
]
reference_weights=[100.,1.,1.,0.,0.,0.]
postureWeights=[0,0,0,0,0,0, #FF
100.,1.,1.,
100.,1.,1.,
100.,1.,1.,
100.,1.,1.,]
# informations required to generate the limbs databases the limbs :
nbSamples = 50000
octreeSize = 0.01
cType = "_3_DOF"
offset = [0.,0.,0.33]
offset = [0.,0.,-0.011]
rLegLimbOffset = [0.373, 0.264, 0.]
lLegLimbOffset = [0.373, -0.264,0.]
......@@ -69,7 +73,7 @@ class Robot (Parent):
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
......@@ -80,7 +84,7 @@ class Robot (Parent):
dict_limb_color_traj = {rfoot:[0,1,0,1], lfoot:[1,0,0,1],rhand:[0,0,1,1],lhand:[0.9,0.5,0,1]}
FOOT_SAFETY_SIZE = 0.01
# size of the contact surface (x,y)
dict_size={rfoot:[0.031 , 0.031], lfoot:[0.031 , 0.031],rhand:[0.031 , 0.031],lhand:[0.031 , 0.031]}
dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
#dict_size={rfoot:[0.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()
......@@ -98,12 +102,30 @@ class Robot (Parent):
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,"RH":18}
def __init__ (self, name = None,load = True):
Parent.__init__ (self,load)
if load:
self.loadFullBodyModel(self.urdfName, self.rootJointType, self.meshPackageName, self.packageName, self.urdfSuffix, self.srdfSuffix)
if name != None:
self.name = name
# save original bounds of the urdf for futur reset
self.LF_HAA_bounds = self.getJointBounds('LF_HAA')
self.LF_HFE_bounds = self.getJointBounds('LF_HFE')
self.LF_HAA_bounds = self.getJointBounds('LF_KFE')
self.RF_HAA_bounds = self.getJointBounds('RF_HAA')
self.RF_HFE_bounds = self.getJointBounds('RF_HFE')
self.RF_HAA_bounds = self.getJointBounds('RF_KFE')
self.LH_HAA_bounds = self.getJointBounds('LH_HAA')
self.LH_HFE_bounds = self.getJointBounds('LH_HFE')
self.LH_HAA_bounds = self.getJointBounds('LH_KFE')
self.RH_HAA_bounds = self.getJointBounds('RH_HAA')
self.RH_HFE_bounds = self.getJointBounds('RH_HFE')
self.RH_HAA_bounds = self.getJointBounds('RH_KFE')
def loadAllLimbs(self,heuristic, analysis = None, nbSamples = nbSamples, octreeSize = octreeSize):
if isinstance(heuristic,basestring):#only one heuristic name given assign it to all the limbs
......@@ -119,7 +141,62 @@ class Robot (Parent):
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.T.tolist()[0],self.dict_normal[eff],self.dict_size[eff][0]/2.,self.dict_size[eff][1]/2.,nbSamples,dict_heuristic[id],octreeSize,self.cType,kinematicConstraintsPath=self.kinematicConstraintsPath+self.dict_limb_rootJoint[id]+"06_com_constraints.obj",limbOffset=self.dict_limb_offset[id],kinematicConstraintsMin=self.minDist)
self.addLimb(id,self.dict_limb_rootJoint[id],eff,self.dict_offset[eff].translation.T.tolist()[0],self.dict_normal[eff],self.dict_size[eff][0]/2.,self.dict_size[eff][1]/2.,nbSamples,dict_heuristic[id],octreeSize,self.cType,kinematicConstraintsPath=self.kinematicConstraintsPath+self.dict_limb_rootJoint[id]+"_06_com_constraints.obj",limbOffset=self.dict_limb_offset[id],kinematicConstraintsMin=self.minDist)
if analysis :
self.runLimbSampleAnalysis(id, analysis, True)
def setConstrainedJointsBounds(self):
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('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 setVeryConstrainedJointsBounds(self):
self.setJointBounds('LF_HAA',[-1.,1.])
self.setJointBounds('LF_HFE',[0,1.])
self.setJointBounds('LF_KFE',[-2.35,0.])
self.setJointBounds('RF_HAA',[-1.,1.])
self.setJointBounds('RF_HFE',[0,1.])
self.setJointBounds('RF_KFE',[-2.35,0.])
self.setJointBounds('LH_HAA',[-1.,1.])
self.setJointBounds('LH_HFE',[-1.1,-0.2])
self.setJointBounds('LH_KFE',[0.,2.35])
self.setJointBounds('RH_HAA',[-1.,1.])
self.setJointBounds('RH_HFE',[-1.1,-0.2])
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_HAA_bounds)
self.setJointBounds('RF_HAA',self.RF_HAA_bounds)
self.setJointBounds('RF_HFE',self.RF_HFE_bounds)
self.setJointBounds('RF_KFE',self.RF_HAA_bounds)
self.setJointBounds('LH_HAA',self.LH_HAA_bounds)
self.setJointBounds('LH_HFE',self.LH_HFE_bounds)
self.setJointBounds('LH_KFE',self.LH_HAA_bounds)
self.setJointBounds('RH_HAA',self.RH_HAA_bounds)
self.setJointBounds('RH_HFE',self.RH_HFE_bounds)
self.setJointBounds('RH_KFE',self.RH_HAA_bounds)
Supports Markdown
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