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

Merge pull request #65 from pFernbach/topic/surface_extraction

fix surface extraction from guide
parents bcfad6ea 92facc79
......@@ -7,12 +7,15 @@ class PathPlanner(TalosPathPlanner):
# select ROM model with really conservative ROM shapes
Robot.urdfName = "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.rbprmBuilder = Robot()
# 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('talos_rleg_rom_reduced', self.rbprmBuilder.ref_EE_rLeg)
self.rbprmBuilder.setReferenceEndEffector(Robot.lLegId, self.rbprmBuilder.ref_EE_lLeg)
self.rbprmBuilder.setReferenceEndEffector(Robot.rLegId, self.rbprmBuilder.ref_EE_rLeg)
def init_problem(self):
super().init_problem()
......@@ -32,10 +35,11 @@ class PathPlanner(TalosPathPlanner):
self.root_translation_bounds = [-1.5, 3, 0., 3.3, 0.95, 2.]
self.init_problem()
self.q_init[:3] = [0.05, 1.2, 0.98]
self.q_goal[:3] = [1.87, 1.2, 1.58]
self.q_init[:2] = [0., 1.2]
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.solve()
self.display_path()
......
......@@ -26,10 +26,12 @@ class PathPlanner(TalosPathPlanner):
Robot.urdfName = "talos_trunk_large_reducedROM"
self.robot_node_name = "talos_trunk_large_reducedROM"
# 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.setReferenceEndEffector('talos_lleg_rom_reduced', self.rbprmBuilder.ref_EE_lLeg)
self.rbprmBuilder.setReferenceEndEffector('talos_rleg_rom_reduced', self.rbprmBuilder.ref_EE_rLeg)
self.rbprmBuilder.setReferenceEndEffector(Robot.lLegId, self.rbprmBuilder.ref_EE_lLeg)
self.rbprmBuilder.setReferenceEndEffector(Robot.rLegId, self.rbprmBuilder.ref_EE_rLeg)
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
......
......@@ -37,12 +37,14 @@ class PathPlanner(TalosPathPlanner):
# select ROM model with really conservative ROM shapes
Robot.urdfName = "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.rbprmBuilder = Robot()
# 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('talos_rleg_rom_reduced', self.rbprmBuilder.ref_EE_rLeg)
self.rbprmBuilder.setReferenceEndEffector(Robot.lLegId, self.rbprmBuilder.ref_EE_lLeg)
self.rbprmBuilder.setReferenceEndEffector(Robot.rLegId, self.rbprmBuilder.ref_EE_rLeg)
def set_random_configs(self):
from hpp.corbaserver.rbprm.tools.sampleRotation import sampleRotationForConfig
......
......@@ -5,7 +5,7 @@ from hpp.corbaserver.rbprm.tools.display_tools import displaySurfaceFromPoints
import numpy as np
from pinocchio import Quaternion, log3
import eigenpy
eigenpy.switchToNumpyMatrix()
eigenpy.switchToNumpyArray()
ROBOT_NAME = 'talos'
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):
# get rotation matrices form configs
def getRotationMatrixFromConfigs(configs):
eigenpy.switchToNumpyMatrix()
R = []
for config in configs:
q = [0, 0, 0] + config[3:7]
......@@ -67,18 +66,18 @@ def getRotationMatrixFromConfigs(configs):
# get contacted surface names at configuration
def getContactsNames(rbprmBuilder, i, q):
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
step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, ROBOT_NAME + '_rleg_rom')
step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.rLegId)
return step_contacts
# get intersections with the rom and surface at configuration
def getContactsIntersections(rbprmBuilder, i, q):
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
intersections = rbprmBuilder.getContactSurfacesAtConfig(q, ROBOT_NAME + '_rleg_rom')
intersections = rbprmBuilder.getContactSurfacesAtConfig(q, rbprmBuilder.rLegId)
return intersections
......@@ -97,11 +96,11 @@ def getMergedPhases(seqs):
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 = np.matrix(log3(quat0.matrix().T * quat1.matrix())).T
#print "q_prev : ",q0
#print "q : ",q1
#print "v_angular = ",v_angular
return v_angular[2, 0]
v_angular = np.array(log3(quat0.matrix() @ quat1.matrix()))
#print ("q_prev : ",q0)
#print ("q : ",q1)
#print ("v_angular = ",v_angular)
return v_angular[2]
def isYawVariationsInsideBounds(q0, q1, max_yaw=0.5):
......@@ -164,7 +163,7 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,
phase_surfaces = []
for name in phase_contacts_names:
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:
intersection = intersections[step_contacts.index(name)]
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