np.argsort(comTrajectoriesToStack[:, 0], axis=None), comTrajectoriesToStack[:, 0].shape) max_motin_index = max_motion_indices[0][0] print("Directed Iterative Projection: --- %s seconds ---" % (time.time() - start_t_IPVC)) ''' plotting Iterative Projection points ''' 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]) IP_points, actuation_polygons, comp_time = compDyn.iterative_projection_bretl( constraint_mode, contacts, normals, trunk_mass, ng, mu) #IP_points_saturated_friction, actuation_polygons = compDyn.iterative_projection_bretl('ONLY_FRICTION', contacts, normals, trunk_mass, ng, mu, saturate_normal_force = True) '''Plotting''' plt.close('all') plotter = Plotter() plt.figure() plt.grid() plt.xlabel("X [m]") plt.ylabel("Y [m]") plt.plot(comTrajectoriesToStack[:, 0], comTrajectoriesToStack[:, 1], 'g^', markersize=20)
params.setCoMPos(comWF) params.setTorqueLims(torque_limits) params.setActiveContacts(stanceFeet) params.setConstraintModes(constraint_mode_IP) params.setContactNormals(normals) params.setFrictionCoefficient(mu) params.setNumberOfFrictionConesEdges(ng) params.setTrunkMass(trunk_mass) feasible, unfeasible, contact_forces = compDyn.LP_projection( params, useVariableJacobian, 0.2, 0.2) print feasible print unfeasible #desired_direction = [-1.0, -1.0] IP_points1, actuation_polygons, comp_time = compDyn.iterative_projection_bretl( params) comWF = np.array([-0.205, 0.1, 0.0]) params.setCoMPos(comWF) IP_points2, actuation_polygons, comp_time = compDyn.iterative_projection_bretl( params) comWF = np.array([-0.131, 0.1, 0.0]) params.setCoMPos(comWF) IP_points3, actuation_polygons, comp_time = compDyn.iterative_projection_bretl( params) comWF = np.array([-0.17, 0.1, 0.0]) params.setCoMPos(comWF) IP_points4, actuation_polygons, comp_time = compDyn.iterative_projection_bretl( params)
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
for com_x in range(lowel_lim, upper_lim, 10): # print com_x/100.0 comWF = np.array([com_x / 100.0, 0.0, 0.0]) # print comWF, comToStack comToStackX = np.hstack([comToStackX, comWF[0]]) comToStackY = np.hstack([comToStackY, comWF[1]]) # params.setCoMPos(comWF) LF_foot_tmp = LF_foot - comWF RF_foot_tmp = RF_foot - comWF LH_foot_tmp = LH_foot - comWF RH_foot_tmp = RH_foot - comWF contactsBF = np.vstack( (LF_foot_tmp, RF_foot_tmp, LH_foot_tmp, RH_foot_tmp)) params.setContactsPosWF(contactsBF) IP_points, actuation_polygons, comp_time = comp_dyn.iterative_projection_bretl( params) point = np.vstack([IP_points]) # print idx, scale # print scale[idx] colorVal = scalarMap.to_rgba(scale[idx]) colorText = ('color: (%4.2f,%4.2f,%4.2f)' % (colorVal[0], colorVal[1], colorVal[2])) idx += 1 #plotter.plot_polygon(np.transpose(IP_points), x[0],'trunk mass ' + str(trunk_mass*10) + ' N') x = np.hstack([point[:, 0], point[0, 0]]) y = np.hstack([point[:, 1], point[0, 1]]) h = plt.plot(x, y, color=colorVal, linewidth=5., label=str(com_x / 100.0) + ' cm')
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): # a = Arrow3D([contacts[j,0], contacts[j,0]+contact_forces[i,j*3]/200], [contacts[j,1], contacts[j,1]+contact_forces[i,j*3+1]/200],[contacts[j,2], contacts[j,2]+contact_forces[i,j*3+2]/200], mutation_scale=20, lw=3, arrowstyle="-|>", color="g") # ax.add_artist(a) #a1 = Arrow3D([0.0, 0.0],[ 0.0,0.0],[ 0.0, -trunk_mass*g/scaling_factor], mutation_scale=20, lw=3, arrowstyle="-|>", color="b") #ax.add_artist(a1) ''' plotting LP test points ''' #if np.size(feasible,0) != 0: