# number of decision variables of the problem
n = nc * 6
''' parameters to be tuned'''
g = 9.81
total_mass = 85.
mu = 0.8
axisZ = array([[0.0], [0.0], [1.0]])
comp_dyn = ComputationalDynamics()
number_of_tests = 10
tests3contacts = np.zeros((number_of_tests))
tests4contacts = np.zeros((number_of_tests))
params = IterativeProjectionParameters()
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))
randRoll = np.random.normal(0.0, 0.2)
randPitch = np.random.normal(0.0, 0.2)
randYaw = np.random.normal(0.0, 0.2)
n2 = np.transpose(
np.transpose(math.rpyToRot(randRoll, randPitch, randYaw)).dot(axisZ))
randRoll = np.random.normal(0.0, 0.2)
randPitch = np.random.normal(0.0, 0.2)
total_mass = 85.
mu = 0.8
axisZ = array([[0.0], [0.0], [1.0]])
comp_dyn = ComputationalDynamics('anymal')
number_of_tests = 1000
onlyFrictionTests3contacts = np.zeros((number_of_tests))
onlyFrictionTests4contacts = np.zeros((number_of_tests))
onlyActuationTests3contacts = np.zeros((number_of_tests))
onlyActuationTests4contacts = np.zeros((number_of_tests))
frictionAndActuation3contacts = np.zeros((number_of_tests))
frictionAndActuation4contacts = np.zeros((number_of_tests))
params = IterativeProjectionParameters()
def perform_statistics(number_of_tests, number_of_contacts, _constraint_mode):
computation_times = np.zeros((number_of_tests))
params.setConstraintModes(constraint_mode_IP)
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))
randRoll = np.random.normal(0.0, 0.2)
randPitch = np.random.normal(0.0, 0.2)
normals = np.vstack([n1, n2, n3, n4])
''' torque limits for each leg (this code assumes a hyq-like design, i.e. three joints per leg)
HAA = Hip Abduction Adduction
HFE = Hip Flextion Extension
KFE = Knee Flextion Extension
'''
LF_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE
RF_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE
LH_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE
RH_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE
torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim])
''' extForceW is an optional external pure force (no external torque for now) applied on the CoM of the robot.'''
extForceW = np.array([0.0, 0.0, 0.0]) # units are Nm
'''You now need to fill the 'params' object with all the relevant
informations needed for the computation of the IP'''
params = IterativeProjectionParameters()
params.setContactsPosWF(contacts)
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
# print "Inequalities", comp_dyn.ineq
# print "actuation polygons"
# print actuation_polygons
'''I now check whether the given CoM configuration is stable or not'''
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()