def test_getRandPosInBounds():
test1 = bounds.getRandPosInBounds()
test2 = bounds.getRandPosInBounds()
test3 = bounds.getRandPosInBounds()
test4 = bounds.getRandPosInBounds()
print(np.around(test1,4))
print(np.around(test2,4))
print(np.around(test3,4))
print(np.around(test4,4))
print("Test1:", "Passed" if not bounds.outOfBounds(test1) else "Failed")
print("Test2:", "Passed" if not bounds.outOfBounds(test2) else "Failed")
print("Test3:", "Passed" if not bounds.outOfBounds(test3) else "Failed")
print("Test4:", "Passed" if not bounds.outOfBounds(test4) else "Failed")
def test_outOfBounds():
ub = np.array(bounds.UPPER_BOUND)
lb = np.array(bounds.LOWER_BOUND)
test1 = np.zeros(7)
test1[[1,3,5]] = ub[[1,3,5]] + 0.1
test1[[2,6,0]] = lb[[2,6,0]] - 0.1
test1[4] = ub[4] - 1
test2 = np.zeros(7)
test2[:] = ub[:] - 1
test2[4] = lb[4] - 0.1
test3 = [-0.0031, -0.9718, -0.7269, -2.4357, -0.0157, 1.5763, 0.7303]
test4 = [-0.5250, -0.6410, 0.1893, -1.3827, -0.2573, 2.1356, 0.7116]
print("Test1:", "Passed" if bounds.outOfBounds(test1) else "Failed")
print("Test2:", "Passed" if bounds.outOfBounds(test2) else "Failed")
print("Test3:", "Passed" if not bounds.outOfBounds(test3) else "Failed")
print("Test4:", "Passed" if not bounds.outOfBounds(test4) else "Failed")
def test_dampingControl():
model = load_model_from_path("robot.xml")
sim = MjSim(model)
viewer = MjViewer(sim)
timestep = generatePatternedTrajectories.TIMESTEP
control_freq = 1/timestep
total_time = 2
num_cycles = int(total_time * control_freq)
plt.ion()
LW = 1.0
fig = plt.figure(figsize=(4,15))
axes = []
lines = []
goals = []
for i in range(7):
axes.append(fig.add_subplot(7,1,i+1))
lines.append(axes[i].plot([],[],'b-', lw=LW)[0])
goals.append(axes[i].plot([],[],'r-', lw=LW)[0])
axes[i].set_ylim([-1, 1])
axes[i].set_xlim([0,total_time])
axes[i].set_ylabel("Angle{}(rad)".format(i), fontsize=8)
axes[i].set_xlabel("Time(s)", fontsize=8)
for test in range(5):
q_init = bounds.getRandPosInBounds()
qd_goal = np.zeros(7)
qd_init = np.random.rand(7)
for g in range(7):
goals[g].set_ydata(np.ones(num_cycles) * qd_goal[g])
goals[g].set_xdata(np.linspace(0,3,num_cycles))
sim.set_state(MjSimState(time=0,qpos=q_init,qvel=qd_init,act=None,udd_state={}))
sim.step()
sim_time = 0
for i in range(num_cycles):
state = sim.get_state()
q = state[1]
qd = state[2]
sim.data.ctrl[:] = controllers.dampingControl(qd=qd)
sim.step()
viewer.render()
if i % 35 == 0:
for a in range(7):
lines[a].set_xdata(np.append(lines[a].get_xdata(), sim_time))
lines[a].set_ydata(np.append(lines[a].get_ydata(), qd[a]))
fig.canvas.draw()
fig.canvas.flush_events()
sim_time += timestep
if bounds.outOfBounds(q):
break
for i in range(7):
lines[i].set_xdata([])
lines[i].set_ydata([])
time.sleep(1)
def main(data_dir):
model = load_model_from_path("robot.xml")
sim = MjSim(model)
viewer = MjViewer(sim)
initial_state = MjSimState(time=0,
qpos=np.array([
0, -np.pi / 4, 0, -3 * np.pi / 4 + 1, 0,
np.pi / 2, np.pi / 4
]),
qvel=np.zeros(7),
act=None,
udd_state={})
sim.set_state(initial_state)
sim.step()
traj_count = 0
control_state = None
while (traj_count < NUM_TRAJECTORIES and control_state != FINISHED):
control_state = ACCELERATING
outputFile = None
initial_q = sim.get_state()[q_INDEX]
velGen = randVelGen.RandVelGenerator()
qd_des = velGen.generatePatternVel()
coming_back_time = 0.0
time = 0
while control_state != FINISHED:
state = sim.get_state()
q = state[q_INDEX]
qd = state[qd_INDEX]
boundViolations = bounds.getBoundViolations(q)
# RD =
TB = bounds.tableBoundViolation(sim)
OB = bounds.outOfBounds(q)
DA = helperFun.moving(qd)
DC = helperFun.stopped(qd)
DD = DC
DB = coming_back_time > RETURN_TIME
FN = traj_count >= NUM_TRAJECTORIES
prev_state = control_state
# transition block
if control_state == ACCELERATING:
if not TB and not OB and not DA:
control_state = ACCELERATING
elif TB:
control_state = COMING_BACK_IN_BOUNDS
coming_back_time = 0.0
elif not TB and OB:
control_state = DAMPING
curBoundViolations = bounds.getBoundViolations(q)
velGen.setJointDirections(curBoundViolations)
elif not TB and not OB and DA:
control_state = COASTING
traj_count += 1
outputFile = open(data_dir +
helperFun.getUniqueFileName("traj"),
mode='x')
outputWriter = csv.writer(outputFile, delimiter=',')
print_count(traj_count)
else:
control_state = FINISHED
print("Unknown transistion! ACCELERATING")
elif control_state == COASTING:
if not FN and not TB and not OB and DC:
control_state = ACCELERATING
qd_des = velGen.generatePatternVel()
outputFile.close()
elif not FN and TB:
control_state = COMING_BACK_IN_BOUNDS
coming_back_time = 0
outputFile.close()
elif not FN and not TB and OB:
control_state = DAMPING
outputFile.close()
curBoundViolations = bounds.getBoundViolations(q)
velGen.setJointDirections(curBoundViolations)
elif FN:
control_state = FINISHED
outputFile.close()
elif not FN and not TB and not OB and not DC:
control_state = COASTING
else:
control_state = FINISHED
print("Unknown transition! COASTING")
outputFile.close()
elif control_state == DAMPING:
if not TB and not DD:
control_state = DAMPING
elif TB:
control_state = COMING_BACK_IN_BOUNDS
coming_back_time = 0.0
elif not TB and DD:
control_state = ACCELERATING
qd_des = velGen.generatePatternVel()
else:
control_state = FINISHED
print("Unknow transition! DAMPING")
elif control_state == COMING_BACK_IN_BOUNDS:
if not DB:
control_state = COMING_BACK_IN_BOUNDS
elif DB and OB:
control_state = DAMPING
curBoundViolations = bounds.getBoundViolations(q)
velGen.setJointDirections(curBoundViolations)
elif DB and not OB:
control_state = ACCELERATING
qd_des = velGen.generatePatternVel()
else:
control_state = FINISHED
print("Unknown transition! COMING_BACK_IN_BOUNDS")
elif control_state == FINISHED:
control_state = FINISHED
else:
control_state = FINISHED
print("Got to an invalid state!")
# debug states
if prev_state != control_state:
if control_state == ACCELERATING:
print("ACCELERATING")
elif control_state == COASTING:
print("COASTING")
elif control_state == DAMPING:
print("DAMPING")
elif control_state == COMING_BACK_IN_BOUNDS:
print("COMING_BACK_IN_BOUNDS")
elif control_state == "FINISHED":
print("FINISHED")
else:
print("In a bad state!")
torques = np.zeros(7)
if control_state == ACCELERATING:
torques = controllers.basicVelControl(qd_des=qd_des, qd_cur=qd)
elif control_state == COASTING:
data = np.concatenate(
(q, qd, data_calc.get_3D_data(sim), [time]))
outputWriter.writerow(data)
torques = controllers.basicVelControl(qd_des=qd_des, qd_cur=qd)
elif control_state == DAMPING:
torques = controllers.dampingControl(qd)
elif control_state == COMING_BACK_IN_BOUNDS:
coming_back_time += TIMESTEP
torques = controllers.moveAwayFromTable(q=q, qd=qd)
elif control_state == FINISHED:
outputFile.close()
break
else:
print("Got to an invalid state!")
control_state = FINISHED
break
sim.data.ctrl[:] = torques
sim.step()
viewer.render()
time += TIMESTEP