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_PDControl():
model = load_model_from_path("robot.xml")
sim = MjSim(model)
viewer = MjViewer(sim)
timestep = generatePatternedTrajectories.TIMESTEP
control_freq = 1/timestep
total_time = 3
num_cycles = int(total_time * control_freq)
qd_init = np.zeros(7)
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([bounds.LOWER_BOUND[i], bounds.UPPER_BOUND[i]])
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()
q_goal = bounds.getRandPosInBounds()
for g in range(7):
goals[g].set_ydata(np.ones(num_cycles) * q_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.PDControl(q=q,qd=qd,qgoal=q_goal)
sim.step()
viewer.render()
if i % 70 == 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(), q[a]))
fig.canvas.draw()
fig.canvas.flush_events()
sim_time += timestep
for i in range(7):
lines[i].set_xdata([])
lines[i].set_ydata([])
time.sleep(1)
def test_inverse_kinematics_pos():
ik = ikpy_panda_kinematics.panda_kinematics()
print("\nTest 1: First 3 elements of xpos should be the same.")
pos = np.zeros(7)
xpos = ik.forward_kinematics(pos)
R1 = ik.euler_angles_to_rpy_rotation_matrix(xpos[3:6])
print("xpos:", np.around(xpos,3))
pos = ik.inverse_kinematics(xpos[0:3], xpos[3:6])
print("pos: ", np.around(pos,3))
xpos = ik.forward_kinematics(pos)
R2 = ik.euler_angles_to_rpy_rotation_matrix(xpos[3:6])
print("xpos:", np.around(xpos,3))
print("\nThe 2 following matricies should be the same.")
print(np.around(R1,3),"\n\n",np.around(R1,3))
print("\nTest 2: First 3 elements of xpos should be the same.")
pos = bounds.getRandPosInBounds()
xpos = ik.forward_kinematics(pos)
R1 = ik.euler_angles_to_rpy_rotation_matrix(xpos[3:6])
print("xpos:", np.around(xpos,3))
pos = ik.inverse_kinematics(xpos[0:3], xpos[3:6])
print("pos: ", np.around(pos,3))
xpos = ik.forward_kinematics(pos)
R2 = ik.euler_angles_to_rpy_rotation_matrix(xpos[3:6])
print("xpos:", np.around(xpos,3))
print("\nThe 2 following matricies should be the same.")
print(np.around(R1,3),"\n\n",np.around(R1,3))
def test_inverse_kinematics_no_plot():
panda_kinematics = ikpy_panda_kinematics.panda_kinematics()
xpos = panda_kinematics.forward_kinematics(np.zeros(7))
theta = panda_kinematics.inverse_kinematics(xpos[0:3], xpos[3:6])
print(np.around(xpos,4))
print(np.around(panda_kinematics.forward_kinematics(theta),4))
xpos = panda_kinematics.forward_kinematics(np.ones(7))
theta = panda_kinematics.inverse_kinematics(xpos[0:3], xpos[3:6])
print(np.around(xpos,4))
print(np.around(panda_kinematics.forward_kinematics(theta),4))
xpos = panda_kinematics.forward_kinematics(bounds.getRandPosInBounds())
theta = panda_kinematics.inverse_kinematics(xpos[0:3], xpos[3:6])
print(np.around(xpos,4))
print(np.around(panda_kinematics.forward_kinematics(theta),4))
def test_inverse_kinematics():
panda_kinematics = ikpy_panda_kinematics.panda_kinematics()
model = load_model_from_path("robot.xml")
sim = MjSim(model)
viewer = MjViewer(sim)
timestep = generatePatternedTrajectories.TIMESTEP
control_freq = 1/timestep
total_time = 3
num_cycles = int(total_time * control_freq)
qd_init = np.zeros(7)
plt.ion()
LW = 1.0
fig = plt.figure(figsize=(4,15))
axes = []
lines = []
goals = []
min_vals = {'x': -0.5, 'y': -0.5, 'z': 0.2}
max_vals = {'x': 0.5, 'y': 0.5, 'z': 0.7}
ylabels = ["x(m)", "y(m)", "z(m)"]
ylbounds = [min_vals['x'], min_vals['y'], min_vals['z']]
yubounds = [max_vals['x'], max_vals['y'], max_vals['z']]
for i in range(3):
axes.append(fig.add_subplot(3,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([ylbounds[i], yubounds[i]])
axes[i].set_xlim([0,total_time])
axes[i].set_ylabel(ylabels[i], fontsize=8)
axes[i].set_xlabel("Time(s)", fontsize=8)
for test in range(5):
x_target = np.random.rand() * (max_vals['x'] - min_vals['x']) + min_vals['x']
y_target = np.random.rand() * (max_vals['y'] - min_vals['y']) + min_vals['y']
z_target = np.random.rand() * (max_vals['z'] - min_vals['z']) + min_vals['z']
if\
min_vals['x'] > x_target or max_vals['x'] < x_target or \
min_vals['y'] > y_target or max_vals['y'] < y_target or \
min_vals['z'] > z_target or max_vals['z'] < z_target:
print("Error! 'xpos' out of range!")
print("x = %.3f\ny = %.3f\nz = %.3f" % (x_target, y_target, z_target))
print(max_vals['y'] - min_vals['y'])
return
# x_target, y_target, z_target = 0.088, -0.0, 0.926
# roll = np.random.rand() * np.pi - np.pi/2
# pitch = np.random.rand() * np.pi - np.pi/2
# yaw = np.random.rand() * np.pi - np.pi/2
roll = 0
pitch = 0
yaw = np.pi
ee_goal = [x_target, y_target, z_target, roll, pitch, yaw]
# temp = bounds.getRandPosInBounds()
# ee_goal = panda_kinematics.forward_kinematics(temp)
q_init = bounds.getRandPosInBounds()
q_goal = panda_kinematics.inverse_kinematics(translation=ee_goal[0:3], rpy=ee_goal[3:6], init_qpos=q_init)
for g in range(3):
goals[g].set_ydata(np.ones(num_cycles) * ee_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.PDControl(q=q,qd=qd,qgoal=q_goal)
sim.step()
viewer.render()
if i % 70 == 0:
xpos = panda_kinematics.forward_kinematics(q)
for a in range(3):
lines[a].set_xdata(np.append(lines[a].get_xdata(), sim_time))
lines[a].set_ydata(np.append(lines[a].get_ydata(), xpos[a]))
fig.canvas.draw()
fig.canvas.flush_events()
print("[q\t]:{}".format(np.around(q,3)))
print("[qgoal\t]:{}".format(np.around(q_goal,3)))
print("[qgoal2\t]:{}".format(np.around(panda_kinematics.inverse_kinematics(ee_goal[0:3], ee_goal[3:6]),3)))
print("[EE\t]:{}".format(np.around(panda_kinematics.forward_kinematics(q),3)))
print("[EEgoal\t]:{}".format(np.around(ee_goal,3)))
sim_time += timestep
# panda_kinematics.plot_stick_figure(q)
for i in range(3):
lines[i].set_xdata([])
lines[i].set_ydata([])
time.sleep(1)