Sim-Biped-Walker/testCode.py at main · shepai/Sim-Biped-Walker · 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
import pybullet as p
import pybullet_data
import time
import random
import keyboard  # using module keyboard
# Initialize PyBullet
physicsClient = p.connect(p.GUI)  # or p.DIRECT for non-graphical version
path="C:/Users/dexte/Documents/GitHub/Sim-Biped-Walker/URDF_files/walker_assembly/"
p.setAdditionalSearchPath(path)  # Set the path for urdf files
p.setAdditionalSearchPath(pybullet_data.getDataPath())  # Set the path for urdf files
# Load ground plane
p.loadURDF("plane.urdf",[0, 0, 0])  # Assuming there's a plane.urdf file in the pybullet data path
# Load URDF file
robot_urdf_path = "urdf/walker_assembly.urdf"  # Replace with the actual path to your URDF file
try:
    robot_id = p.loadURDF(path+robot_urdf_path, [0, 0, 0])  # Load URDF at position [0, 0, 0]

except p.error as e:
    print("error loading file",e)
    p.disconnect()
try:
    file=open(path+robot_urdf_path,"r")
    r=file.read()
    file.close()
    print("File exists")
except:
    print("cannot find file")
gravity = [0, 0, -9.8]
p.setGravity(gravity[0], gravity[1], gravity[2])
# Move joint at index 0 (assuming it's a revolute joint)
joint_index_to_move = 0


# Set the initial joint position to avoid a sudden jump
#p.resetJointState(robot_id, joint_index_to_move, targetValue=desired_joint_position)

# Simulation loop
for _ in range(5000):  # Run the simulation for 1000 steps (adjust as needed)
    p.stepSimulation()
    if keyboard.is_pressed('1'):  # if key 'q' is pressed qqqqq q
        joint_state = p.getJointState(robot_id, 0)
        joint_position = joint_state[0]
        p.setJointMotorControl2(robot_id,
                                0,
                                p.POSITION_CONTROL,
                                targetPosition=joint_position+0.1,
                                force=500)
    elif keyboard.is_pressed('2'):  # if key 'q' is pressed qqqqq q
        joint_state = p.getJointState(robot_id, 1)
        joint_position = joint_state[0]
        p.setJointMotorControl2(robot_id,
                                1,
                                p.POSITION_CONTROL,
                                targetPosition=joint_position+0.1,
                                force=500)
    elif keyboard.is_pressed('3'):  # if key 'q' is pressed qqqqq q
        joint_state = p.getJointState(robot_id, 2)
        joint_position = joint_state[0]
        p.setJointMotorControl2(robot_id,
                                2,
                                p.POSITION_CONTROL,
                                targetPosition=joint_position+0.1,
                                force=500)
    elif keyboard.is_pressed('4'):  # if key 'q' is pressed qqqqq q
        joint_state = p.getJointState(robot_id, 3)
        joint_position = joint_state[0]
        p.setJointMotorControl2(robot_id,
                                3,
                                p.POSITION_CONTROL,
                                targetPosition=joint_position+0.1,
                                force=500)
    elif keyboard.is_pressed('5'):  # if key 'q' is pressed qqqqq q
        joint_state = p.getJointState(robot_id, 0)
        joint_position = joint_state[0]
        p.setJointMotorControl2(robot_id,
                                0,
                                p.POSITION_CONTROL,
                                targetPosition=joint_position-0.1,
                                force=500)
    elif keyboard.is_pressed('6'):  # if key 'q' is pressed qqqqq q
        joint_state = p.getJointState(robot_id, 1)
        joint_position = joint_state[0]
        p.setJointMotorControl2(robot_id,
                                1,
                                p.POSITION_CONTROL,
                                targetPosition=joint_position-0.1,
                                force=500)
    elif keyboard.is_pressed('7'):  # if key 'q' is pressed qqqqq q
        joint_state = p.getJointState(robot_id, 2)
        joint_position = joint_state[0]
        p.setJointMotorControl2(robot_id,
                                2,
                                p.POSITION_CONTROL,
                                targetPosition=joint_position-0.1,
                                force=500)
    elif keyboard.is_pressed('8'):  # if key 'q' is pressed qqqqq q
        joint_state = p.getJointState(robot_id, 3)
        joint_position = joint_state[0]
        p.setJointMotorControl2(robot_id,
                                3,
                                p.POSITION_CONTROL,
                                targetPosition=joint_position-0.1,
                                force=500)
    time.sleep(1./240.)

p.disconnect()
print("Finished")