def constraints(self, x):
#
# The callback for calculating the constraints
#
math = Math()
pos = x[0:3]
force = x[3:6]
tau_constraint = np.dot(math.skew(pos), force)
return np.array((tau_constraint))
def test_square_line_intersection_random_values(self):
math = Math()
for iter in range(0, 10):
starting_point = np.array([
np.random.randint(-100, 100) / 100.0,
np.random.randint(-100, 100) / 100.0, 0.0
])
#starting_point = np.array([0.0,0.0,0.0])
#print "starting point is: ",starting_point
#angle = np.random.randint(0,600)/600.0
search_direction = np.array([1.0, 1.0, 0.0])
vertices = np.array([[1.0, 1.0, 0.0], [1.0, -1.0, 0.0],
[-1.0, -1.0, 0.0], [-1.0, 1.0, 0.0]])
new_p, all_points = math.find_polygon_segment_intersection(
vertices, search_direction, starting_point)
expected_intersection_point = np.array([1.0, 1.0])
self.assertTrue((new_p[1] >= -new_p[0]).all())
for iter in range(0, 10):
starting_point = np.array([
np.random.randint(-100, 100) / 100.0,
np.random.randint(-100, 100) / 100.0, 0.0
])
#starting_point = np.array([0.0,0.0,0.0])
#print "starting point is: ",starting_point
#angle = np.random.randint(0,600)/600.0
search_direction = np.array([-1.0, -1.0, 0.0])
vertices = np.array([[1.0, 1.0, 0.0], [1.0, -1.0, 0.0],
[-1.0, -1.0, 0.0], [-1.0, 1.0, 0.0]])
new_p, all_points = math.find_polygon_segment_intersection(
vertices, search_direction, starting_point)
expected_intersection_point = np.array([1.0, 1.0])
self.assertTrue((new_p[1] <= -new_p[0]).all())
for iter in range(0, 10):
starting_point = np.array([
np.random.randint(-100, 100) / 100.0,
np.random.randint(-100, 100) / 100.0, 0.0
])
#starting_point = np.array([0.0,0.0,0.0])
#print "starting point is: ",starting_point
#angle = np.random.randint(0,600)/600.0
search_direction = np.array([1.0, 0.0, 0.0])
vertices = np.array([[1.0, 1.0, 0.0], [1.0, -1.0, 0.0],
[-1.0, -1.0, 0.0], [-1.0, 1.0, 0.0]])
new_p, all_points = math.find_polygon_segment_intersection(
vertices, search_direction, starting_point)
expected_intersection_point = np.array([1.0, 1.0])
self.assertTrue((new_p[0] > 0).all())
def test_2D_and_3D_jacobians_and_actuation_polygons(self):
math = Math()
random.seed(9001)
constraint_mode = 'ONLY_ACTUATION'
nc = 1
ng = 4
friction_coeff = 0.6
axisZ = np.array([[0.0], [0.0], [1.0]])
n1 = np.transpose(
np.transpose(math.rpyToRot(1.5, 1.5, 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])
foot_vel = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]])
LF_foot = np.array([0.3, 0.2, -0.5])
RF_foot = np.array([0.3, -0.2, -0.5])
LH_foot = np.array([-0.2, 0.2, -0.5])
RH_foot = np.array([-0.3, -0.2, -0.5])
contacts = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot))
constraint = Constraints()
kin = HyQKinematics()
q, q_dot, J_LF, J_RF, J_LH, J_RH, isOutOfWorkspace = kin.inverse_kin(
np.transpose(contacts[:, 0]), np.transpose(foot_vel[:, 0]),
np.transpose(contacts[:, 1]), np.transpose(foot_vel[:, 1]),
np.transpose(contacts[:, 2]), np.transpose(foot_vel[:, 2]))
if (not isOutOfWorkspace):
kin.update_jacobians(q)
#print J_LF
G, h, isConstraintOk, actuationPolygons = constraint.getInequalities(
constraint_mode, nc, ng, normals, friction_coeff, J_LF, J_RF,
J_LH, J_RH)
J_LF_3D, J_RF_3D, J_LH_3D, J_RH_3D = kin.update_jacobians(q)
if (not isOutOfWorkspace):
kin.update_jacobians(q)
#print J_LF
G_new, h_new, isConstraintOk, actuationPolygons = constraint.getInequalities(
constraint_mode, nc, ng, normals, friction_coeff, J_LF_3D,
J_RF_3D, J_LH_3D, J_RH_3D)
for j in range(0, len(h)):
self.assertTrue(h[j] - h_new[j] < self.epsilon)
for i in range(0, np.size(G, 1)):
#print G[j,i]/h[j]
#print G_new[j,i]/h_new[j]
self.assertTrue(
G[j, i] / h[j] - G_new[j, i] / h_new[j] < self.epsilon)
def optimize_direction_variable_constraints(self, constraint_mode,
desired_direction, contacts,
comWF):
final_points = np.zeros((0, 2))
newCoM = comWF
comToStack = np.zeros((0, 3))
increment = np.array([100.0, 100.0, 0.0])
while_iter = 0
math = Math()
#print "enter while loop"
while (np.amax(np.abs(increment)) > 0.01) and (while_iter < 100):
comToStack = np.vstack([comToStack, newCoM])
polygon = self.compute_polygon_variable_constraint(
constraint_mode, newCoM, contacts)
#print polygon.export_vertices()
if polygon:
polygon.sort_vertices()
vertices_list = polygon.export_vertices()
vertices1 = [array([v.x, v.y]) for v in vertices_list]
new_p, all_points = math.find_polygon_segment_intersection(
vertices1, desired_direction, comWF)
final_points = np.vstack([final_points, new_p])
#print "new com: ", newCoM
#print "new p ", new_p, np.size(new_p)
if (np.size(new_p) == 0):
print "intersections not found!!"
else:
increment = np.stack([new_p[0], new_p[1], 0.0]) - newCoM
newCoM = 0.2 * increment + newCoM
while_iter += 1
else:
print "foot position is out of workspace!"
while_iter += 10
return comToStack
def test_3D_jacobians(self):
math = Math()
random.seed(9001)
constraint_mode = 'ONLY_ACTUATION'
nc = 1
ng = 4
friction_coeff = 0.6
axisZ = np.array([[0.0], [0.0], [1.0]])
n1 = np.transpose(
np.transpose(math.rpyToRot(1.5, 1.5, 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])
J_LF = np.eye((3))
J_RF = np.eye((3))
J_LH = np.eye((3))
J_RH = np.eye((3))
constraint = Constraints()
G, h, isConstraintOk, actuationPolygons = constraint.getInequalities(
constraint_mode, nc, ng, normals, friction_coeff, J_LF, J_RF, J_LH,
J_RH)
#print G, h
for j in range(0, len(h)):
self.assertTrue(h[j] > self.epsilon)
self.assertTrue(h[0] / G[0, 2] - 120.0 < self.epsilon)
self.assertTrue(h[2] / G[2, 0] - 80.0 < self.epsilon)
self.assertTrue(h[4] / G[3, 1] - 120.0 < self.epsilon)
def talker():
compDyn = ComputationalDynamics()
math = Math()
p = HyQSim()
p.start()
p.register_node()
name = "Actuation_region"
point = Point()
actuationParams = ActuationParameters()
i = 0
start_t_IP = time.time()
for j in range(0, 1000):
vertices = [point]
# print("Time: " + str(i*0.004) + "s and Simulation time: " + str(p.get_sim_time()/60))
p.get_sim_wbs()
actuationParams.getParams(p.hyq_rcf_debug)
trunk_mass = 85.
axisZ = np.array([[0.0], [0.0], [1.0]])
''' normals '''
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))
normals = np.vstack([n1, n2, n3])
""" contact points """
nc = actuationParams.numberOfContacts
contacts = actuationParams.contacts[0:nc + 1, :]
print 'contacts number: ', actuationParams.numberOfContacts
print contacts, actuationParams.stanceFeet
IAR, actuation_polygons, computation_time = compDyn.instantaneous_actuation_region_bretl(
actuationParams.stanceFeet, contacts, normals, trunk_mass)
number_of_vertices = np.size(IAR, 0)
# number_of_vertices = 10
# print IAR
for i in range(0, number_of_vertices):
point = Point()
point.x = IAR[i][0]
point.y = IAR[i][1]
point.z = 0.0
vertices = np.hstack([vertices, point])
# print'vertices', vertices
p.send_polygons(name, vertices)
time.sleep(1.0 / 5.0)
i += 1
print 'de registering...'
p.deregister_node()
computation_time = (time.time() - start_t_IP)
print("Total time: --- %s seconds ---" % computation_time)
print 'number of published messages ', actuationParams.numberOfPublishedMessages
avgTime = computation_time / actuationParams.numberOfPublishedMessages
print 'average publishing time [ms]', avgTime
print 'average publishing frequency [Hz]', 1.0 / avgTime
print 'number of received messages ', p.numberOfReceivedMessages
avgTime = computation_time / p.numberOfReceivedMessages
print 'average subscription time [ms]', avgTime
print 'average subscription frequency [Hz]', 1.0 / avgTime
from numpy import array
from numpy.linalg import norm
from jet_leg.plotting_tools import Plotter
import random
from jet_leg.math_tools import Math
from jet_leg.computational_dynamics import ComputationalDynamics
from jet_leg.iterative_projection_parameters import IterativeProjectionParameters
from jet_leg.foothold_planning_interface import FootholdPlanningInterface
from jet_leg.foothold_planning import FootHoldPlanning
import matplotlib.pyplot as plt
from jet_leg.arrow3D import Arrow3D
plt.close('all')
math = Math()
# number of contacts
#nc = 3
# number of generators, i.e. rays used to linearize the friction cone
ng = 4
# ONLY_ACTUATION, ONLY_FRICTION or FRICTION_AND_ACTUATION
constraint_mode_IP = [
'FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION'
]
useVariableJacobian = False
# number of decision variables of the problem
#n = nc*6
# contact positions
def bilinear_formulation():
''' Definition the bilinear problem '''
math = Math()
pos0 = np.array([2.5, 2.5, 2.5])
force0 = np.array([-10.0, 10.0, 10.0])
posDesired = np.array([2.0, 2.0, 2.0])
forceDesired = np.array([0.0, 0.0, 10.0])
tauDesired = np.dot(math.skew(posDesired), forceDesired)
print "initial contact position: ", pos0
print "initial force: ", force0
print "Desired torque: ", tauDesired
x0 = np.hstack([pos0, force0])
lb = [1.0, 1.0, 1.0, -20.0, -20.0, 0.0]
ub = [3.0, 3.0, 3.0, 20.0, 20.0, 20.0]
tau_x_constraints_lb = np.array([20.0])
tau_x_constraints_ub = np.array([20.0])
tau_y_constraints_lb = np.array([-20.0])
tau_y_constraints_ub = np.array([-20.0])
tau_z_constraints_lb = np.array([0.0])
tau_z_constraints_ub = np.array([0.0])
cl = np.hstack(
[tau_x_constraints_lb, tau_y_constraints_lb, tau_z_constraints_lb])
cu = np.hstack(
[tau_x_constraints_ub, tau_y_constraints_ub, tau_z_constraints_ub])
nlpConvex = ipopt.problem(n=len(x0),
m=len(cl),
problem_obj=buildBilinearFormulation(x0),
lb=lb,
ub=ub,
cl=cl,
cu=cu)
#
# Set solver options
#
#nlp.addOption('derivative_test', 'second-order')
nlpConvex.addOption('mu_strategy', 'adaptive')
nlpConvex.addOption('jacobian_approximation', 'finite-difference-values')
nlpConvex.addOption('hessian_approximation', 'limited-memory')
nlpConvex.addOption('tol', 1e-2)
#
# Scale the problem (Just for demonstration purposes)
#
nlpConvex.setProblemScaling(obj_scaling=1,
x_scaling=[1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
nlpConvex.addOption('nlp_scaling_method', 'user-scaling')
#
# Solve the problem
#
sol, info = nlpConvex.solve(x0)
print "Solution of the primal variables: x=%s\n" % repr(sol)
print "Solution of the dual variables: lambda=%s\n" % repr(info['mult_g'])
print "Objective=%s\n" % repr(info['obj_val'])
l = sol[0:3]
f = sol[3:6]
tau = np.dot(math.skew(l), f)
print colored('-----------> Results check: ', 'red')
print colored('desired torque: ',
'blue'), tauDesired[0], tauDesired[1], tauDesired[2]
print colored('actual torque: ', 'green'), tau
print colored('torque error: ', 'red'), np.subtract(tau, tauDesired)
print colored('foot pos: ', 'green'), l
print colored('force: ', 'green'), f
def test_LP_friction_constraints_only(self):
math = Math()
# number of contacts
nc = 3
# number of generators, i.e. rays used to linearize the friction cone
ng = 4
# ONLY_FRICTION
constraint_mode_IP = ['ONLY_FRICTION',
'ONLY_FRICTION',
'ONLY_FRICTION',
'ONLY_FRICTION']
useVariableJacobian = True
LF_foot = np.array([0.3, 0.2, -0.65])
RF_foot = np.array([0.3, -0.2, -0.65])
LH_foot = np.array([-0.2, 0.2, -0.4])
RH_foot = np.array([-0.3, -0.2, -0.65])
contactsToStack = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot))
contacts = contactsToStack[0:4, :]
''' parameters to be tuned'''
g = 9.81
trunk_mass = 90.
mu = 0.8
axisZ= np.array([[0.0], [0.0], [1.0]])
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
''' stanceFeet vector contains 1 is the foot is on the ground and 0 if it is in the air'''
stanceFeet = [1, 1, 1, 1]
randomSwingLeg = random.randint(0, 3)
tripleStance = True # if you want you can define a swing leg using this variable
if tripleStance:
print 'Swing leg', randomSwingLeg
stanceFeet[randomSwingLeg] = 0
print 'stanceLegs ', stanceFeet
normals = np.vstack([n1, n2, n3, n4])
comp_dyn = ComputationalDynamics()
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)
feasible, unfeasible, contact_forces = comp_dyn.LP_projection(params)
print 'result',feasible, unfeasible, contact_forces
expected_feasible = np.array([[ -1.00000000e-01, 1.50000000e-01, -2.00000000e-01],
[ -1.00000000e-01, 1.50000000e-01, -1.50000000e-01],
[ -1.00000000e-01, 1.50000000e-01, -1.00000000e-01],
[ -1.00000000e-01, 1.50000000e-01, -5.00000000e-02],
[ -1.00000000e-01, 1.50000000e-01, -5.55111512e-17],
[ -1.00000000e-01, 1.50000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 1.50000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -2.00000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -1.50000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -1.00000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 1.00000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 1.00000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 1.00000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -2.00000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -1.50000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -1.00000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 1.50000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 1.50000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 1.50000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -2.00000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -1.50000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -1.00000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -5.00000000e-02],
[ -1.11022302e-16, 5.00000000e-02, -5.55111512e-17],
[ -1.11022302e-16, 5.00000000e-02, 5.00000000e-02],
[ -1.11022302e-16, 5.00000000e-02, 1.00000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -2.00000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -1.50000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -1.00000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 1.00000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 1.00000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 1.00000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -2.00000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -1.50000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -1.00000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 1.50000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 1.50000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 1.50000000e-01, 1.00000000e-01],
[ 5.00000000e-02, -1.11022302e-16, -2.00000000e-01],
[ 5.00000000e-02, -1.11022302e-16, -1.50000000e-01],
[ 5.00000000e-02, -1.11022302e-16, -1.00000000e-01],
[ 5.00000000e-02, -1.11022302e-16, -5.00000000e-02],
[ 5.00000000e-02, -1.11022302e-16, -5.55111512e-17],
[ 5.00000000e-02, -1.11022302e-16, 5.00000000e-02],
[ 5.00000000e-02, -1.11022302e-16, 1.00000000e-01],
[ 5.00000000e-02, 5.00000000e-02, -2.00000000e-01],
[ 5.00000000e-02, 5.00000000e-02, -1.50000000e-01],
[ 5.00000000e-02, 5.00000000e-02, -1.00000000e-01],
[ 5.00000000e-02, 5.00000000e-02, -5.00000000e-02],
[ 5.00000000e-02, 5.00000000e-02, -5.55111512e-17],
[ 5.00000000e-02, 5.00000000e-02, 5.00000000e-02],
[ 5.00000000e-02, 5.00000000e-02, 1.00000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -2.00000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 1.00000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 1.00000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 1.00000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -2.00000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 1.50000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 1.50000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 1.50000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -2.00000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 2.00000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 2.00000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 2.00000000e-01, 1.00000000e-01],
[ 1.00000000e-01, -1.11022302e-16, -2.00000000e-01],
[ 1.00000000e-01, -1.11022302e-16, -1.50000000e-01],
[ 1.00000000e-01, -1.11022302e-16, -1.00000000e-01],
[ 1.00000000e-01, -1.11022302e-16, -5.00000000e-02],
[ 1.00000000e-01, -1.11022302e-16, -5.55111512e-17],
[ 1.00000000e-01, -1.11022302e-16, 5.00000000e-02],
[ 1.00000000e-01, -1.11022302e-16, 1.00000000e-01],
[ 1.00000000e-01, 5.00000000e-02, -2.00000000e-01],
[ 1.00000000e-01, 5.00000000e-02, -1.50000000e-01],
[ 1.00000000e-01, 5.00000000e-02, -1.00000000e-01],
[ 1.00000000e-01, 5.00000000e-02, -5.00000000e-02],
[ 1.00000000e-01, 5.00000000e-02, -5.55111512e-17],
[ 1.00000000e-01, 5.00000000e-02, 5.00000000e-02],
[ 1.00000000e-01, 5.00000000e-02, 1.00000000e-01],
[ 1.00000000e-01, 1.00000000e-01, -2.00000000e-01],
[ 1.00000000e-01, 1.00000000e-01, -1.50000000e-01],
[ 1.00000000e-01, 1.00000000e-01, -1.00000000e-01],
[ 1.00000000e-01, 1.00000000e-01, -5.00000000e-02],
[ 1.00000000e-01, 1.00000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 1.00000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 1.00000000e-01, 1.00000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -1.50000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -1.00000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -5.00000000e-02],
[ 1.00000000e-01, 1.50000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 1.50000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 1.50000000e-01, 1.00000000e-01],
[ 1.50000000e-01, -5.00000000e-02, -2.00000000e-01],
[ 1.50000000e-01, -5.00000000e-02, -1.50000000e-01],
[ 1.50000000e-01, -5.00000000e-02, -1.00000000e-01],
[ 1.50000000e-01, -5.00000000e-02, -5.00000000e-02],
[ 1.50000000e-01, -5.00000000e-02, -5.55111512e-17],
[ 1.50000000e-01, -5.00000000e-02, 5.00000000e-02],
[ 1.50000000e-01, -1.11022302e-16, -2.00000000e-01],
[ 1.50000000e-01, -1.11022302e-16, -1.50000000e-01],
[ 1.50000000e-01, -1.11022302e-16, -1.00000000e-01],
[ 1.50000000e-01, -1.11022302e-16, -5.00000000e-02],
[ 1.50000000e-01, -1.11022302e-16, -5.55111512e-17],
[ 1.50000000e-01, -1.11022302e-16, 5.00000000e-02],
[ 1.50000000e-01, 5.00000000e-02, -2.00000000e-01],
[ 1.50000000e-01, 5.00000000e-02, -1.50000000e-01],
[ 1.50000000e-01, 5.00000000e-02, -1.00000000e-01],
[ 1.50000000e-01, 5.00000000e-02, -5.00000000e-02],
[ 1.50000000e-01, 5.00000000e-02, -5.55111512e-17],
[ 1.50000000e-01, 5.00000000e-02, 5.00000000e-02],
[ 1.50000000e-01, 1.00000000e-01, -2.00000000e-01],
[ 1.50000000e-01, 1.00000000e-01, -1.50000000e-01],
[ 1.50000000e-01, 1.00000000e-01, -1.00000000e-01],
[ 1.50000000e-01, 1.00000000e-01, -5.00000000e-02],
[ 1.50000000e-01, 1.00000000e-01, -5.55111512e-17],
[ 1.50000000e-01, 1.00000000e-01, 5.00000000e-02],
[ 1.50000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 1.50000000e-01, 1.50000000e-01, -1.50000000e-01],
[ 1.50000000e-01, 1.50000000e-01, -1.00000000e-01],
[ 1.50000000e-01, 1.50000000e-01, -5.00000000e-02],
[ 1.50000000e-01, 1.50000000e-01, -5.55111512e-17],
[ 1.50000000e-01, 1.50000000e-01, 5.00000000e-02],
[ 2.00000000e-01, -1.00000000e-01, -2.00000000e-01],
[ 2.00000000e-01, -1.00000000e-01, -1.50000000e-01],
[ 2.00000000e-01, -1.00000000e-01, -1.00000000e-01],
[ 2.00000000e-01, -1.00000000e-01, -5.00000000e-02],
[ 2.00000000e-01, -1.00000000e-01, -5.55111512e-17],
[ 2.00000000e-01, -1.00000000e-01, 5.00000000e-02],
[ 2.00000000e-01, -5.00000000e-02, -2.00000000e-01],
[ 2.00000000e-01, -5.00000000e-02, -1.50000000e-01],
[ 2.00000000e-01, -5.00000000e-02, -1.00000000e-01],
[ 2.00000000e-01, -5.00000000e-02, -5.00000000e-02],
[ 2.00000000e-01, -5.00000000e-02, -5.55111512e-17],
[ 2.00000000e-01, -5.00000000e-02, 5.00000000e-02],
[ 2.00000000e-01, -1.11022302e-16, -2.00000000e-01],
[ 2.00000000e-01, -1.11022302e-16, -1.50000000e-01],
[ 2.00000000e-01, -1.11022302e-16, -1.00000000e-01],
[ 2.00000000e-01, -1.11022302e-16, -5.00000000e-02],
[ 2.00000000e-01, -1.11022302e-16, -5.55111512e-17],
[ 2.00000000e-01, -1.11022302e-16, 5.00000000e-02],
[ 2.00000000e-01, 5.00000000e-02, -2.00000000e-01],
[ 2.00000000e-01, 5.00000000e-02, -1.50000000e-01],
[ 2.00000000e-01, 5.00000000e-02, -1.00000000e-01],
[ 2.00000000e-01, 5.00000000e-02, -5.00000000e-02],
[ 2.00000000e-01, 5.00000000e-02, -5.55111512e-17],
[ 2.00000000e-01, 5.00000000e-02, 5.00000000e-02],
[ 2.00000000e-01, 1.00000000e-01, -2.00000000e-01],
[ 2.00000000e-01, 1.00000000e-01, -1.50000000e-01],
[ 2.00000000e-01, 1.00000000e-01, -1.00000000e-01],
[ 2.00000000e-01, 1.00000000e-01, -5.00000000e-02],
[ 2.00000000e-01, 1.00000000e-01, -5.55111512e-17],
[ 2.00000000e-01, 1.00000000e-01, 5.00000000e-02],
[ 2.00000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 2.00000000e-01, 1.50000000e-01, -1.50000000e-01],
[ 2.00000000e-01, 1.50000000e-01, -1.00000000e-01],
[ 2.00000000e-01, 1.50000000e-01, -5.00000000e-02],
[ 2.00000000e-01, 1.50000000e-01, -5.55111512e-17],
[ 2.00000000e-01, 1.50000000e-01, 5.00000000e-02],
[ 2.00000000e-01, 2.00000000e-01, -2.00000000e-01],
[ 2.00000000e-01, 2.00000000e-01, -1.50000000e-01],
[ 2.00000000e-01, 2.00000000e-01, -1.00000000e-01],
[ 2.00000000e-01, 2.00000000e-01, -5.00000000e-02],
[ 2.00000000e-01, 2.00000000e-01, -5.55111512e-17],
[ 2.00000000e-01, 2.00000000e-01, 5.00000000e-02],
[ 2.50000000e-01, -1.50000000e-01, -2.00000000e-01],
[ 2.50000000e-01, -1.50000000e-01, -1.50000000e-01],
[ 2.50000000e-01, -1.50000000e-01, -1.00000000e-01],
[ 2.50000000e-01, -1.50000000e-01, -5.00000000e-02],
[ 2.50000000e-01, -1.50000000e-01, -5.55111512e-17],
[ 2.50000000e-01, -1.00000000e-01, -2.00000000e-01],
[ 2.50000000e-01, -1.00000000e-01, -1.50000000e-01],
[ 2.50000000e-01, -1.00000000e-01, -1.00000000e-01],
[ 2.50000000e-01, -1.00000000e-01, -5.00000000e-02],
[ 2.50000000e-01, -1.00000000e-01, -5.55111512e-17],
[ 2.50000000e-01, -5.00000000e-02, -2.00000000e-01],
[ 2.50000000e-01, -5.00000000e-02, -1.50000000e-01],
[ 2.50000000e-01, -5.00000000e-02, -1.00000000e-01],
[ 2.50000000e-01, -5.00000000e-02, -5.00000000e-02],
[ 2.50000000e-01, -5.00000000e-02, -5.55111512e-17],
[ 2.50000000e-01, -1.11022302e-16, -2.00000000e-01],
[ 2.50000000e-01, -1.11022302e-16, -1.50000000e-01],
[ 2.50000000e-01, -1.11022302e-16, -1.00000000e-01],
[ 2.50000000e-01, -1.11022302e-16, -5.00000000e-02],
[ 2.50000000e-01, -1.11022302e-16, -5.55111512e-17],
[ 2.50000000e-01, 5.00000000e-02, -2.00000000e-01],
[ 2.50000000e-01, 5.00000000e-02, -1.50000000e-01],
[ 2.50000000e-01, 5.00000000e-02, -1.00000000e-01],
[ 2.50000000e-01, 5.00000000e-02, -5.00000000e-02],
[ 2.50000000e-01, 5.00000000e-02, -5.55111512e-17],
[ 2.50000000e-01, 1.00000000e-01, -2.00000000e-01],
[ 2.50000000e-01, 1.00000000e-01, -1.50000000e-01],
[ 2.50000000e-01, 1.00000000e-01, -1.00000000e-01],
[ 2.50000000e-01, 1.00000000e-01, -5.00000000e-02],
[ 2.50000000e-01, 1.00000000e-01, -5.55111512e-17],
[ 2.50000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 2.50000000e-01, 1.50000000e-01, -1.50000000e-01],
[ 2.50000000e-01, 1.50000000e-01, -1.00000000e-01],
[ 2.50000000e-01, 1.50000000e-01, -5.00000000e-02],
[ 2.50000000e-01, 1.50000000e-01, -5.55111512e-17],
[ 3.00000000e-01, -1.50000000e-01, -2.00000000e-01],
[ 3.00000000e-01, -1.50000000e-01, -1.50000000e-01],
[ 3.00000000e-01, -1.50000000e-01, -1.00000000e-01],
[ 3.00000000e-01, -1.50000000e-01, -5.00000000e-02],
[ 3.00000000e-01, -1.50000000e-01, -5.55111512e-17],
[ 3.00000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 3.00000000e-01, 1.50000000e-01, -1.50000000e-01],
[ 3.00000000e-01, 1.50000000e-01, -1.00000000e-01],
[ 3.00000000e-01, 1.50000000e-01, -5.00000000e-02],
[ 3.00000000e-01, 1.50000000e-01, -5.55111512e-17]]);
# print expected_feasible
rows = np.size(expected_feasible, 0)
cols = np.size(expected_feasible, 1)
for i in range(0, rows):
for j in range (0, cols):
self.assertAlmostEquals(expected_feasible[i,j], feasible[i,j], self.assertPrecision)
def test_lp_stability_check(self):
math = Math()
# number of contacts
nc = 3
# number of generators, i.e. rays used to linearize the friction cone
ng = 4
# ONLY_FRICTION
# ONLY_ACTUATION
constraint_mode_IP = ['ONLY_ACTUATION',
'ONLY_ACTUATION',
'ONLY_ACTUATION',
'ONLY_ACTUATION']
useVariableJacobian = True
comWF = np.array([1.25, 0.0, 0.0])
""" contact points in the World Frame"""
LF_foot = np.array([1.3, 0.2, -0.6])
RF_foot = np.array([1.3, -0.2, -0.5])
LH_foot = np.array([0.7, 0.2, -0.45])
RH_foot = np.array([0.7, -0.2, -0.5])
contactsToStack = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot))
contacts = contactsToStack[0:4, :]
''' parameters to be tuned'''
g = 9.81
trunk_mass = 50.
mu = 0.8
axisZ = np.array([[0.0], [0.0], [1.0]])
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
''' stanceFeet vector contains 1 is the foot is on the ground and 0 if it is in the air'''
stanceFeet = [1, 1, 1, 1]
randomSwingLeg = random.randint(0, 3)
tripleStance = True # if you want you can define a swing leg using this variable
if tripleStance:
print 'Swing leg', randomSwingLeg
#stanceFeet[randomSwingLeg] = 0
print 'stanceLegs ', stanceFeet
normals = np.vstack([n1, n2, n3, n4])
comp_dyn = ComputationalDynamics()
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)
status, x, force_polytopes = comp_dyn.check_equilibrium(params)
print status, x
'''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')
''' plotting Iterative Projection points '''
plotter = Plotter()
scaling_factor = 2000
stanceID = params.getStanceIndex(stanceFeet)
for j in range(0, nc): # this will only show the force polytopes of the feet that are defined to be in stance
idx = int(stanceID[j])
if (constraint_mode_IP[idx] == 'ONLY_ACTUATION') or (constraint_mode_IP[idx] == 'FRICTION_AND_ACTUATION'):
plotter.plot_actuation_polygon(ax, force_polytopes[idx], contacts[idx, :], scaling_factor)
plt.show()
def test_LP_actuation_constraints_only(self):
math = Math()
# number of contacts
nc = 3
# number of generators, i.e. rays used to linearize the friction cone
ng = 4
# ONLY_ACTUATION or ONLY_FRICTION
constraint_mode = 'ONLY_ACTUATION'
useVariableJacobian = True
""" contact points """
LF_foot = np.array([0.3, 0.2, -0.65])
RF_foot = np.array([0.3, -0.2, -0.65])
LH_foot = np.array([-0.2, 0.2, -0.4])
RH_foot = np.array([-0.3, -0.2, -0.65])
contactsToStack = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot))
contacts = contactsToStack[0:nc, :]
''' parameters to be tuned'''
g = 9.81
trunk_mass = 90.
mu = 0.8
axisZ= np.array([[0.0], [0.0], [1.0]])
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])
comp_dyn = ComputationalDynamics()
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)
feasible, unfeasible, contact_forces = comp_dyn.LP_projection(params)
expected_feasible = np.array([[ -1.50000000e-01, 2.50000000e-01, 5.00000000e-02],
[ -1.50000000e-01, 2.50000000e-01, 1.00000000e-01],
[ -1.50000000e-01, 3.00000000e-01, 5.00000000e-02],
[ -1.50000000e-01, 3.00000000e-01, 1.00000000e-01],
[ -1.50000000e-01, 3.50000000e-01, 1.00000000e-01],
[ -1.00000000e-01, 1.00000000e-01, -5.55111512e-17],
[ -1.00000000e-01, 1.00000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 1.00000000e-01, 1.00000000e-01],
[ -1.00000000e-01, 1.50000000e-01, -5.55111512e-17],
[ -1.00000000e-01, 1.50000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 1.50000000e-01, 1.00000000e-01],
[ -1.00000000e-01, 2.00000000e-01, -5.55111512e-17],
[ -1.00000000e-01, 2.00000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 2.00000000e-01, 1.00000000e-01],
[ -1.00000000e-01, 2.50000000e-01, -5.55111512e-17],
[ -1.00000000e-01, 2.50000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 2.50000000e-01, 1.00000000e-01],
[ -1.00000000e-01, 3.00000000e-01, -5.55111512e-17],
[ -1.00000000e-01, 3.00000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 3.00000000e-01, 1.00000000e-01],
[ -1.00000000e-01, 3.50000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 3.50000000e-01, 1.00000000e-01],
[ -5.00000000e-02, -1.11022302e-16, -1.00000000e-01],
[ -5.00000000e-02, -1.11022302e-16, -5.00000000e-02],
[ -5.00000000e-02, -1.11022302e-16, -5.55111512e-17],
[ -5.00000000e-02, -1.11022302e-16, 5.00000000e-02],
[ -5.00000000e-02, -1.11022302e-16, 1.00000000e-01],
[ -5.00000000e-02, 5.00000000e-02, -1.50000000e-01],
[ -5.00000000e-02, 5.00000000e-02, -1.00000000e-01],
[ -5.00000000e-02, 5.00000000e-02, -5.00000000e-02],
[ -5.00000000e-02, 5.00000000e-02, -5.55111512e-17],
[ -5.00000000e-02, 5.00000000e-02, 5.00000000e-02],
[ -5.00000000e-02, 5.00000000e-02, 1.00000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -2.00000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -1.50000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -1.00000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 1.00000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 1.00000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 1.00000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -2.00000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -1.50000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -1.00000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 1.50000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 1.50000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 1.50000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 2.00000000e-01, -2.00000000e-01],
[ -5.00000000e-02, 2.00000000e-01, -1.50000000e-01],
[ -5.00000000e-02, 2.00000000e-01, -1.00000000e-01],
[ -5.00000000e-02, 2.00000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 2.00000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 2.00000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 2.00000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 2.50000000e-01, -2.00000000e-01],
[ -5.00000000e-02, 2.50000000e-01, -1.50000000e-01],
[ -5.00000000e-02, 2.50000000e-01, -1.00000000e-01],
[ -5.00000000e-02, 2.50000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 2.50000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 2.50000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 2.50000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 3.00000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 3.00000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 3.00000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 3.00000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 3.50000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 3.50000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 3.50000000e-01, 1.00000000e-01],
[ -1.11022302e-16, -5.00000000e-02, -1.00000000e-01],
[ -1.11022302e-16, -5.00000000e-02, -5.00000000e-02],
[ -1.11022302e-16, -5.00000000e-02, -5.55111512e-17],
[ -1.11022302e-16, -5.00000000e-02, 5.00000000e-02],
[ -1.11022302e-16, -5.00000000e-02, 1.00000000e-01],
[ -1.11022302e-16, -1.11022302e-16, -1.50000000e-01],
[ -1.11022302e-16, -1.11022302e-16, -1.00000000e-01],
[ -1.11022302e-16, -1.11022302e-16, -5.00000000e-02],
[ -1.11022302e-16, -1.11022302e-16, -5.55111512e-17],
[ -1.11022302e-16, -1.11022302e-16, 5.00000000e-02],
[ -1.11022302e-16, -1.11022302e-16, 1.00000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -1.50000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -1.00000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -5.00000000e-02],
[ -1.11022302e-16, 5.00000000e-02, -5.55111512e-17],
[ -1.11022302e-16, 5.00000000e-02, 5.00000000e-02],
[ -1.11022302e-16, 5.00000000e-02, 1.00000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -2.00000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -1.50000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -1.00000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 1.00000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 1.00000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 1.00000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -2.00000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -1.50000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -1.00000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 1.50000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 1.50000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 1.50000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 2.00000000e-01, -2.00000000e-01],
[ -1.11022302e-16, 2.00000000e-01, -1.50000000e-01],
[ -1.11022302e-16, 2.00000000e-01, -1.00000000e-01],
[ -1.11022302e-16, 2.00000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 2.00000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 2.00000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 2.00000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 2.50000000e-01, -1.50000000e-01],
[ -1.11022302e-16, 2.50000000e-01, -1.00000000e-01],
[ -1.11022302e-16, 2.50000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 2.50000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 2.50000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 2.50000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 3.00000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 3.00000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 3.00000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 3.00000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 3.50000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 3.50000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 3.50000000e-01, 1.00000000e-01],
[ 5.00000000e-02, -5.00000000e-02, -1.00000000e-01],
[ 5.00000000e-02, -5.00000000e-02, -5.00000000e-02],
[ 5.00000000e-02, -5.00000000e-02, -5.55111512e-17],
[ 5.00000000e-02, -5.00000000e-02, 5.00000000e-02],
[ 5.00000000e-02, -5.00000000e-02, 1.00000000e-01],
[ 5.00000000e-02, -1.11022302e-16, -1.00000000e-01],
[ 5.00000000e-02, -1.11022302e-16, -5.00000000e-02],
[ 5.00000000e-02, -1.11022302e-16, -5.55111512e-17],
[ 5.00000000e-02, -1.11022302e-16, 5.00000000e-02],
[ 5.00000000e-02, -1.11022302e-16, 1.00000000e-01],
[ 5.00000000e-02, 5.00000000e-02, -1.00000000e-01],
[ 5.00000000e-02, 5.00000000e-02, -5.00000000e-02],
[ 5.00000000e-02, 5.00000000e-02, -5.55111512e-17],
[ 5.00000000e-02, 5.00000000e-02, 5.00000000e-02],
[ 5.00000000e-02, 5.00000000e-02, 1.00000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -2.00000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 1.00000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 1.00000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 1.00000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -2.00000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 1.50000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 1.50000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 1.50000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -2.00000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 2.00000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 2.00000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 2.00000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 2.50000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 2.50000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 2.50000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 2.50000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 2.50000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 2.50000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 3.00000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 3.00000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 3.00000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 3.00000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 3.50000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 3.50000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 3.50000000e-01, 1.00000000e-01],
[ 1.00000000e-01, -5.00000000e-02, -5.55111512e-17],
[ 1.00000000e-01, -5.00000000e-02, 5.00000000e-02],
[ 1.00000000e-01, -5.00000000e-02, 1.00000000e-01],
[ 1.00000000e-01, -1.11022302e-16, -5.55111512e-17],
[ 1.00000000e-01, -1.11022302e-16, 5.00000000e-02],
[ 1.00000000e-01, -1.11022302e-16, 1.00000000e-01],
[ 1.00000000e-01, 5.00000000e-02, -5.55111512e-17],
[ 1.00000000e-01, 5.00000000e-02, 5.00000000e-02],
[ 1.00000000e-01, 5.00000000e-02, 1.00000000e-01],
[ 1.00000000e-01, 1.00000000e-01, -1.00000000e-01],
[ 1.00000000e-01, 1.00000000e-01, -5.00000000e-02],
[ 1.00000000e-01, 1.00000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 1.00000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 1.00000000e-01, 1.00000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -1.50000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -1.00000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -5.00000000e-02],
[ 1.00000000e-01, 1.50000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 1.50000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 1.50000000e-01, 1.00000000e-01],
[ 1.00000000e-01, 2.00000000e-01, -2.00000000e-01],
[ 1.00000000e-01, 2.00000000e-01, -1.50000000e-01],
[ 1.00000000e-01, 2.00000000e-01, -1.00000000e-01],
[ 1.00000000e-01, 2.00000000e-01, -5.00000000e-02],
[ 1.00000000e-01, 2.00000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 2.00000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 2.00000000e-01, 1.00000000e-01],
[ 1.00000000e-01, 2.50000000e-01, -1.00000000e-01],
[ 1.00000000e-01, 2.50000000e-01, -5.00000000e-02],
[ 1.00000000e-01, 2.50000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 2.50000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 2.50000000e-01, 1.00000000e-01],
[ 1.00000000e-01, 3.00000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 3.00000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 3.00000000e-01, 1.00000000e-01],
[ 1.00000000e-01, 3.50000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 3.50000000e-01, 1.00000000e-01],
[ 1.50000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 1.50000000e-01, 1.50000000e-01, -5.55111512e-17],
[ 1.50000000e-01, 1.50000000e-01, 5.00000000e-02],
[ 1.50000000e-01, 2.00000000e-01, -2.00000000e-01],
[ 1.50000000e-01, 2.00000000e-01, -1.50000000e-01],
[ 1.50000000e-01, 2.00000000e-01, -1.00000000e-01],
[ 1.50000000e-01, 2.00000000e-01, -5.00000000e-02],
[ 1.50000000e-01, 2.00000000e-01, -5.55111512e-17],
[ 1.50000000e-01, 2.00000000e-01, 5.00000000e-02],
[ 1.50000000e-01, 2.50000000e-01, -1.00000000e-01],
[ 1.50000000e-01, 2.50000000e-01, -5.00000000e-02],
[ 1.50000000e-01, 2.50000000e-01, -5.55111512e-17],
[ 1.50000000e-01, 2.50000000e-01, 5.00000000e-02],
[ 1.50000000e-01, 3.00000000e-01, -5.55111512e-17],
[ 1.50000000e-01, 3.00000000e-01, 5.00000000e-02],
[ 1.50000000e-01, 3.50000000e-01, 5.00000000e-02],
[ 2.00000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 2.00000000e-01, 2.00000000e-01, -1.50000000e-01],
[ 2.00000000e-01, 2.50000000e-01, -5.00000000e-02],
[ 2.00000000e-01, 2.50000000e-01, -5.55111512e-17],
[ 2.00000000e-01, 3.00000000e-01, -5.55111512e-17]]);
# print expected_feasible
rows = np.size(expected_feasible, 0)
cols = np.size(expected_feasible, 1)
for i in range(0, rows):
for j in range (0, cols):
self.assertAlmostEquals(expected_feasible[i,j], feasible[i,j], self.assertPrecision)
def compute_polygon_variable_constraint(self,
constraint_mode,
comWorldFrame,
contactsWorldFrame,
max_iter=100,
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.
"""
math = Math()
trunk_mass = 100
mu = 0.8
axisZ = array([[0.0], [0.0], [1.0]])
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])
lp, actuation_polygons, isOutOfWorkspace = self.setup_iterative_projection(
constraint_mode, contactsWorldFrame, comWorldFrame, trunk_mass, mu,
normals)
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)
#print z
#print init_vertices
if len(z) != 0:
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 talker():
compDyn = ComputationalDynamics()
math = Math()
p=HyQSim()
p.start()
p.register_node()
name = "Actuation_region"
point = Point()
actuationParams = ActuationParameters()
i = 0
start_t_IP = time.time()
for j in range (0,100):
vertices = [point]
# print("Time: " + str(i*0.004) + "s and Simulation time: " + str(p.get_sim_time()/60))
p.get_sim_wbs()
actuationParams.getParams(p.hyq_rcf_debug)
trunk_mass = 85.
axisZ= np.array([[0.0], [0.0], [1.0]])
''' normals '''
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))
normals = np.vstack([n1, n2, n3])
""" contact points """
nc = actuationParams.numberOfContacts
contacts = actuationParams.contacts[0:nc+1, :]
LF_tau_lim = [50.0, 100.0, 100.0]
RF_tau_lim = [50.0, 100.0, 100.0]
LH_tau_lim = [50.0, 100.0, 100.0]
RH_tau_lim = [50.0, 100.0, 100.0]
torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim])
comWF = np.array([0.0, 0.0, 0.0])
extForceW = np.array([0.0,0.0, 0.0])
constraint_mode_IP = ['FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION']
mu = 0.8
ng = 4
# print 'contacts: ',contacts
# print contacts, actuationParams.stanceFeet
params = IterativeProjectionParameters()
stanceFeet = [1,1,1,1]
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 + extForceW[2]/9.81)
params.externalForceWF = extForceW
''' compute iterative projection '''
IAR, actuation_polygons, computation_time = compDyn.iterative_projection_bretl(params)
number_of_vertices = np.size(IAR, 0)
# number_of_vertices = 10
# print IAR
for i in range(0, number_of_vertices):
point = Point()
point.x = IAR[i][0]
point.y = IAR[i][1]
point.z = 0.0
vertices = np.hstack([vertices, point])
# print'vertices', vertices
p.send_polygons(name, vertices)
# time.sleep(1.0/5.0)
i+=1
print 'de registering...'
p.deregister_node()
computation_time = (time.time() - start_t_IP)
print("Total time: --- %s seconds ---" % computation_time)
print 'number of published messages ', actuationParams.numberOfPublishedMessages
avgTime = computation_time/actuationParams.numberOfPublishedMessages
print 'average publishing time [ms]', avgTime
print 'average publishing frequency [Hz]', 1.0/avgTime
print 'number of received messages ', p.numberOfReceivedMessages
avgTime = computation_time/p.numberOfReceivedMessages
print 'average subscription time [ms]', avgTime
print 'average subscription frequency [Hz]', 1.0/avgTime
def test_square_line_intersection_predefined_values(self):
math = Math()
#starting_point = np.array([np.random.randint(0,100)/100.0,np.random.randint(0,100)/100.0,0.0])
starting_point = np.array([0.0, 0.0, 0.0])
#print "starting point is: ",starting_point
#angle = np.random.randint(0,600)/600.0
search_direction = np.array([1.0, 1.0, 0.0])
vertices = np.array([[1.0, 1.0, 0.0], [1.0, -1.0, 0.0],
[-1.0, -1.0, 0.0], [-1.0, 1.0, 0.0]])
new_p, all_points = math.find_polygon_segment_intersection(
vertices, search_direction, starting_point)
expected_intersection_point = np.array([1.0, 1.0])
self.assertTrue(
(new_p - expected_intersection_point < self.epsilon).all())
search_direction = np.array([-1.0, 1.0, 0.0])
vertices = np.array([[1.0, 1.0, 0.0], [1.0, -1.0, 0.0],
[-1.0, -1.0, 0.0], [-1.0, 1.0, 0.0]])
new_p, all_points = math.find_polygon_segment_intersection(
vertices, search_direction, starting_point)
expected_intersection_point = np.array([-1.0, 1.0])
self.assertTrue(
(new_p - expected_intersection_point < self.epsilon).all())
search_direction = np.array([1.0, -1.0, 0.0])
vertices = np.array([[1.0, 1.0, 0.0], [1.0, -1.0, 0.0],
[-1.0, -1.0, 0.0], [-1.0, 1.0, 0.0]])
new_p, all_points = math.find_polygon_segment_intersection(
vertices, search_direction, starting_point)
expected_intersection_point = np.array([1.0, -1.0])
self.assertTrue(
(new_p - expected_intersection_point < self.epsilon).all())
search_direction = np.array([-1.0, -1.0, 0.0])
vertices = np.array([[1.0, 1.0, 0.0], [1.0, -1.0, 0.0],
[-1.0, -1.0, 0.0], [-1.0, 1.0, 0.0]])
new_p, all_points = math.find_polygon_segment_intersection(
vertices, search_direction, starting_point)
expected_intersection_point = np.array([-1.0, -1.0])
self.assertTrue(
(new_p - expected_intersection_point < self.epsilon).all())
search_direction = np.array([1.0, 0.0, 0.0])
vertices = np.array([[1.0, 1.0, 0.0], [1.0, -1.0, 0.0],
[-1.0, -1.0, 0.0], [-1.0, 1.0, 0.0]])
new_p, all_points = math.find_polygon_segment_intersection(
vertices, search_direction, starting_point)
expected_intersection_point = np.array([1.0, 0.0])
self.assertTrue(
(new_p - expected_intersection_point < self.epsilon).all())
search_direction = np.array([0.0, 1.0, 0.0])
vertices = np.array([[1.0, 1.0, 0.0], [1.0, -1.0, 0.0],
[-1.0, -1.0, 0.0], [-1.0, 1.0, 0.0]])
new_p, all_points = math.find_polygon_segment_intersection(
vertices, search_direction, starting_point)
expected_intersection_point = np.array([0.0, 1.0])
self.assertTrue(
(new_p - expected_intersection_point < self.epsilon).all())
search_direction = np.array([0.0, -1.0, 0.0])
vertices = np.array([[1.0, 1.0, 0.0], [1.0, -1.0, 0.0],
[-1.0, -1.0, 0.0], [-1.0, 1.0, 0.0]])
new_p, all_points = math.find_polygon_segment_intersection(
vertices, search_direction, starting_point)
expected_intersection_point = np.array([0.0, -1.0])
self.assertTrue(
(new_p - expected_intersection_point < self.epsilon).all())
search_direction = np.array([-1.0, 0.0, 0.0])
vertices = np.array([[1.0, 1.0, 0.0], [1.0, -1.0, 0.0],
[-1.0, -1.0, 0.0], [-1.0, 1.0, 0.0]])
new_p, all_points = math.find_polygon_segment_intersection(
vertices, search_direction, starting_point)
expected_intersection_point = np.array([-1.0, 0.0])
self.assertTrue(
(new_p - expected_intersection_point < self.epsilon).all())
return new_p, all_points
from context import jet_leg
from numpy import array
from numpy.linalg import norm
from jet_leg.plotting_tools import Plotter
from jet_leg.math_tools import Math
from jet_leg.computational_dynamics import ComputationalDynamics
from jet_leg.iterative_projection_parameters import IterativeProjectionParameters
import matplotlib as mpl
import matplotlib.pyplot as plt
import random
plt.close('all')
math = Math()
# number of contacts
nc = 3
# number of generators, i.e. rays used to linearize the friction cone
ng = 4
# ONLY_ACTUATION, ONLY_FRICTION or FRICTION_AND_ACTUATION
constraint_mode_IP = [
'ONLY_ACTUATION', 'ONLY_FRICTION', 'FRICTION_AND_ACTUATION',
'ONLY_ACTUATION'
]
useVariableJacobian = False
# number of decision variables of the problem
n = nc * 6