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()
constraint_mode_IP = [
'FRICTION_AND_ACTUATION', 'ONLY_FRICTION', 'ONLY_ACTUATION',
'FRICTION_AND_ACTUATION'
]
useVariableJacobian = False
# 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)
possible constraints for each foot:
ONLY_ACTUATION = only joint-torque limits are enforces
ONLY_FRICTION = only friction cone constraints are enforced
FRICTION_AND_ACTUATION = both friction cone constraints and joint-torque limits
'''
constraint_mode_IP = [
'FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION'
]
# number of decision variables of the problem
# n = nc*6
comWF = np.array([0.0, -0.0, 0.0])
''' Set the robot's name (either 'hyq', 'hyqreal' or 'anymal')'''
robot_name = 'anymal'
comp_dyn = ComputationalDynamics(robot_name)
stackedForcePolytopesLF = np.zeros((3, 8))
stackedFootPosLF = []
for lf_x in np.arange(-0.2, 0.2, 0.1):
""" contact points in the World Frame"""
LF_foot = np.array([lf_x, 0.2, -0.4])
RF_foot = np.array([0.3, -0.2, -0.4])
LH_foot = np.array([-0.3, 0.2, -0.4])
RH_foot = np.array([-0.3, -0.2, -0.4])
print LF_foot
contacts = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot))
# contacts = contactsToStack[0:nc+1, :]
# print contacts
''' parameters to be tuned'''
math = Math()
print("''' ONLY_FRICTION, 3 CONTACTS'''")
# number of generators, i.e. rays used to linearize the friction cone
ng = 4
useVariableJacobian = False
''' parameters to be tuned'''
g = 9.81
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)
def __init__(self):
self.compDyn = ComputationalDynamics()
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()