[cs] fix error when robot don't have any extraDOF

scripts/hpp_wholebody_motion/contact_sequence/rbprm.py

@@ -21,8 +21,7 @@ def generateContactSequence(fb,configs,beginId,endId):
     n_steps = n_double_support*2 -1 
     # Notice : what we call double support / simple support are in fact the state with all the contacts and the state without the next moving contact
     extraDOF = int(fb.client.robot.getDimensionExtraConfigSpace())
-    if extraDOF <= 0 :
-        extraDOF = None
+    configSize = fb.client.robot.getConfigSize() - extraDOF
     cs = ContactSequenceHumanoid(n_steps)
     unusedPatch = cs.contact_phases[0].LF_patch.copy()
     unusedPatch.placement = SE3.Identity()
@@ -141,7 +140,7 @@ def generateContactSequence(fb,configs,beginId,endId):
         #phase_d.time_trajectory.append((fb.getDurationForState(stateId))*cfg.DURATION_n_CONTACTS/cfg.SPEED)
         phase_d.init_state=init_state
         phase_d.final_state=final_state
-        phase_d.reference_configurations.append(np.matrix((configs[config_id][:-extraDOF])).T)        
+        phase_d.reference_configurations.append(np.matrix((configs[config_id][:configSize])).T)        
         #print "done for double support"
         
         
@@ -168,7 +167,7 @@ def generateContactSequence(fb,configs,beginId,endId):
                 phase_s.RH_patch.active = False
         # retrieve the COM position for init and final state 
          
-        phase_s.reference_configurations.append(np.matrix((configs[config_id][:-extraDOF])).T)
+        phase_s.reference_configurations.append(np.matrix((configs[config_id][:configSize])).T)
         init_state = phase_d.init_state.copy()
         final_state = phase_d.final_state.copy()
         fb.setCurrentConfig(configs[config_id+1])
@@ -239,7 +238,7 @@ def generateContactSequence(fb,configs,beginId,endId):
         if var == fb.rArmId:
             phase_d.RH_patch.placement = MRH
     # retrieve the COM position for init and final state (equal for double support phases)    
-    phase_d.reference_configurations.append(np.matrix((configs[-1][:-extraDOF])).T)
+    phase_d.reference_configurations.append(np.matrix((configs[-1][:configSize])).T)
     init_state = phase_d.init_state
     init_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose()
     init_state[3:9] = np.matrix(configs[-1][-6:]).transpose()