def talker():
compDyn = ComputationalDynamics()
p = HyQSim()
p.start()
p.register_node()
name = "Support region"
params = IterativeProjectionParameters()
foothold_params = FootholdPlanningInterface()
i = 0
p.get_sim_wbs()
params.getParamsFromRosDebugTopic(p.hyq_rcf_debug)
foothold_params.getParamsFromRosDebugTopic(p.hyq_rcf_debug)
params.getCurrentStanceFeetFlags(p.hyq_rcf_debug)
params.getCurrentFeetPos(p.hyq_rcf_debug)
""" contact points """
ng = 4
params.setNumberOfFrictionConesEdges(ng)
while not ros.is_shutdown():
p.get_sim_wbs()
params.getParamsFromRosDebugTopic(p.hyq_rcf_debug)
foothold_params.getParamsFromRosDebugTopic(p.hyq_rcf_debug)
params.getCurrentStanceFeetFlags(p.hyq_rcf_debug)
# params.getCurrentFeetPosFlags(p.hyq_rcf_debug)
# 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)
frictionRegionWF, actuation_polygons, computation_time = compDyn.try_iterative_projection_bretl(
params)
print frictionRegionWF
p.send_support_region(name, p.fillPolygon(frictionRegionWF))
time.sleep(0.05)
i += 1
print 'de registering...'
p.deregister_node()
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
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, n4]) 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]) nc = np.sum(stanceFeet) comp_dyn = ComputationalDynamics() params = IterativeProjectionParameters() params.setContactsPosWF(contacts) 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) #inputs for foothold planning foothold_params = FootholdPlanningInterface() foothold_params.com_position_to_validateW = [0.05, 0.05, 0.55] maxCorrection = 0.2
from numpy import array, cross, dot, eye, hstack, vstack, zeros import matplotlib.pyplot as plt from jet_leg.plotting_tools import Plotter from jet_leg.constraints import Constraints from jet_leg.hyq_kinematics import HyQKinematics from jet_leg.math_tools import Math from jet_leg.computational_dynamics import ComputationalDynamics from jet_leg.height_map import HeightMap from path_sequential_iterative_projection import PathIterativeProjection ''' MAIN ''' start_t_IPVC = time.time() math = Math() compDyn = ComputationalDynamics() pathIP = PathIterativeProjection() # 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 trunk_mass = 100 mu = 0.8 # number of decision variables of the problem n = nc * 6 i = 0 comTrajectoriesToStack = np.zeros((0, 3))
from scipy.linalg import block_diag
from numpy import array, cross, dot, eye, hstack, vstack, zeros, matrix
import matplotlib.pyplot as plt
from jet_leg.plotting_tools import Plotter
from jet_leg.constraints import Constraints
from jet_leg.hyq_kinematics import HyQKinematics
from jet_leg.math_tools import Math
from jet_leg.computational_dynamics import ComputationalDynamics
from jet_leg.vertex_based_projection import VertexBasedProjection
from jet_leg.iterative_projection_parameters import IterativeProjectionParameters
''' MAIN '''
start_t_IPVC = time.time()
math = Math()
compDyn = ComputationalDynamics()
# number of generators, i.e. rays used to linearize the friction cone
ng = 4
# ONLY_ACTUATION or ONLY_FRICTION or FRICTION_AND_ACTUATION
constraint_mode_IP = [
'ONLY_ACTUATION', 'ONLY_ACTUATION', 'ONLY_ACTUATION', 'ONLY_ACTUATION'
]
useVariableJacobian = True
g = 9.81
trunk_mass = 85.
mu = 0.9
""" contact points """
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.3, 0.2, -0.5])
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)
else:
swingIndex = iter
''' parameters to be tuned'''
g = 9.81
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])
comp_dyn = ComputationalDynamics()
comWF = np.array([0.0, 0.0, 0.0])
trunk_mass = 90
idx = 0
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])
params = IterativeProjectionParameters()
params.setContactsPosWF(contacts)
params.setCoMPosWF(comWF)
params.setTorqueLims(torque_limits)
params.setActiveContacts(stanceLegs)
def setup_iterative_projection(self, constraint_mode, contacts, comWF,
trunk_mass, mu, normals):
''' parameters to be tuned'''
g = 9.81
isOutOfWorkspace = False
compDyn = ComputationalDynamics()
grav = array([0., 0., -g])
contactsNumber = np.size(contacts, 0)
# Unprojected state is:
#
# x = [f1_x, f1_y, f1_z, ... , f3_x, f3_y, f3_z]
Ex = np.zeros((0))
Ey = np.zeros((0))
G = np.zeros((6, 0))
for j in range(0, contactsNumber):
r = contacts[j, :]
graspMatrix = compDyn.getGraspMatrix(r)[:, 0:3]
Ex = hstack([Ex, -graspMatrix[4]])
Ey = hstack([Ey, graspMatrix[3]])
G = hstack([G, graspMatrix])
E = vstack((Ex, Ey)) / (g)
f = zeros(2)
proj = (E, f) # y = E * x + f
# number of the equality constraints
m_eq = 6
# see Equation (52) in "ZMP Support Areas for Multicontact..."
A_f_and_tauz = array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 1]])
A = dot(A_f_and_tauz, G)
t = hstack([0, 0, g, 0])
#print A,t
eq = (A, t) # A * x == t
act_LF = np.zeros((0, 1))
act_RF = np.zeros((0, 1))
act_LH = np.zeros((0, 1))
act_RH = np.zeros((0, 1))
actuation_polygons = np.zeros((0, 1))
# Inequality matrix for a contact force in local contact frame:
constr = Constraints()
#C_force = constr.linearized_cone_halfspaces(ng, mu)
# Inequality matrix for stacked contact forces in world frame:
if constraint_mode == 'ONLY_FRICTION':
C, d = constr.linearized_cone_halfspaces_world(
contactsNumber, ng, mu, normals)
elif constraint_mode == 'ONLY_ACTUATION':
#kin = Kinematics()
kin = HyQKinematics()
foot_vel = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]])
contactsFourLegs = np.vstack(
[contacts, np.zeros((4 - contactsNumber, 3))])
q, q_dot, J_LF, J_RF, J_LH, J_RH, isOutOfWorkspace = kin.inverse_kin(
np.transpose(contactsFourLegs[:, 0] - comWF[0]),
np.transpose(foot_vel[:, 0]),
np.transpose(contactsFourLegs[:, 1] - comWF[1]),
np.transpose(foot_vel[:, 1]),
np.transpose(contactsFourLegs[:, 2] - comWF[2]),
np.transpose(foot_vel[:, 2]))
J_LF, J_RF, J_LH, J_RH = kin.update_jacobians(q)
if isOutOfWorkspace:
C = np.zeros((0, 0))
d = np.zeros((1, 0))
else:
act_LF = constr.computeLegActuationPolygon(J_LF)
act_RF = constr.computeLegActuationPolygon(J_RF)
act_LH = constr.computeLegActuationPolygon(J_LH)
act_RH = constr.computeLegActuationPolygon(J_RH)
''' in the case of the IP alg. the contact force limits must be divided by the mass
because the gravito inertial wrench is normalized'''
C = np.zeros((0, 0))
d = np.zeros((1, 0))
actuation_polygons = np.array([act_LF, act_RF, act_LH, act_RH])
for j in range(0, contactsNumber):
hexahedronHalfSpaceConstraints, knownTerm = constr.hexahedron(
actuation_polygons[j] / trunk_mass)
C = block_diag(C, hexahedronHalfSpaceConstraints)
d = hstack([d, knownTerm.T])
d = d.reshape(6 * contactsNumber)
#C = block_diag(c1, c2, c3, c4)
#d = np.vstack([e1, e2, e3, e4]).reshape(6*4)
#print C, d
ineq = (C, d) # C * x <= d
if isOutOfWorkspace:
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
return lp, actuation_polygons / trunk_mass, isOutOfWorkspace
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 optimize_direction_variable_constraint(comWorld,
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 """
LF_foot = np.array([0.3, 0.3, -0.5])
RF_foot = np.array([0.3, -0.3, -0.5])
LH_foot = np.array([-0.3, 0.3, -0.5])
RH_foot = np.array([-0.3, -0.2, -0.5])
nc = 3
numberOfGenerators = 4
trunk_mass = 100
mu = 0.8
verbose = True
contactsToStack = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot))
contactsWorldFrame = contactsToStack[0:nc, :]
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])
iterProj = IterativeProjection()
#comWorldFrame = np.array([0.0, 0.0, 0.0])
#comZero = np.array([0.0, 0.0, 0.0])
#comWorldFrame = np.array([0.28975694, 0.04463657, 0.0])
optimalSolutionFound = True
comp_dyn = ComputationalDynamics()
iter = 0
tol = 0.02
errorNorm = np.zeros((0, 1))
while (optimalSolutionFound) & (iter < 50):
print "New tested CoM: ", comWorld
iter += 1
p, G, h, A, b, isConstraintOk, LP_actuation_polygons = comp_dyn.setup_lp(
trunk_mass, contactsWorldFrame, nc, numberOfGenerators, normals,
comWorld, constraint_mode, mu)
if not isConstraintOk:
#unfeasible_points = np.vstack([unfeasible_points, com_WF])
if verbose:
print 'something is wrong in the inequalities or the point is out of workspace'
else:
sol = solvers.lp(p, G, h, A, b)
x = sol['x']
status = sol['status']
#print x
if status == 'optimal':
print "state is Feasible!"
#feasible_points = np.vstack([feasible_points, com_WF])
#contact_forces = np.vstack([contact_forces, np.transpose(x)])
else:
print "state is UNfeasible!"
#unfeasible_points = np.vstack([unfeasible_points, com_WF])
new_lp, actuation_polygons, isOutOfWorkspace = iterProj.setup_iterative_projection(
contactsWorldFrame, comWorld, trunk_mass, mu, normals)
if isOutOfWorkspace:
optimalSolutionFound = False
print "Point is out of workspace!"
else:
lp_q, lp_Gextended, lp_hextended, lp_A, lp_b = new_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:]
forces = x[0:10]
print "New solution point: ", tempSolution
#print forces
#print actuation_polygons[0:3]
#print "LP actuation polygons: ", LP_actuation_polygons[0:3]/trunk_mass
delta_x = tempSolution[0] - comWorld[0]
delta_y = tempSolution[1] - comWorld[1]
tempSolutionNorm = norm(tempSolution)
comWorldFrameNorm = norm(comWorld[0:2])
errorNorm = np.vstack(
[errorNorm, tempSolutionNorm - comWorldFrameNorm])
print 'eer', tempSolutionNorm - comWorldFrameNorm
#if (np.abs(tempSolution[0])<np.abs(comWorldFrame[0]))|(np.abs(tempSolution[1])<np.abs(comWorldFrame[1])):
if (np.abs(tempSolutionNorm - comWorldFrameNorm) < tol):
optimalSolutionFound = False
else:
stepIncrement = 0.1
comWorld[0] += delta_x * stepIncrement
comWorld[1] += delta_y * stepIncrement
#return tempSolution, errorNorm
return comWorld[0:2], errorNorm
max_iter -= 1
if len(init_vertices) < 3:
raise Exception("problem is not linearly feasible")
v0 = VertexVariableConstraints(init_vertices[0])
v1 = VertexVariableConstraints(init_vertices[1])
v2 = VertexVariableConstraints(init_vertices[2])
polygon = PolygonVariableConstraint()
polygon.from_vertices(v0, v1, v2)
polygon.iter_expand(max_iter)
return polygon
''' MAIN '''
start_t_IPVC = time.time()
math = Math()
compDyn = ComputationalDynamics()
# 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
trunk_mass = 100
mu = 0.8
# number of decision variables of the problem
n = nc * 6
""" contact points """
LF_foot = np.array([0.3, 0.3, -0.5])
RF_foot = np.array([0.3, -0.3, -0.5])
n4 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) normals = np.vstack([n1, n2, n3, n4]) 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.3, 0.0]) # fig = plt.figure() nc = np.sum(stanceFeet) plt.plot(contacts[0:nc, 0], contacts[0:nc, 1], 'ko', markersize=15) comp_dyn = ComputationalDynamics() params = IterativeProjectionParameters() params.setContactsPosBF(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) ''' compute iterative projection ''' feasible_points = np.zeros((0, 3)) unfeasible_points = np.zeros((0, 3)) contact_forces = np.zeros((0, nc * 3))
''' torque limits for each leg (this code assumes a hyq-like design, i.e. three joints per leg)
HAA = Hip Abduction Adduction
HFE = Hip Flextion Extension
KFE = Knee Flextion Extension
'''
LF_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE
RF_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE
LH_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE
RH_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE
torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim])
''' extForceW is an optional external pure force (no external torque for now) applied on the CoM of the robot.'''
extForceW = np.array([0.0, 0.0, 0.0]) # units are Nm
comp_dyn = ComputationalDynamics()
'''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
