stanceLegs = [1, 1, 1, 1] randomSwingLeg = random.randint(0, 3) # print 'Swing leg', randomSwingLeg stanceLegs[randomSwingLeg] = 0 params.setContactsPosWF(contacts) params.setCoMPosWF(comWF) params.setTorqueLims(torque_limits) params.setActiveContacts(stanceLegs) params.setConstraintModes(constraint_mode_IP) params.setContactNormals(normals) params.setFrictionCoefficient(mu) params.setNumberOfFrictionConesEdges(ng) params.setTotalMass(total_mass) # IP_points, actuation_polygons, comp_time = comp_dyn.support_region_bretl(stanceLegs, contacts, normals, trunk_mass) IP_points, actuation_polygons, comp_time = comp_dyn.iterative_projection_bretl( params) comp_time = comp_time * 1000.0 print comp_time tests3contacts[iter] = comp_time nc = 4 comWF = np.array([0.0, 0.0, 0.0]) for iter in range(0, number_of_tests): ''' random normals ''' randRoll = np.random.normal(0.0, 0.2) randPitch = np.random.normal(0.0, 0.2) randYaw = np.random.normal(0.0, 0.2) n1 = np.transpose( np.transpose(math.rpyToRot(randRoll, randPitch, randYaw)).dot(axisZ))
params.setCoMPosWF(comWF) params.setTorqueLims(torque_limits) params.setActiveContacts(stanceFeet) params.setConstraintModes(constraint_mode_IP) params.setContactNormals(normals) params.setFrictionCoefficient(mu) params.setNumberOfFrictionConesEdges(ng) params.setTotalMass(trunk_mass) params.externalForceWF = extForceW # params.externalForceWF is actually used anywhere at the moment ''' compute iterative projection Outputs of "iterative_projection_bretl" are: IP_points = resulting 2D vertices actuation_polygons = these are the vertices of the 3D force polytopes (one per leg) computation_time = how long it took to compute the iterative projection ''' IP_points, force_polytopes, IP_computation_time = comp_dyn.iterative_projection_bretl( params) #print "Inequalities", comp_dyn.ineq #print "actuation polygons" #print actuation_polygons '''I now check whether the given CoM configuration is stable or not''' isConfigurationStable, contactForces, forcePolytopes = comp_dyn.check_equilibrium( params) print isConfigurationStable print 'contact forces', contactForces '''Plotting the contact points in the 3D figure''' fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.set_xlabel('X axis') ax.set_ylabel('Y axis') ax.set_zlabel('Z axis')
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