diff --git a/src/hpp/corbaserver/rbprm/anymal_contact6D/robot.py b/src/hpp/corbaserver/rbprm/anymal_contact6D/robot.py index 628124ba8f53700805ac97da36e8c8b6fb008062..7ea9c95239cc81c3d12166d8ef09a3d5e9e20190 100644 --- a/src/hpp/corbaserver/rbprm/anymal_contact6D/robot.py +++ b/src/hpp/corbaserver/rbprm/anymal_contact6D/robot.py @@ -24,79 +24,62 @@ class Robot (Parent): ## # Information to retrieve urdf and srdf files. - packageName = "hyq_description" - meshPackageName = "hyq_description" + packageName = "anymal_description" + meshPackageName = "anymal_description" rootJointType = "freeflyer" - urdfName = "hyq" - urdfSuffix = "_contact6D" - srdfSuffix = "" + urdfName = "anymal" + urdfSuffix = "_reachability" + srdfSuffix = "_reachability" ## Information about the names of thes joints defining the limbs of the robot - rLegId = 'rfleg' - rleg = 'rf_haa_joint' - rfoot = 'rf_foot_Z' - lLegId = 'lfleg' - lleg = 'lf_haa_joint' - lfoot = 'lf_foot_Z' - lArmId = 'lhleg' - larm = 'lh_haa_joint' - lhand = 'lh_foot_Z' - rArmId = 'rhleg' - rarm = 'rh_haa_joint' - rhand = 'rh_foot_Z' + rLegId = 'RFleg' + rleg = 'RF_HAA' + rfoot = 'RF_CONTACT_3' + lLegId = 'LFleg' + lleg = 'LF_HAA' + lfoot = 'LF_CONTACT_3' + lArmId = 'LHleg' + larm = 'LH_HAA' + lhand = 'LH_CONTACT_3' + rArmId = 'RHleg' + rarm = 'RH_HAA' + rhand = 'RH_CONTACT_3' - referenceConfig = [0., - 0., - 0.6, - 0., - 0., - 0., - 1., - 0, # LF - 0.7853981633974483, - -1.5707963267948966, - 0, - 0.7853981633974483, - 0, - 0, # LH - -0.7853981633974483, - 1.5707963267948966, - 0, - -0.7853981633974483, - 0, - 0, # RF - 0.7853981633974483, - -1.5707963267948966, - 0, - 0.7853981633974483, - 0, - 0, # RH - -0.7853981633974483, - 1.5707963267948966, - 0, - -0.7853981633974483, - 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. + ] # 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.021] + rLegLimbOffset = [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://hyq-rbprm/com_inequalities/" + kinematicConstraintsPath="package://anymal-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.2 # data used by scripts : - limbs_names = [rLegId,lLegId,rArmId,lArmId] + limbs_names = [rLegId,lArmId,lLegId,rArmId] # default order to try to remove contacts at the beginning of the contact plan + 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.04 , 0.04], lfoot:[0.04 , 0.04],rhand:[0.04 , 0.04],lhand:[0.04 , 0.04]} + dict_size={rfoot:[0.031 , 0.031], lfoot:[0.031 , 0.031],rhand:[0.031 , 0.031],lhand:[0.031 , 0.031]} #various offset used by scripts MRsole_offset = SE3.Identity() @@ -105,7 +88,8 @@ class Robot (Parent): 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} # display transform : MRsole_display = SE3.Identity() MLsole_display = SE3.Identity() @@ -119,3 +103,13 @@ class Robot (Parent): self.loadFullBodyModel(self.urdfName, self.rootJointType, self.meshPackageName, self.packageName, self.urdfSuffix, self.srdfSuffix) if name != None: self.name = name + + def loadAllLimbs(self,heuristic, analysis = None, nbSamples = nbSamples, octreeSize = octreeSize): + for id in self.limbs_names: + 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,heuristic,octreeSize,self.cType,kinematicConstraintsPath=self.kinematicConstraintsPath+self.dict_limb_rootJoint[id]+"_com_constraints.obj",kinematicConstraintsMin=self.minDist,limbOffset=self.dict_limb_offset[id]) + if analysis : + self.runLimbSampleAnalysis(id, analysis, True) +