Commit 8d66377c authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[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
parent 59442668
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment