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 #square #predictedLF_foot = np.add(LF_foot,np.array([0.1,0.3,0.0])) #footOption0 = [0., 0., 0.] + predictedLF_foot
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