-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathsimulation.py
More file actions
133 lines (99 loc) · 4.8 KB
/
Copy pathsimulation.py
File metadata and controls
133 lines (99 loc) · 4.8 KB
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
125
126
127
128
129
130
131
132
133
from sim_class import Simulation
import pybullet as p
import numpy as np
import imageio
sim = Simulation(num_agents=1, rgb_array=True)
def torque_exceeds_threshold(torque, threshold=500):
print(torque)
return abs(torque) > threshold
def generate_actions(vel_x, vel_y, vel_z, drop):
return [[vel_x, vel_y, vel_z, drop]]
def get_boundaries(simulation, robot_id):
boundaries = {"x_min": None, "x_max": None, "y_min": None, "y_max": None}
frames = [] # List to store captured frames
while True:
actions = generate_actions(0.0, 0.0, 0.0, 0)
status = sim.run(actions)
joint_states = status['robotId_1']['joint_states']
torque_state_joint_0 = joint_states['joint_0']['motor_torque']
torque_above_thres_counter = 0
while torque_above_thres_counter <= 10:
actions = generate_actions(0.8, 0.0, 0.0, 0)
status = sim.run(actions, num_steps=1)
joint_states = status['robotId_1']['joint_states']
torque_state_joint_0 = joint_states['joint_0']['motor_torque']
# Capture the frame after each step
img = sim.get_frame()
frames.append(img) # Add frame to the list
if torque_exceeds_threshold(torque_state_joint_0):
torque_above_thres_counter += 1
boundaries["x_max"] = status['robotId_1']['robot_position'][0]
torque_above_thres_counter = 0
actions = generate_actions(-0.8, 0.0, 0.0, 0)
status = sim.run(actions, num_steps=1)
joint_states = status['robotId_1']['joint_states']
torque_state_joint_0 = joint_states['joint_0']['motor_torque']
while torque_above_thres_counter <= 10:
actions = generate_actions(-0.8, 0.0, 0.0, 0)
status = sim.run(actions, num_steps=1)
joint_states = status['robotId_1']['joint_states']
torque_state_joint_0 = joint_states['joint_0']['motor_torque']
# Capture the frame after each step
img = sim.get_frame()
frames.append(img) # Add frame to the list
if torque_exceeds_threshold(torque_state_joint_0):
torque_above_thres_counter += 1
boundaries["x_min"] = status['robotId_1']['robot_position'][0]
torque_above_thres_counter = 0
actions = generate_actions(0.8, 0.0, 0.0, 0)
status = sim.run(actions, num_steps=1)
joint_states = status['robotId_1']['joint_states']
torque_state_joint_1 = joint_states['joint_1']['motor_torque']
while torque_above_thres_counter <= 10:
actions = generate_actions(0.0, 0.8, 0.0, 0)
status = sim.run(actions, num_steps=1)
joint_states = status['robotId_1']['joint_states']
torque_state_joint_1 = joint_states['joint_1']['motor_torque']
# Capture the frame after each step
img = sim.get_frame()
frames.append(img) # Add frame to the list
if torque_exceeds_threshold(torque_state_joint_1):
torque_above_thres_counter += 1
boundaries["y_max"] = status['robotId_1']['robot_position'][1]
torque_above_thres_counter = 0
actions = generate_actions(0.0, -0.8, 0.0, 0)
status = sim.run(actions, num_steps=1)
joint_states = status['robotId_1']['joint_states']
torque_state_joint_1 = joint_states['joint_1']['motor_torque']
while torque_above_thres_counter <= 10:
actions = generate_actions(0.0, -0.8, 0.0, 0)
status = sim.run(actions, num_steps=1)
joint_states = status['robotId_1']['joint_states']
torque_state_joint_1 = joint_states['joint_1']['motor_torque']
# Capture the frame after each step
img = sim.get_frame()
frames.append(img) # Add frame to the list
if torque_exceeds_threshold(torque_state_joint_1):
torque_above_thres_counter += 1
boundaries["y_min"] = status['robotId_1']['robot_position'][1]
torque_above_thres_counter = 0
actions = generate_actions(0.0, 0.8, 0.0, 0)
status = sim.run(actions, num_steps=1)
joint_states = status['robotId_1']['joint_states']
torque_state_joint_1 = joint_states['joint_1']['motor_torque']
break
# Save captured frames as a GIF
if frames:
print(len(frames))
#frames = [f for i, f in enumerate(frames) if i % 40 == 0]
imageio.mimsave('simulation.gif', frames, duration=0.5)
return boundaries
if __name__ == '__main__':
actions = generate_actions(-0.5,0.5, 0, 1)
sim.run(actions, num_steps=200)
frame = sim.current_frame
print('frame shape:', frame.shape)
imageio.mimsave('simulation.gif', frame, duration=0.5)
print(frame)
#boundaries = get_boundaries(sim, 'robotId_1')
#print(boundaries)