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
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
''' compute iterative projection
Outputs of "iterative_projection_bretl" are:
IP_points = resulting 2D vertices
actuation_polygons = these are the vertices of the 3D force polytopes (one per leg)
computation_time = how long it took to compute the iterative projection
'''
IP_points, force_polytopes, IP_computation_time = comp_dyn.iterative_projection_bretl(params)
''' plotting Iterative Projection points '''
feasible, unfeasible, contact_forces = comp_dyn.LP_projection(params)
#print contact_forces
#for i in range(0, np.size(contact_forces,0)):
# for j in range(0,nc):