예제 #1
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])
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(
예제 #2
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)
예제 #3
0
      (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,
예제 #4
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)
예제 #5
0
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)