From eca08ff6f5ab02efe1c8b956b147a259d9f5e55c Mon Sep 17 00:00:00 2001
From: pFernbach <pierre.fernbach@gmail.com>
Date: Fri, 31 May 2019 16:38:18 +0200
Subject: [PATCH] add methods to set constrained joints bounds for contact
generation
---
src/hpp/corbaserver/rbprm/anymal/robot.py | 113 ++++++++++++++++++----
1 file changed, 95 insertions(+), 18 deletions(-)
diff --git a/src/hpp/corbaserver/rbprm/anymal/robot.py b/src/hpp/corbaserver/rbprm/anymal/robot.py
index 63c50c4..1004af0 100644
--- a/src/hpp/corbaserver/rbprm/anymal/robot.py
+++ b/src/hpp/corbaserver/rbprm/anymal/robot.py
@@ -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)
+
+
--
GitLab