copy changes of 6D to 3D

src/hpp/corbaserver/rbprm/anymal/robot.py

@@ -27,9 +27,9 @@ class Robot (Parent):
     packageName = "anymal_description"
     meshPackageName = "anymal_description"
     rootJointType = "freeflyer"
-    urdfName = "anymal"
-    urdfSuffix = "_reachability"
-    srdfSuffix = "_reachability"
+    urdfName = "anymal_reachability"
+    urdfSuffix = "_boxFeet"
+    srdfSuffix = ""
 
     ## Information about the names of thes joints defining the limbs of the robot
     rLegId = 'RFleg'
@@ -53,29 +53,35 @@ class Robot (Parent):
         -0.04, -0.74, 1.08,-0.34,0.04,0.
         ]
     
+    reference_weights=[100.,1.,1.,0.,0.,0.]
+
     # 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.33]
+
+    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://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,lArmId,lLegId,rArmId] # default order to try to remove contacts at the beginning of the contact plan
+    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]}
     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
     MRsole_offset = SE3.Identity()
     MRsole_offset.translation = np.matrix(offset).T
@@ -83,6 +89,7 @@ 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()
@@ -99,11 +106,20 @@ class Robot (Parent):
             self.name = name
 
     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
+            dict_heuristic = {}
+            for id in self.limbs_names:
+                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"}
         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)
+            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)