Unverified Commit 7c4f4837 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #65 from pFernbach/topic/surface_extraction

fix surface extraction from guide
parents bcfad6ea 92facc79
...@@ -7,12 +7,15 @@ class PathPlanner(TalosPathPlanner): ...@@ -7,12 +7,15 @@ class PathPlanner(TalosPathPlanner):
# select ROM model with really conservative ROM shapes # select ROM model with really conservative ROM shapes
Robot.urdfName = "talos_trunk_large_reducedROM" Robot.urdfName = "talos_trunk_large_reducedROM"
self.robot_node_name = "talos_trunk_large_reducedROM" self.robot_node_name = "talos_trunk_large_reducedROM"
Robot.urdfNameRom = ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced'] Robot.rLegId = 'talos_rleg_rom_reduced'
Robot.lLegId = 'talos_lleg_rom_reduced'
Robot.urdfNameRom = [Robot.lLegId, Robot.rLegId]
self.used_limbs = Robot.urdfNameRom self.used_limbs = Robot.urdfNameRom
self.rbprmBuilder = Robot() self.rbprmBuilder = Robot()
# As the ROM names have changed, we need to set this values again (otherwise it's automatically done) # As the ROM names have changed, we need to set this values again (otherwise it's automatically done)
self.rbprmBuilder.setReferenceEndEffector('talos_lleg_rom_reduced', self.rbprmBuilder.ref_EE_lLeg) self.rbprmBuilder.setReferenceEndEffector(Robot.lLegId, self.rbprmBuilder.ref_EE_lLeg)
self.rbprmBuilder.setReferenceEndEffector('talos_rleg_rom_reduced', self.rbprmBuilder.ref_EE_rLeg) self.rbprmBuilder.setReferenceEndEffector(Robot.rLegId, self.rbprmBuilder.ref_EE_rLeg)
def init_problem(self): def init_problem(self):
super().init_problem() super().init_problem()
...@@ -32,10 +35,11 @@ class PathPlanner(TalosPathPlanner): ...@@ -32,10 +35,11 @@ class PathPlanner(TalosPathPlanner):
self.root_translation_bounds = [-1.5, 3, 0., 3.3, 0.95, 2.] self.root_translation_bounds = [-1.5, 3, 0., 3.3, 0.95, 2.]
self.init_problem() self.init_problem()
self.q_init[:3] = [0.05, 1.2, 0.98] self.q_init[:2] = [0., 1.2]
self.q_goal[:3] = [1.87, 1.2, 1.58] self.q_goal[:2] = [1.87, 1.2]
self.q_goal[2] += 0.6
self.init_viewer("multicontact/bauzil_stairs", reduce_sizes=[0.11, 0., 0.], visualize_affordances=["Support"]) self.init_viewer("multicontact/bauzil_stairs", reduce_sizes=[0.08, 0., 0.], visualize_affordances=["Support"])
self.init_planner() self.init_planner()
self.solve() self.solve()
self.display_path() self.display_path()
......
...@@ -26,10 +26,12 @@ class PathPlanner(TalosPathPlanner): ...@@ -26,10 +26,12 @@ class PathPlanner(TalosPathPlanner):
Robot.urdfName = "talos_trunk_large_reducedROM" Robot.urdfName = "talos_trunk_large_reducedROM"
self.robot_node_name = "talos_trunk_large_reducedROM" self.robot_node_name = "talos_trunk_large_reducedROM"
# select conservative ROM for feet # select conservative ROM for feet
Robot.urdfNameRom += ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced'] Robot.rLegId = 'talos_rleg_rom_reduced'
Robot.lLegId = 'talos_lleg_rom_reduced'
Robot.urdfNameRom = [Robot.lLegId, Robot.rLegId]
self.rbprmBuilder = Robot() self.rbprmBuilder = Robot()
self.rbprmBuilder.setReferenceEndEffector('talos_lleg_rom_reduced', self.rbprmBuilder.ref_EE_lLeg) self.rbprmBuilder.setReferenceEndEffector(Robot.lLegId, self.rbprmBuilder.ref_EE_lLeg)
self.rbprmBuilder.setReferenceEndEffector('talos_rleg_rom_reduced', self.rbprmBuilder.ref_EE_rLeg) self.rbprmBuilder.setReferenceEndEffector(Robot.rLegId, self.rbprmBuilder.ref_EE_rLeg)
def compute_extra_config_bounds(self): def compute_extra_config_bounds(self):
# bounds for the extradof : by default use v_max/a_max on x and y axis and a large value on z axis # bounds for the extradof : by default use v_max/a_max on x and y axis and a large value on z axis
......
...@@ -37,12 +37,14 @@ class PathPlanner(TalosPathPlanner): ...@@ -37,12 +37,14 @@ class PathPlanner(TalosPathPlanner):
# select ROM model with really conservative ROM shapes # select ROM model with really conservative ROM shapes
Robot.urdfName = "talos_trunk_large_reducedROM" Robot.urdfName = "talos_trunk_large_reducedROM"
self.robot_node_name = "talos_trunk_large_reducedROM" self.robot_node_name = "talos_trunk_large_reducedROM"
Robot.urdfNameRom = ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced'] Robot.rLegId = 'talos_rleg_rom_reduced'
Robot.lLegId = 'talos_lleg_rom_reduced'
Robot.urdfNameRom = [Robot.lLegId, Robot.rLegId]
self.used_limbs = Robot.urdfNameRom self.used_limbs = Robot.urdfNameRom
self.rbprmBuilder = Robot() self.rbprmBuilder = Robot()
# As the ROM names have changed, we need to set this values again (otherwise it's automatically done) # As the ROM names have changed, we need to set this values again (otherwise it's automatically done)
self.rbprmBuilder.setReferenceEndEffector('talos_lleg_rom_reduced', self.rbprmBuilder.ref_EE_lLeg) self.rbprmBuilder.setReferenceEndEffector(Robot.lLegId, self.rbprmBuilder.ref_EE_lLeg)
self.rbprmBuilder.setReferenceEndEffector('talos_rleg_rom_reduced', self.rbprmBuilder.ref_EE_rLeg) self.rbprmBuilder.setReferenceEndEffector(Robot.rLegId, self.rbprmBuilder.ref_EE_rLeg)
def set_random_configs(self): def set_random_configs(self):
from hpp.corbaserver.rbprm.tools.sampleRotation import sampleRotationForConfig from hpp.corbaserver.rbprm.tools.sampleRotation import sampleRotationForConfig
......
...@@ -5,7 +5,7 @@ from hpp.corbaserver.rbprm.tools.display_tools import displaySurfaceFromPoints ...@@ -5,7 +5,7 @@ from hpp.corbaserver.rbprm.tools.display_tools import displaySurfaceFromPoints
import numpy as np import numpy as np
from pinocchio import Quaternion, log3 from pinocchio import Quaternion, log3
import eigenpy import eigenpy
eigenpy.switchToNumpyMatrix() eigenpy.switchToNumpyArray()
ROBOT_NAME = 'talos' ROBOT_NAME = 'talos'
MAX_SURFACE = 1. # if a contact surface is greater than this value, the intersection is used instead of the whole surface MAX_SURFACE = 1. # if a contact surface is greater than this value, the intersection is used instead of the whole surface
...@@ -50,7 +50,6 @@ def getAllSurfacesDict(afftool): ...@@ -50,7 +50,6 @@ def getAllSurfacesDict(afftool):
# get rotation matrices form configs # get rotation matrices form configs
def getRotationMatrixFromConfigs(configs): def getRotationMatrixFromConfigs(configs):
eigenpy.switchToNumpyMatrix()
R = [] R = []
for config in configs: for config in configs:
q = [0, 0, 0] + config[3:7] q = [0, 0, 0] + config[3:7]
...@@ -67,18 +66,18 @@ def getRotationMatrixFromConfigs(configs): ...@@ -67,18 +66,18 @@ def getRotationMatrixFromConfigs(configs):
# get contacted surface names at configuration # get contacted surface names at configuration
def getContactsNames(rbprmBuilder, i, q): def getContactsNames(rbprmBuilder, i, q):
if i % 2 == LF: # left leg if i % 2 == LF: # left leg
step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, ROBOT_NAME + '_lleg_rom') step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.lLegId)
elif i % 2 == RF: # right leg elif i % 2 == RF: # right leg
step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, ROBOT_NAME + '_rleg_rom') step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.rLegId)
return step_contacts return step_contacts
# get intersections with the rom and surface at configuration # get intersections with the rom and surface at configuration
def getContactsIntersections(rbprmBuilder, i, q): def getContactsIntersections(rbprmBuilder, i, q):
if i % 2 == LF: # left leg if i % 2 == LF: # left leg
intersections = rbprmBuilder.getContactSurfacesAtConfig(q, ROBOT_NAME + '_lleg_rom') intersections = rbprmBuilder.getContactSurfacesAtConfig(q, rbprmBuilder.lLegId)
elif i % 2 == RF: # right leg elif i % 2 == RF: # right leg
intersections = rbprmBuilder.getContactSurfacesAtConfig(q, ROBOT_NAME + '_rleg_rom') intersections = rbprmBuilder.getContactSurfacesAtConfig(q, rbprmBuilder.rLegId)
return intersections return intersections
...@@ -97,11 +96,11 @@ def getMergedPhases(seqs): ...@@ -97,11 +96,11 @@ def getMergedPhases(seqs):
def computeRootYawAngleBetwwenConfigs(q0, q1): def computeRootYawAngleBetwwenConfigs(q0, q1):
quat0 = Quaternion(q0[6], q0[3], q0[4], q0[5]) quat0 = Quaternion(q0[6], q0[3], q0[4], q0[5])
quat1 = Quaternion(q1[6], q1[3], q1[4], q1[5]) quat1 = Quaternion(q1[6], q1[3], q1[4], q1[5])
v_angular = np.matrix(log3(quat0.matrix().T * quat1.matrix())).T v_angular = np.array(log3(quat0.matrix() @ quat1.matrix()))
#print "q_prev : ",q0 #print ("q_prev : ",q0)
#print "q : ",q1 #print ("q : ",q1)
#print "v_angular = ",v_angular #print ("v_angular = ",v_angular)
return v_angular[2, 0] return v_angular[2]
def isYawVariationsInsideBounds(q0, q1, max_yaw=0.5): def isYawVariationsInsideBounds(q0, q1, max_yaw=0.5):
...@@ -164,7 +163,7 @@ def getSurfacesFromGuideContinuous(rbprmBuilder, ...@@ -164,7 +163,7 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,
phase_surfaces = [] phase_surfaces = []
for name in phase_contacts_names: for name in phase_contacts_names:
surface = surfaces_dict[name][0] surface = surfaces_dict[name][0]
if useIntersection and area(surface) > MAX_SURFACE: if useIntersection and area(surface) > max_surface_area:
if name in step_contacts: if name in step_contacts:
intersection = intersections[step_contacts.index(name)] intersection = intersections[step_contacts.index(name)]
if len(intersection) > 3: if len(intersection) > 3:
......
Markdown is supported
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