pybullet-resources/squat_analysis.py at master · PotentiaRobotics/pybullet-resources · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
import pybullet as p
import time
import pybullet_data
import math
import matplotlib.pyplot as plt
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version

p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-10)
planeId = p.loadURDF("plane.urdf")
startPos = [0,0,0]
startOrientation = p.getQuaternionFromEuler([0,0,0])
robotId = p.loadURDF("olympian.urdf",startPos, startOrientation,
                   # useMaximalCoordinates=1, ## New feature in Pybullet
                   flags=p.URDF_USE_INERTIA_FROM_FILE)

print("==========SIMULATION ENABLED================")

#GET JOINT INFO

print(p.getNumJoints(robotId))
for i in range(0, p.getNumJoints(robotId)):
    print(p.getJointInfo(robotId, i)[0:13])

listOfJointIndeces = []
for i in range(0, p.getNumJoints(robotId)):
    listOfJointIndeces.append(i)


def calcCOM():

    #CALCULATE COM
    masstimesxpossum = 0.0
    masstimesypossum = 0.0
    masstimeszpossum = 0.0
    masssum = 0.0
    for i in range(0, p.getNumJoints(robotId) -1):

        # if(i >= 0):
        #     print(p.getJointInfo(robotId, i)[0:13])

        wheight = p.getDynamicsInfo(robotId, i)[0]
        xpos = p.getLinkState(robotId, i)[0][0]
        ypos = p.getLinkState(robotId, i)[0][1]
        zpos = p.getLinkState(robotId, i)[0][2]

        masstimesxpossum += (wheight * xpos)
        masstimesypossum += (wheight * ypos)
        masstimeszpossum += (wheight * zpos)
        masssum += wheight

        # print(wheight)
        # print(xpos)
        # print(ypos)
        # print(zpos)
        # print("\n")
        p.stepSimulation()
    com = (masstimesxpossum/masssum, masstimesypossum/masssum, masstimeszpossum/masssum)

    print("mass: " + str(masssum))
    print("center of mass: " + str(com))
    print("\n")
    return com

newi = []

torque = 50
comPos = []
comPos2 = []
def squatDown():

    for i in range(0,30):
        comPos.append(calcCOM())
        newi.insert(0, i)
        positionsList = [0] * 23
        positionsList[11] = i/100 * 1 * math.pi #leg pitch 1
        positionsList[17] = i/100 * -1 * math.pi #leg pitch 2

        positionsList[14] = i/50 * math.pi #leg knee 1
        positionsList[20] = i/50 * -1  * math.pi #leg knee 2

        positionsList[16] = i/100 * 1  * math.pi#foot pitch 1
        positionsList[22] = i/100 * -1  * math.pi#foot pitch 2

        # positionsList[0] = math.pi / 4
        forceArray = [torque]*23

        p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, targetPositions = positionsList, forces = forceArray)
        p.stepSimulation()


def squatUp():
    for i in newi:
        comPos2.append(calcCOM())
        positionsList = [0] * 23
        positionsList[11] = i/100 * 1 * math.pi #leg pitch 1
        positionsList[17] = i/100 * -1 * math.pi #leg pitch 2

        positionsList[14] = i/50 * math.pi #leg knee 1
        positionsList[20] = i/50 * -1  * math.pi #leg knee 2

        positionsList[16] = i/100 * 1  * math.pi#foot pitch 1
        positionsList[22] = i/100 * -1  * math.pi#foot pitch 2

        # positionsList[0] = math.pi / 4
        forceArray = [torque]*23

        p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, targetPositions = positionsList, forces = forceArray)
        p.stepSimulation()

squatDown()
squatUp()


plt.plot(newi, comPos)
for i in newi[:-1]:
    newi[i] += 30
plt.plot(newi, comPos2)
plt.show()

# for i in range(0,1000000):
#     p.stepSimulation()
#     time.sleep(1./240.)