def talker(robotName):
compDyn = ComputationalDynamics(robotName)
footHoldPlanning = FootHoldPlanning(robotName)
math = Math()
p = HyQSim()
p.start()
p.register_node()
name = "Actuation_region"
force_polytopes_name = "force_polytopes"
params = IterativeProjectionParameters()
foothold_params = FootholdPlanningInterface()
i = 0
p.get_sim_wbs()
params.getParamsFromRosDebugTopic(p.hyq_debug_msg)
foothold_params.getParamsFromRosDebugTopic(p.hyq_debug_msg)
params.getFutureStanceFeetFlags(p.hyq_debug_msg)
""" contact points """
ng = 4
params.setNumberOfFrictionConesEdges(ng)
while not ros.is_shutdown():
print 'CIAOOOOOOO'
p.get_sim_wbs()
params.getParamsFromRosDebugTopic(p.hyq_debug_msg)
foothold_params.getParamsFromRosDebugTopic(p.hyq_debug_msg)
#params.getFutureStanceFeet(p.hyq_debug_msg)
params.getCurrentStanceFeetFlags(p.hyq_debug_msg)
# USE THIS ONLY TO PLOT THE ACTUAL REGION FOR A VIDEO FOR THE PAPER DO NOT USE FOR COM PLANNING
params.setConstraintModes([
'FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION'
])
IAR, actuation_polygons_array, computation_time = compDyn.try_iterative_projection_bretl(
params)
# print 'feasible region', IAR
# if IAR is not False:
# p.send_actuation_polygons(name, p.fillPolygon(IAR), foothold_params.option_index, foothold_params.ack_optimization_done)
# old_IAR = IAR
# else:
# print 'Could not compute the feasible region'
# p.send_actuation_polygons(name, p.fillPolygon(old_IAR), foothold_params.option_index,
#
# foothold_params.ack_optimization_done)
##
p.send_actuation_polygons(name, p.fillPolygon(IAR),
foothold_params.option_index,
foothold_params.ack_optimization_done)
constraint_mode_IP = 'ONLY_FRICTION'
params.setConstraintModes([
constraint_mode_IP, constraint_mode_IP, constraint_mode_IP,
constraint_mode_IP
])
params.setNumberOfFrictionConesEdges(ng)
params.contactsWF[params.actual_swing] = foothold_params.footOptions[
foothold_params.option_index]
# uncomment this if you dont want to use the vars read in iterative_proJ_params
# params.setContactNormals(normals)
# params.setFrictionCoefficient(mu)
# params.setTrunkMass(trunk_mass)
# IP_points, actuation_polygons, comp_time = comp_dyn.support_region_bretl(stanceLegs, contacts, normals, trunk_mass)
frictionRegion, actuation_polygons, computation_time = compDyn.iterative_projection_bretl(
params)
p.send_support_region(name, p.fillPolygon(frictionRegion))
#print "AA"
#1 - INSTANTANEOUS FEASIBLE REGION
# ONLY_ACTUATION, ONLY_FRICTION or FRICTION_AND_ACTUATION
#IAR, actuation_polygons_array, computation_time = compDyn.iterative_projection_bretl(params)
#print 'feasible region', IAR,
#p.send_actuation_polygons(name, p.fillPolygon(IAR), foothold_params.option_index, foothold_params.ack_optimization_done)
#2 - FORCE POLYGONS
#point = Point()
#polygonVertex = Point()
#polygon = Polygon3D()
# point.x = actuation_polygons_array[0][0][0]/1000.0
# point.y = actuation_polygons_array[0][1][0]/1000.0
# point.z = actuation_polygons_array[0][2][0]/1000.0
# forcePolygons = []
# for i in range(0,4):
# singlePolygon = Polygon3D()
## print actuation_polygons_array[i]
# vertices = []
# for j in range(0,8):
# vx = Point()
# vx.x = actuation_polygons_array[i][0][j]/1000.0
# vx.y = actuation_polygons_array[i][1][j]/1000.0
# vx.z = actuation_polygons_array[i][2][j]/1000.0
# vertices = np.hstack([vertices, vx])
# singlePolygon.vertices = vertices
# forcePolygons = np.hstack([forcePolygons, singlePolygon])
# p.send_force_polytopes(force_polytopes_name, forcePolygons)
#4 - FOOTHOLD PLANNING
#print 'opt started?', foothold_params.optimization_started
#print 'ack opt done', foothold_params.ack_optimization_done
# foothold_params.ack_optimization_done = True
actuationRegions = []
# print 'robot mass', params.robotMass
if (foothold_params.optimization_started == False):
foothold_params.ack_optimization_done = False
''' The optimization-done-flag is set by the planner. It is needed to tell the controller whether the optimization
is finished or not. When this flag is true the controller will read the result of the optimization that has read
from the planner'''
print 'optimization done flag', foothold_params.ack_optimization_done
''' The optimization-started-flag is set by the controller. It is needed to tell the planner that a new optimization should start.
When this flag is true the planner (in jetleg) will start a new computation of the feasible region.'''
print 'optimization started flag', foothold_params.optimization_started
if foothold_params.optimization_started and not foothold_params.ack_optimization_done:
print '============================================================'
print 'current swing ', params.actual_swing
print '============================================================'
#print foothold_params.footOptions
#chosen_foothold, actuationRegions = footHoldPlanning.selectMaximumFeasibleArea(foothold_params, params)
# print 'current swing ',params.actual_swing
foothold_params.option_index, stackedResidualRadius, actuationRegions, mapFootHoldIdxToPolygonIdx = footHoldPlanning.selectMaximumFeasibleArea(
foothold_params, params)
if actuationRegions is False:
foothold_params.option_index = -1
else:
print 'min radius ', foothold_params.minRadius, 'residual radius ', stackedResidualRadius
#print 'feet options', foothold_params.footOptions
print 'final index', foothold_params.option_index, 'index list', mapFootHoldIdxToPolygonIdx
foothold_params.ack_optimization_done = 1
# ONLY_ACTUATION, ONLY_FRICTION or FRICTION_AND_ACTUATION
# 3 - FRICTION REGION
constraint_mode_IP = 'ONLY_FRICTION'
params.setConstraintModes([
constraint_mode_IP, constraint_mode_IP, constraint_mode_IP,
constraint_mode_IP
])
params.setNumberOfFrictionConesEdges(ng)
params.contactsWF[
params.actual_swing] = foothold_params.footOptions[
foothold_params.option_index]
# uncomment this if you dont want to use the vars read in iterative_proJ_params
# params.setContactNormals(normals)
# params.setFrictionCoefficient(mu)
# params.setTrunkMass(trunk_mass)
# IP_points, actuation_polygons, comp_time = comp_dyn.support_region_bretl(stanceLegs, contacts, normals, trunk_mass)
frictionRegion, actuation_polygons, computation_time = compDyn.iterative_projection_bretl(
params)
print 'friction region is: ', frictionRegion
p.send_support_region(name, p.fillPolygon(frictionRegion))
#this sends the data back to ros that contains the foot hold choice (used for stepping) and the corrspondent region (that will be used for com planning TODO update with the real footholds)
if (actuationRegions is not False) and (np.size(actuationRegions)
is not 0):
print 'sending actuation region'
p.send_actuation_polygons(
name, p.fillPolygon(actuationRegions[-1]),
foothold_params.option_index,
foothold_params.ack_optimization_done)
# print actuationRegions[-1]
else:
#if it cannot compute anything it will return the frictin region
p.send_actuation_polygons(
name, p.fillPolygon(frictionRegion),
foothold_params.option_index,
foothold_params.ack_optimization_done)
time.sleep(0.1)
i += 1
print 'de registering...'
p.deregister_node()
class FootHoldPlanning:
def __init__(self, robot_name):
self.robotName = robot_name
self.compGeo = ComputationalGeometry()
self.compDyn = ComputationalDynamics(self.robotName)
self.footPlanning = FootholdPlanningInterface()
self.math = Math()
self.dog = DogInterface()
self.rbd = RigidBodyDynamics()
def selectMaximumFeasibleArea(self, footPlanningParams, params):
params.setCoMPosWF(footPlanningParams.com_position_to_validateW)
# print "com pos to validate" , params.com_position_to_validateW
# print "sample contacts" , params.sample_contacts
# footPlanningParams.numberOfFeetOptions = np.size(footPlanningParams.footOptions,0)
print 'number of feet options ',footPlanningParams.numberOfFeetOptions
# print numberOfFeetOptions
feasible_regions = []
area = []
for i in range(0, footPlanningParams.numberOfFeetOptions):
#these two lines go together to overwrite the future swing foot
params.contactsWF[params.actual_swing] = footPlanningParams.footOptions[i]
IAR, actuation_polygons_array, computation_time = self.compDyn.iterative_projection_bretl(params)
# print 'IAR', IAR
d = self.math.find_residual_radius(IAR, footPlanningParams.com_position_to_validateW)
print 'residual radius', d
feasible_regions.append(IAR)
# print 'FR', feasible_regions
area.append( self.compGeo.computePolygonArea(IAR))
print 'area ', area
print 'max arg ',np.argmax(np.array(area), axis=0)
return np.argmax(np.array(area), axis=0), feasible_regions
def selectMinumumRequiredFeasibleAreaResidualRadius(self, footPlanningParams, params):
ng = 4
params.setConstraintModes(['FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION'])
params.setNumberOfFrictionConesEdges(ng)
params.setCoMPosWF(footPlanningParams.com_position_to_validateW)
# print numberOfFeetOptions
feasible_regions = []
residualRadiusToStack = []
# print 'empty res radii', residualRadiusToStack
# footOptions = []
area = []
mapFootHoldIdxToPolygonIdx = []
# counter = 0
print 'number of feet options ',footPlanningParams.numberOfFeetOptions
numberOfOptions = footPlanningParams.numberOfFeetOptions
#check the prediction point at the beginning
if numberOfOptions > 0:
foothold_index = int((numberOfOptions -1)/2.0) #assumes numberOfOptions is odd
# print 'initial foothold index', foothold_index
#these two lines go together to overwrite the future swing foot
params.contactsWF[params.actual_swing] = footPlanningParams.footOptions[foothold_index]
# print 'contacts WF', params.contactsWF
# print 'com pos WF', params.getCoMPosWF()
IAR, actuation_polygons_array, computation_time = self.compDyn.try_iterative_projection_bretl(params)
# print 'IAR', IAR
if IAR is False:
return False, False, False, False
else:
residualRadius = deepcopy(self.math.find_residual_radius(IAR, footPlanningParams.com_position_to_validateW))
area.append( self.compGeo.computePolygonArea(IAR))
mapFootHoldIdxToPolygonIdx.append(foothold_index)
# footOptions.append(deepcopy(params.contactsWF[params.actual_swing] ))
feasible_regions.append(IAR)
residualRadiusToStack.append(residualRadius)
if residualRadius < footPlanningParams.minRadius:
gradient, searchDirection, residualRadius, foothold_index, residualRadiusToStack, feasible_regions, mapFootHoldIdxToPolygonIdx = self.compute_search_direciton(params, footPlanningParams, residualRadius, foothold_index, area,
feasible_regions, mapFootHoldIdxToPolygonIdx, residualRadiusToStack)
else:
gradient = False
if gradient is not False:
# print 'gradient before while', gradient
foothold_index += searchDirection
params.contactsWF[params.actual_swing] = footPlanningParams.footOptions[foothold_index]
# print 'number of option',numberOfOptions
#move along the grid to find the feasible point
while ((gradient > 0.0) and (residualRadius < footPlanningParams.minRadius) and (foothold_index > 0) and (foothold_index < numberOfOptions-1)):
#these two lines go together to overwrite the future swing foot
params.contactsWF[params.actual_swing] = footPlanningParams.footOptions[foothold_index+searchDirection]
IAR, actuation_polygons_array, computation_time = self.compDyn.try_iterative_projection_bretl(params)
if IAR is False:
residualRadius = 0.0
newArea = 0.0
else:
residualRadius = self.math.find_residual_radius(IAR, footPlanningParams.com_position_to_validateW)
newArea = self.compGeo.computePolygonArea(IAR)
oldArea = area[-1]
oldResidualRadius = residualRadiusToStack[-1]
# print 'old residual radius', oldResidualRadius
# gradient = residualRadius - oldResidualRadius
gradient = newArea - oldArea
# print 'area gradient ', gradient
# gradient = (residualRadius - newResidualRadius)/gridResolution
if gradient > 0:
foothold_index += searchDirection
mapFootHoldIdxToPolygonIdx.append(foothold_index)
feasible_regions.append(IAR)
residualRadiusToStack.append(residualRadius)
area.append(newArea)
print 'area ', area
else:
foothold_index = -1
# feasible_regions = false
# print 'res radii', residualRadiusToStack
# print 'foothold index ', foothold_index
# footPlanningParams.option_index = foothold_index
return foothold_index, residualRadiusToStack, feasible_regions, mapFootHoldIdxToPolygonIdx
def compute_search_direciton(self, params, footPlanningParams, residualRadius, foothold_index, area, feasible_regions, mapFootHoldIdxToPolygonIdx, residualRadiusToStack):
# check the fist point after and before the heuristic one along the direction
# these two lines go together to overwrite the future swing foot
if foothold_index < footPlanningParams.numberOfFeetOptions:
params.contactsWF[params.actual_swing] = footPlanningParams.footOptions[foothold_index + 1]
#print "residualRadius, params.actual_swing, foothold_index, params.contactsWF", residualRadius, params.actual_swing, foothold_index, params.contactsWF
IAR1, actuation_polygons_array, computation_time = self.compDyn.try_iterative_projection_bretl(params)
newResidualRadius1 = deepcopy(
self.math.find_residual_radius(IAR1, footPlanningParams.com_position_to_validateW))
searchDirection1 = +1
area.append(self.compGeo.computePolygonArea(IAR1))
# footOptions.append(deepcopy(params.contactsWF[params.actual_swing]))
feasible_regions.append(IAR1)
mapFootHoldIdxToPolygonIdx.append(foothold_index + 1)
residualRadiusToStack.append(newResidualRadius1)
else:
newResidualRadius1 = 0.0
# these two lines go together to overwrite the future swing foot
params.contactsWF[params.actual_swing] = footPlanningParams.footOptions[foothold_index - 1]
IAR2, actuation_polygons_array, computation_time = self.compDyn.try_iterative_projection_bretl(params)
if IAR2 is not False:
if foothold_index > 0:
newResidualRadius2 = deepcopy(
self.math.find_residual_radius(IAR2, footPlanningParams.com_position_to_validateW))
searchDirection2 = -1
area.append(self.compGeo.computePolygonArea(IAR2))
mapFootHoldIdxToPolygonIdx.append(foothold_index - 1)
# footOptions.append(deepcopy(params.contactsWF[params.actual_swing]))
feasible_regions.append(IAR2)
residualRadiusToStack.append(newResidualRadius2)
else:
newResidualRadius2 = 0.0
else:
newResidualRadius2 = 0.0
# print 'RADS', residualRadius, newResidualRadius1, newResidualRadius2
if (newResidualRadius1 > (residualRadius + footPlanningParams.TOL)) and (
newResidualRadius1 > (newResidualRadius2 + footPlanningParams.TOL)):
searchDirection = searchDirection1
gradient = newResidualRadius1 - residualRadius
residualRadius = newResidualRadius1
elif (newResidualRadius2 > (residualRadius + footPlanningParams.TOL)) and (
newResidualRadius2 > (newResidualRadius1 + footPlanningParams.TOL)):
searchDirection = searchDirection2
gradient = newResidualRadius2 - residualRadius
residualRadius = newResidualRadius2
else: # you are already in the max
searchDirection = 0
# print 'final foothold index', foothold_index
print 'RETURN before entering while loop'
gradient = False
return gradient, searchDirection, residualRadius, foothold_index, residualRadiusToStack, feasible_regions, mapFootHoldIdxToPolygonIdx
return gradient, searchDirection, residualRadius, foothold_index, residualRadiusToStack, feasible_regions, mapFootHoldIdxToPolygonIdx
def selectMaximumFeasibleArea(self, footPlanningParams, params):
ng = 4
params.setConstraintModes(['FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION'])
params.setNumberOfFrictionConesEdges(ng)
params.setCoMPosWF(footPlanningParams.com_position_to_validateW)
# print numberOfFeetOptions
feasible_regions = []
residualRadiusToStack = []
# print 'empty res radii', residualRadiusToStack
# footOptions = []
area = []
mapFootHoldIdxToPolygonIdx = []
# counter = 0
print 'number of feet options ', footPlanningParams.numberOfFeetOptions
numberOfOptions = footPlanningParams.numberOfFeetOptions
print footPlanningParams.footOptions
# check the prediction point at the beginning
if numberOfOptions > 0:
for footIndex in range(0, int(numberOfOptions)):
# these two lines go together to overwrite the future swing foot
params.contactsWF[params.actual_swing] = footPlanningParams.footOptions[footIndex]
IAR, actuation_polygons_array, computation_time = self.compDyn.try_iterative_projection_bretl(params)
if IAR is False:
residualRadius = 0.0
newArea = 0.0
else:
residualRadius = self.math.find_residual_radius(IAR,
footPlanningParams.com_position_to_validateW)
newArea = self.compGeo.computePolygonArea(IAR)
mapFootHoldIdxToPolygonIdx.append(footIndex)
feasible_regions.append(IAR)
residualRadiusToStack.append(residualRadius)
area.append(newArea)
print 'area ', area
if np.size(area, 0) > 0:
maxFootIndex = np.argmax(area)
else:
maxFootIndex = -1
print 'max foothold: ', maxFootIndex
else:
maxFootIndex = -1
# feasible_regions = false
# print 'res radii', residualRadiusToStack
# print 'foothold index ', foothold_index
# footPlanningParams.option_index = foothold_index
return maxFootIndex, residualRadiusToStack, feasible_regions, mapFootHoldIdxToPolygonIdx