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)
n4 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # RH
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 = [40.0, 40.0, 40.0] # HAA, HFE, KFE
RF_tau_lim = [40.0, 40.0, 40.0] # HAA, HFE, KFE
LH_tau_lim = [40.0, 40.0, 40.0] # HAA, HFE, KFE
RH_tau_lim = [40.0, 40.0, 40.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
comp_dyn = ComputationalDynamics(robot_name)
'''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
def __init__(self):
self.compDyn = ComputationalDynamics()
class PathIterativeProjection:
def __init__(self):
self.compDyn = ComputationalDynamics()
def setup_path_iterative_projection(self, params):
''' parameters to be tuned'''
g = 9.81
ng = params.getNumberOfFrictionConesEdges();
proj, eq, ineq, actuation_polygons, isIKoutOfWorkSpace = self.compDyn.setup_iterative_projection(params, False)
if isIKoutOfWorkSpace:
lp = 0
else:
max_radius=1e5
(E, f), (A, b), (C, d) = proj, ineq, eq
assert E.shape[0] == f.shape[0] == 2
# Inequality constraints: A_ext * [ x u v ] <= b_ext iff
# (1) A * x <= b and (2) |u|, |v| <= max_radius
A_ext = zeros((A.shape[0] + 4, A.shape[1] + 2))
A_ext[:-4, :-2] = A
A_ext[-4, -2] = 1
A_ext[-3, -2] = -1
A_ext[-2, -1] = 1
A_ext[-1, -1] = -1
A_ext = cvxopt.matrix(A_ext)
b_ext = zeros(b.shape[0] + 4)
b_ext[:-4] = b
b_ext[-4:] = array([max_radius] * 4)
b_ext = cvxopt.matrix(b_ext)
# Equality constraints: C_ext * [ x u v ] == d_ext iff
# (1) C * x == d and (2) [ u v ] == E * x + f
C_ext = zeros((C.shape[0] + 2, C.shape[1] + 2))
C_ext[:-2, :-2] = C
C_ext[-2:, :-2] = E[:2]
C_ext[-2:, -2:] = array([[-1, 0], [0, -1]])
C_ext = cvxopt.matrix(C_ext)
d_ext = zeros(d.shape[0] + 2)
d_ext[:-2] = d
d_ext[-2:] = -f[:2]
d_ext = cvxopt.matrix(d_ext)
lp_obj = cvxopt.matrix(zeros(A.shape[1] + 2))
lp = lp_obj, A_ext, b_ext, C_ext, d_ext
# print 'act pol ',lp
# print 'a', actuation_polygons, trunk_mass, isIKoutOfWorkSpace
return lp, actuation_polygons/params.getTotalMass(), isIKoutOfWorkSpace
def optimize_direction_variable_constraint(self, lp, vdir, solver=GLPK_IF_AVAILABLE):
#print 'I am hereeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeee'
"""
Optimize in one direction.
Parameters
----------
vdir : (3,) array
Direction in which the optimization is performed.
lp : array tuple
Tuple `(q, G, h, A, b)` defining the LP. See
:func:`pypoman.lp..solve_lp` for details.
solver : string, optional
Backend LP solver to call.
Returns
-------
succ : bool
Success boolean.
z : (3,) array, or 0
Maximum vertex of the polygon in the direction `vdir`, or 0 in case of
solver failure.
"""
""" contact points """
lp_q, lp_Gextended, lp_hextended, lp_A, lp_b = lp
lp_q[-2] = -vdir[0]
lp_q[-1] = -vdir[1]
x = solve_lp(lp_q, lp_Gextended, lp_hextended, lp_A, lp_b, solver=solver)
tempSolution = x[-2:]
return tempSolution
#return comWorld[0:2], errorNorm
def optimize_angle_variable_constraint(self, lp, theta, solver=GLPK_IF_AVAILABLE):
"""
Optimize in one direction.
Parameters
----------
theta : scalar
Angle of the direction in which the optimization is performed.
lp : array tuple
Tuple `(q, G, h, A, b)` defining the LP. See
:func:`pypoman.lp..solve_lp` for details.
solver : string, optional
Backend LP solver to call.
Returns
-------
succ : bool
Success boolean.
z : (3,) array, or 0
Maximum vertex of the polygon in the direction `vdir`, or 0 in case of
solver failure.
"""
#print "Optimize angle!!!!!!!!!!!!!!!!!!!!!!"
d = array([cos(theta), sin(theta)])
z = self.optimize_direction_variable_constraint(lp, d, solver=solver)
return z
def compute_polygon_variable_constraint(self, params, max_iter=50, solver=GLPK_IF_AVAILABLE):
"""
Expand a polygon iteratively.
Parameters
----------
lp : array tuple
Tuple `(q, G, h, A, b)` defining the linear program. See
:func:`pypoman.lp.solve_lp` for details.
max_iter : integer, optional
Maximum number of calls to the LP solver.
solver : string, optional
Name of backend LP solver.
Returns
-------
poly : Polygon
Output polygon.
"""
total_mass = params.getTotalMass()
# mu = params.getFrictionCoeffcient()
# axisZ= array([[0.0], [0.0], [1.0]])
# math = Math()
# n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
# n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
# n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
# n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
# # %% Cell 2
# normals = np.vstack([n1, n2, n3, n4])
# normals = params.getNormals()
iterProj = PathIterativeProjection()
lp, actuation_polygons, isOutOfWorkspace = iterProj.setup_path_iterative_projection(params)
if isOutOfWorkspace:
return False
else:
two_pi = 2 * pi
theta = pi * random()
init_vertices = [self.optimize_angle_variable_constraint(lp, theta, solver)]
step = two_pi / 3
while len(init_vertices) < 3 and max_iter >= 0:
theta += step
if theta >= two_pi:
step *= 0.25 + 0.5 * random()
theta += step - two_pi
#comWorldFrame = np.array([0.0, 0.0, 0.0])
z = self.optimize_angle_variable_constraint(lp, theta, solver)
if all([norm(z - z0) > 1e-5 for z0 in init_vertices]):
init_vertices.append(z)
max_iter -= 1
if len(init_vertices) < 3:
raise Exception("problem is not linearly feasible")
v0 = Vertex(init_vertices[0])
v1 = Vertex(init_vertices[1])
v2 = Vertex(init_vertices[2])
polygon = Polygon()
polygon.from_vertices(v0, v1, v2)
polygon.iter_expand(lp, max_iter)
return polygon
def line(self, p1, p2):
A = (p1[1] - p2[1])
B = (p2[0] - p1[0])
C = (p1[0]*p2[1] - p2[0]*p1[1])
return A, B, -C
def two_lines_intersection(self, L1, L2):
D = L1[0] * L2[1] - L1[1] * L2[0]
Dx = L1[2] * L2[1] - L1[1] * L2[2]
Dy = L1[0] * L2[2] - L1[2] * L2[0]
if D != 0:
x = Dx / D
y = Dy / D
return x,y
else:
return False
def find_intersection(self, vertices_input, desired_direction, comWF):
desired_direction = desired_direction/np.linalg.norm(desired_direction)
#print "desired dir: ", desired_direction
desired_com_line = self.line(comWF, comWF+desired_direction)
#print "des line : ", desired_com_line
tmp_vertices = np.vstack([vertices_input, vertices_input[0]])
intersection_points = np.zeros((0,2))
points = np.zeros((0,2))
for i in range(0,len(vertices_input)):
v1 = tmp_vertices[i]
v2 = tmp_vertices[i+1]
actuation_region_edge = self.line(v1, v2)
#print desired_com_line, actuation_region_edge
new_point = self.two_lines_intersection(desired_com_line, actuation_region_edge)
#print "new point ", new_point
if new_point:
intersection_points = np.vstack([intersection_points, new_point])
else:
print "lines are parallel!"
while new_point is False:
desired_com_line = self.line(comWF, comWF+desired_direction+np.array([random()*0.01,random()*0.01,0.0]))
new_point = self.two_lines_intersection(desired_com_line, actuation_region_edge)
intersection_points = np.vstack([intersection_points, new_point])
# print new_point
#print intersection_points
epsilon = 0.0001;
if np.abs(desired_direction[0]- comWF[0]) > epsilon:
alpha_com_x_line = (new_point[0] - comWF[0])/(desired_direction[0]- comWF[0])
else:
alpha_com_x_line = 1000000000.0;
if np.abs(desired_direction[1]- comWF[1]) > epsilon:
alpha_com_y_line = (new_point[1] - comWF[1])/(desired_direction[1]- comWF[1])
else:
alpha_com_y_line = 1000000000.0
#print alpha_com_x_line, alpha_com_y_line
if alpha_com_x_line > 0.0 and alpha_com_y_line >= 0.0:
if np.abs(v2[0] - v1[0]) > epsilon:
alpha_vertices_x = (new_point[0] - v1[0])/(v2[0] - v1[0])
else:
alpha_vertices_x = 0.5
#print "alpha_vertices_x ", alpha_vertices_x
if alpha_vertices_x >= 0.0 and alpha_vertices_x <= 1.0:
if np.abs(v2[1] - v1[1]) > epsilon:
alpha_vertices_y = (new_point[1] - v1[1])/(v2[1] - v1[1])
else:
alpha_vertices_y = 0.5
#print "alpha vertx y ", alpha_vertices_y
if alpha_vertices_y >= 0.0 and alpha_vertices_y <= 1.0:
points = np.vstack([points, new_point])
elif np.abs(v2[1] - v1[1]):
alpha_vertices_y = (new_point[1] - v1[1])/(v2[1] - v1[1])
if alpha_vertices_y >= 0.0 and alpha_vertices_y <= 1.0:
points = np.vstack([points, new_point])
# print points
return points, intersection_points
def find_vertex_along_path(self, params, desired_direction, tolerance = 0.05, max_iteration_number = 10):
final_points = np.zeros((0,2))
newCoM = params.getCoMPosWF()
comWF = params.getCoMPosWF()
contactsBF = params.getContactsPosBF()
comToStack = np.zeros((0,3))
stackedIncrements = np.zeros((0,3))
increment = np.array([100.0, 100.0, 0.0])
while_iter = 0
polygon_to_stack = []
while (np.amax(np.abs(increment))>tolerance) and (while_iter<max_iteration_number):
comToStack = np.vstack([comToStack, newCoM])
params.setCoMPosWF(newCoM)
contactsBF_tmp = contactsBF - newCoM
params.setContactsPosBF(contactsBF_tmp)
polygon = self.compute_polygon_variable_constraint(params)
if polygon:
polygon.sort_vertices()
vertices_list = polygon.export_vertices()
vertices1 = [array([v.x, v.y]) for v in vertices_list]
polygon_to_stack.append(vertices1)
print while_iter
# print 'poly',vertices1
new_p, all_points = self.find_intersection(vertices1, desired_direction, comWF)
if np.size(new_p, 0)==0:
while_iter+= max_iteration_number
else:
final_points = np.vstack([final_points, new_p])
increment = np.hstack([new_p[0], 0.0]) - newCoM
stackedIncrements = np.vstack([stackedIncrements, increment])
newCoM = 0.2*increment + newCoM
while_iter += 1
else:
print "foot position is out of workspace!"
while_iter += max_iteration_number
print polygon_to_stack
return comToStack, stackedIncrements, polygon_to_stack
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()
import matplotlib.pyplot as plt
import matplotlib.colors as colors
import matplotlib.cm as cmx
from jet_leg.plotting.plotting_tools import Plotter
from jet_leg.maths.math_tools import Math
from jet_leg.dynamics.computational_dynamics import ComputationalDynamics
from jet_leg.map.height_map import HeightMap
from jet_leg.optimization.path_sequential_iterative_projection import PathIterativeProjection
from jet_leg.maths.iterative_projection_parameters import IterativeProjectionParameters
''' MAIN '''
start_t_IPVC = time.time()
math = Math()
compDyn = ComputationalDynamics()
pathIP = PathIterativeProjection()
# number of contacts
number_of_contacts = 4
# ONLY_ACTUATION, ONLY_FRICTION or FRICTION_AND_ACTUATION
constraint_mode = [
'FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION'
]
useVariableJacobian = True
total_mass = 100
mu = 0.8
terrain = HeightMap()
comWF = np.array([0.1, 0.1, 0.0])
n4 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # RH
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 = [40.0, 40.0, 40.0] # HAA, HFE, KFE
RF_tau_lim = [40.0, 40.0, 40.0] # HAA, HFE, KFE
LH_tau_lim = [40.0, 40.0, 40.0] # HAA, HFE, KFE
RH_tau_lim = [40.0, 40.0, 40.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
comp_dyn = ComputationalDynamics(robot_name)
'''You now need to fill the 'params' object with all the relevant
informations needed for the computation of the IP'''
params = IterativeProjectionParameters()
params.setContactsPosWF(contactsWF)
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
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