dynamic_robot/control.py at master · chaojie-fu/dynamic_robot · 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
import pybullet as p
import time
import pybullet_data
import numpy as np
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(numSolverIterations=50)
p.setTimeOut(40000000)
p.setTimeStep(1./100)

planeId = p.loadURDF("plane.urdf")
robotStartPos = [0, 0, 0.5]
robotStartOrientation = p.getQuaternionFromEuler([0,0,0])
robotId = p.loadURDF("robot.urdf", robotStartPos, robotStartOrientation)

nJoints = p.getNumJoints(robotId)
jointNameToId = {}
for j in range(nJoints):
    jointInfo = p.getJointInfo(robotId, j)
    jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]

bodyLF = jointNameToId['bodyLF']
bodyRF = jointNameToId['bodyRF']
bodyLB = jointNameToId['bodyLB']
bodyRB = jointNameToId['bodyRB']
hipLF = jointNameToId['hipLF']
hipRF = jointNameToId['hipRF']
hipLB = jointNameToId['hipLB']
hipRB = jointNameToId['hipRB']
kneeLF = jointNameToId['kneeLF']
kneeRF = jointNameToId['kneeRF']
kneeLB = jointNameToId['kneeLB']
kneeRB = jointNameToId['kneeRB']

legnumbering = [
    bodyLF, bodyRF, bodyLB, bodyRB,
    hipLF, hipRF, hipLB, hipRB,
    kneeLF, kneeRF, kneeLB, kneeRB
]

halfpi = 1.57079632679
twopi = 4 * halfpi
pi = 2 * halfpi
motordir = [halfpi, -halfpi, -halfpi, halfpi,
            halfpi, -halfpi, halfpi, -halfpi,
            -halfpi, halfpi, -halfpi, halfpi
]

para = [
    0.2, 0.2, 0.2, 0.2,
    0.45, 0.45, 0.45, 0.45,
    1, 1, 1, 1
]

control = np.multiply(motordir,para)

#initialize
for i in range(12):
    p.setJointMotorControl2(
        bodyIndex=robotId,
        jointIndex=legnumbering[i],
        controlMode=p.POSITION_CONTROL,
        targetPosition=control[i],
        positionGain=1,
        velocityGain=0.5,
        force=10
    )

for i in range (10000):

    p.stepSimulation()
    time.sleep(1./100.)
p.disconnect()