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]) params = IterativeProjectionParameters() params.setContactsPos(contacts) 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(
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)
(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 = compDyn.iterative_projection_bretl( constraint_mode, contacts, normals, trunk_mass, ng, mu) feasible, unfeasible, contact_forces = compDyn.LP_projection( constraint_mode, contacts, normals, trunk_mass, mu, ng, nc, mu, useVariableJacobian, 0.025, 0.025) #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]") h1 = plt.plot(contacts[0:nc, 0], contacts[0:nc, 1], 'ko', markersize=15,
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)
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: # ax.scatter(feasible[:,0], feasible[:,1], feasible[:,2],c='g',s=50) #if np.size(unfeasible,0) != 0: # ax.scatter(unfeasible[:,0], unfeasible[:,1], unfeasible[:,2],c='r',s=50)