[main][util] add class to check motion for collision and joint limits

scripts/hpp_wholebody_motion/config.py

@@ -9,7 +9,8 @@ LOAD_CS = False
 LOAD_CS_COM = False
 SAVE_CS = not LOAD_CS and True 
 SAVE_CS_COM = not LOAD_CS_COM and True
-EXPORT_GAZEBO = True
+EXPORT_GAZEBO = False
+EXPORT_OPENHRP = True
 EXPORT_PATH = OUTPUT_DIR+"/export"
 
 ##DISPLAY settings : 
@@ -19,13 +20,13 @@ DISPLAY_INIT_GUESS_TRAJ = False
 DISPLAY_WP_COST=True
 DISPLAY_COM_TRAJ = True
 DISPLAY_FEET_TRAJ = True
-DISPLAY_WB_MOTION = True
+DISPLAY_WB_MOTION = False
 DT_DISPLAY = 0.05 # dt used to display the wb motion
 
 ###  Settings for generate_contact_sequence
 FORCE_STRAIGHT_LINE = False # DEBUG ONLY should be false
 
-### Settings for locomote script :
+### Settings for centroidal script :
 MU=0.5
 USE_GEOM_INIT_GUESS = True
 USE_CROC_INIT_GUESS = False
@@ -37,19 +38,22 @@ COM_SHIFT_Z = 0.0
 TIME_SHIFT_COM = 0.0
 
 USE_WP_COST = False # use wp from the contact sequence in the cost function of timeopt
-#end effector :
+
+## Settings for end effector :
 USE_LIMB_RRT = False
 USE_CONSTRAINED_BEZIER = True
 USE_BEZIER_EE = True
 EFF_CHECK_COLLISION = True
+WB_ABORT_WHEN_INVALID = True
+
 ##  Settings for whole body : 
 YAW_ROT_GAIN = 1.
 USE_CROC_COM = False
-WB_VERBOSE = True
+WB_VERBOSE = False
 WB_STOP_AT_EACH_PHASE = False
 IK_dt = 0.001  # controler time step
 IK_PRINT_N = 500  # print state of the problem every IK_PRINT_N time steps (if verbose = True)
-
+CHECK_FINAL_MOTION = True
 
 # import specific settings for the selected demo. This settings may override default ones.
 import importlib

scripts/hpp_wholebody_motion/utils/check_path.py

 # Copyright 2018, LAAS-CNRS
 # Author: Pierre Fernbach
 
-from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
-from config import *
-
+import hpp_wholebody_motion.config as cfg
 import numpy as np
-def pinnochioQuaternion(q):
-    assert len(q)>6, "config vector size must be superior to 7"
-    w = q[3]
-    q[3:6] = q[4:7]
-    q[6] = w
-    return q
-
-def HPPQuaternion(q):
-    assert len(q)>6, "config vector size must be superior to 7"
-    w = q[6]
-    q[4:7] = q[3:6]
-    q[3] = w
-    return q
-
 
 class PathChecker():
     
-    def __init__(self,r,fullBody,cs,q_t,t_t):
+    def __init__(self,r,fullBody,cs,sizeQ,verbose = False):
         self.r = r
         self.cs = cs
-        self.q_t = q_t
-        self.t_t = t_t
-        self.size_conf = q_t.shape[0]
         self.fullBody = fullBody # with effector collision disabled
+        self.sizeQ = sizeQ
+        self.configSize = fullBody.getConfigSize()
+        self.dt = cfg.IK_dt
+        self.verbose = verbose
+        self.extraDof =  int(fullBody.client.robot.getDimensionExtraConfigSpace())
+    
+    # convert to correct format for hpp, add extra dof if necessary
+    # return valid,message  : valid = bool, message = string
+    def checkConfig(self,q_m):
+        q = [0]*self.configSize
+        q[:self.sizeQ] = q_m.T.tolist()[0]
+        res = self.fullBody.isConfigValid(q)
+        return res[0],res[1]
+    
+    
+    
+    def qAtT(self,t,q_t):
+        for it in range(1,len(q_t)-1):
+            if t>(self.dt*(it-0.5)) and t<=(self.dt*(it+0.5)):
+                return q_t[it]    
+    
+    
+  
+    def phaseOfT(self,t_switch):
+        for i in range(self.cs.size()):
+            p = self.cs.contact_phases[i]
+            if t_switch>=p.time_trajectory[0] and t_switch<=p.time_trajectory[-1]:
+                return i
+        
+    
+    def phaseOfId(self,id):
+        t_switch = self.dt*float(id)
+        return self.phaseOfT(t_switch)
+        
+    # return a bool (true = valid) and the time of the first invalid q (None if always valid): 
+    # if Verbose = True : check the complete motion before returning,
+    # if False stop at the first invalid
+    def check_motion(self,q_t):
+        always_valid = True
+        first_invalid = None
+        for i in range(len(q_t)):
+            valid,mess = self.checkConfig(q_t[i])
+            if not valid : 
+                if always_valid : # first invalid config
+                    always_valid = False
+                    first_invalid = self.dt*(float(i))
+                if self.verbose:
+                    print "Invalid config at t= ",self.dt*(float(i))
+                    print mess                    
+                else:
+                    return always_valid,first_invalid
+        return always_valid,first_invalid
+    
     
     
+    ############# old stuffs (need to be updated if necessary : #############
+    """
     def check_postures(self,verbose = True):
         bad_phases = []
         success_global=True
@@ -149,34 +185,12 @@ class PathChecker():
         q[:self.size_conf] = self.q_t[:,id].transpose().tolist()[0]
         q = HPPQuaternion(q)        
         self.r(q)
-        
-    def qAtT(self,t):
-        for it in range(1,len(self.t_t)-1):
-            if t>((self.t_t[it]+self.t_t[it-1])/2.) and t<=((self.t_t[it]+self.t_t[it+1])/2.):
-                q = [0]*self.fullBody.getConfigSize() 
-                q[:self.size_conf] = self.q_t[:,it].transpose().tolist()[0]
-                q = HPPQuaternion(q)
-                #print "closer t : ",self.t_t[it]
-                #print "at id : ",it
-                return q
+
         
     def showConfigAtTime(self,t):
         q = self.qAtT(t)      
         self.r(q) 
-    
-    def phaseOfT(self,t_switch):
-        for i in range(self.cs.size()):
-            p = self.cs.contact_phases[i]
-            if t_switch>=p.time_trajectory[0] and t_switch<=p.time_trajectory[-1]:
-                return i
-        
-    
-    def phaseOfId(self,id):
-        t_switch = self.t_t[id]
-        return self.phaseOfT(t_switch)
-        
-     
-
+  
 
     def checkAdditionalJointsConstraints(self,list_of_id,mins,maxs):
         valid = True
@@ -191,4 +205,4 @@ class PathChecker():
         
         
         return valid
-    
\ No newline at end of file
+    """
\ No newline at end of file

scripts/run_locomote.py

@@ -73,6 +73,25 @@ if cfg.DISPLAY_WB_MOTION:
     raw_input("Press Enter to display the whole body motion ...")
     display_tools.displayWBmotion(v,q_t,cfg.IK_dt,cfg.DT_DISPLAY)
 
+if cfg.CHECK_FINAL_MOTION :
+    from hpp_wholebody_motion.utils import check_path
+    print "## Begin validation of the final motion (collision and joint-limits)"
+    validator = check_path.PathChecker(v,cp.fullBody,cs_com,len(q_t[0]),True)
+    motion_valid,t_invalid = validator.check_motion(q_t)
+    print "## Check final motion, valid = ",motion_valid
+    if not motion_valid:
+        print "## First invalid time : ",t_invalid
+else :
+    motion_valid = True
+
+
+if cfg.EXPORT_OPENHRP and motion_valid:
+    from hpp_wholebody_motion.export import openHRP
+    openHRP.export(q_t,v_t,a_t)
+if cfg.EXPORT_GAZEBO and motion_valid:
+    from hpp_wholebody_motion.export import gazebo
+    gazebo.export(q_t)
+