[tools] surfaces_from_config : take an argument to define a maximal yaw...

[tools] surfaces_from_config : take an argument to define a maximal yaw variations between two config

script/tools/surfaces_from_path.py

@@ -3,6 +3,7 @@ from pinocchio import Quaternion,SE3,XYZQUATToSe3
 from tools.narrow_convex_hull import getSurfaceExtremumPoints, removeDuplicates, normal, area
 from tools.display_tools import displaySurfaceFromPoints
 import numpy as np
+from pinocchio import Quaternion, log3
 import eigenpy
 eigenpy.switchToNumpyMatrix()
 
@@ -11,6 +12,8 @@ MAX_SURFACE = 0.3 # if a contact surface is greater than this value, the interse
 LF = 0
 RF = 1  
 
+
+
 # change the format into an array  
 def listToArray (seqs):
   nseq = []; nseqs= []
@@ -82,31 +85,58 @@ def getMergedPhases (seqs):
     nseq = removeDuplicates(nseq)
     nseqs.append(nseq)  
   return nseqs    
-  
 
-def getSurfacesFromGuideContinuous(rbprmBuilder,ps,afftool,pId,viewer = None,step = 1.,useIntersection= False,mergeCandidates = False):
+def computeRootYawAngleBetwwenConfigs(q0,q1):
+  quat0 = Quaternion(q0[6],q0[3],q0[4],q0[5])
+  quat1 = Quaternion(q1[6],q1[3],q1[4],q1[5])
+  v_angular = log3(quat0.matrix().T*quat1.matrix())
+  #print "q_prev : ",q0
+  #print "q      : ",q1
+  #print "v_angular = ",v_angular
+  return v_angular[2,0]
+
+def isYawVariationsInsideBounds(q0,q1,max_yaw = 0.5):
+  yaw = abs(computeRootYawAngleBetwwenConfigs(q0,q1))
+  print "yaw = ",yaw
+  return  yaw < max_yaw
+
+def getSurfacesFromGuideContinuous(rbprmBuilder,ps,afftool,pId,viewer = None,step = 1.,useIntersection= False,mergeCandidates = False,max_yaw = 0.5):
   pathLength = ps.pathLength(pId) #length of the path
-  discretizationStep = 0.2 # step at which we check the colliding surfaces
+  discretizationStep = 0.1 # step at which we check the colliding surfaces
   #print "path length = ",pathLength
   # get surface information
   all_surfaces = getAllSurfaces(afftool) 
   all_names = afftool.getAffRefObstacles("Support") # id in names and surfaces match
   surfaces_dict = dict(zip(all_names, all_surfaces)) # map surface names to surface points
   seqs = [] # list of list of surfaces : for each phase contain a list of surfaces. One phase is defined by moving of 'step' along the path
-  t = 0.
+  configs = []
+  t = -discretizationStep
   current_phase_end = step
   end = False
   i = 0
+  q_prev = ps.configAtParam(pId, 0)
+  q = q_prev[::]
+  configs.append(q)
   while not end: # for all the path
-    #print "Looking for surfaces for phase "+str(len(seqs))+" for t in ["+str(t)+" ; "+str(current_phase_end)+" ] "
+    print "Looking for surfaces for phase "+str(len(seqs))+" for t in ["+str(t+discretizationStep)+" ; "+str(current_phase_end)+" ] "
     phase_contacts_names = []
-    while t < current_phase_end: # get the names of all the surfaces that the rom collide while moving from current_phase_end-step to current_phase_end
+    rot_valid = True
+    while t < current_phase_end and rot_valid: # get the names of all the surfaces that the rom collide while moving from current_phase_end-step to current_phase_end
+      t += discretizationStep
       q = ps.configAtParam(pId, t)
+      if not isYawVariationsInsideBounds(q_prev,q,max_yaw):
+        print "yaw variation out of bounds, try to reduce the time step : "
+        rot_valid = False
+        t -= discretizationStep
+        q = ps.configAtParam(pId, t)
+        while isYawVariationsInsideBounds(q_prev,q,max_yaw):
+          t += 0.001
+          q = ps.configAtParam(pId, t)
+      print " t in getSurfacesFromGuideContinuous : ",t
       step_contacts = getContactsNames(rbprmBuilder,i,q)
       for contact_name in step_contacts : 
         if not contact_name in phase_contacts_names:
           phase_contacts_names.append(contact_name)
-      t += discretizationStep
     # end current phase
     # get all the surfaces from the names and add it to seqs: 
     if useIntersection : 
@@ -131,18 +161,17 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,ps,afftool,pId,viewer = None,ste
     #print "phase_surfaces_array = ",phase_surfaces_array    
     seqs.append(phase_surfaces_array)
     # increase values for next phase
-    t = current_phase_end
+    q_prev = q[::]
+    configs.append(q)
     i += 1 
-    if current_phase_end == pathLength:
+    t -= discretizationStep # because we want the first iteration of the next phase to test the same t as the last iter of this phase
+    if t >= (pathLength - discretizationStep/2.):
       end = True
-    current_phase_end += step
+    current_phase_end = t + step
     if current_phase_end >= pathLength:
       current_phase_end = pathLength
   # end for all the guide path
   #get rotation matrix of the root at each discretization step
-  configs = []
-  for t in arange (0, pathLength, step) :
-    configs.append(ps.configAtParam(pId, t)) 
   R = getRotationMatrixFromConfigs(configs)
   return R,seqs