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
Пример #4
0
#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()
Пример #5
0
    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)
Пример #6
0
    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()
Пример #7
0
    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        
Пример #10
0
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)