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 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
#contacts = contactsToStack[0:nc+1, :] #print contacts ''' parameters to be tuned''' g = 9.81 trunk_mass = 65. mu = 0.9 stanceFeet = [1, 1, 1, 0] randomSwingLeg = random.randint(0, 3) print 'Swing leg', randomSwingLeg print 'stanceLegs ', stanceFeet 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)) 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()
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
comp_dyn = ComputationalDynamics() number_of_tests = 1000 tests3contacts = np.zeros((number_of_tests)) tests4contacts = np.zeros((number_of_tests)) params = IterativeProjectionParameters() for iter in range(0, number_of_tests): ''' random normals ''' randRoll = np.random.normal(0.0, 0.2) randPitch = np.random.normal(0.0, 0.2) randYaw = np.random.normal(0.0, 0.2) n1 = np.transpose( np.transpose(math.rpyToRot(randRoll, randPitch, randYaw)).dot(axisZ)) randRoll = np.random.normal(0.0, 0.2) randPitch = np.random.normal(0.0, 0.2) randYaw = np.random.normal(0.0, 0.2) n2 = np.transpose( np.transpose(math.rpyToRot(randRoll, randPitch, randYaw)).dot(axisZ)) randRoll = np.random.normal(0.0, 0.2) randPitch = np.random.normal(0.0, 0.2) randYaw = np.random.normal(0.0, 0.2) n3 = np.transpose( np.transpose(math.rpyToRot(randRoll, randPitch, randYaw)).dot(axisZ)) normals = np.vstack([n1, n2, n3]) """ contact points """ sigma = 0.05 # mean and standard deviation randX = np.random.normal(0.3, sigma) randY = np.random.normal(0.2, sigma)