plt.plot(sensor_readings[5, 10:], label="torque z [Nm]")
plt.xlabel("number of iterations (adding up to 10 seconds)")
plt.legend()
plt.subplot(122)
plt.title("Deviations from desired pose")
plt.plot(pose_error[0, 10:], label="deviation x [m]")
plt.plot(pose_error[1, 10:], label="deviation y [m]")
plt.plot(pose_error[2, 10:], label="deviation z [m]")
plt.plot(pose_error[3, 10:], label="quaternion x")
plt.plot(pose_error[4, 10:], label="quaternion y")
plt.plot(pose_error[5, 10:], label="quaternion z")
plt.xlabel("number of iterations (adding up to 10 seconds)")
plt.legend()
plt.show()
if __name__ == "__main__":
rospy.init_node("ts_control_sim_only")
#rospy.on_shutdown(_on_shutdown)
publish_rate = 500
rate = rospy.Rate(publish_rate)
robot = ArmInterface()
print('Moving to starting position')
robot.move_to_joint_positions(starting_position)
#robot.move_to_neutral()
impedance_control(rate, K_Pt, K_Po, K_m, K_d)
return dofs
if __name__ == '__main__':
rospy.init_node("path_testing")
realarm = ArmInterface()
q0 = realarm.convertToList(realarm.joint_angles())
startq = realarm.joint_angles()
while True:
# Rather than peturbing pose, perturb q as a hack
delta_q = numpy.random.rand(7) * 0.2
q1_seed = numpy.add(q0, delta_q)
raw_input("seed")
realarm.move_to_joint_positions(realarm.convertToDict(q1_seed))
p1 = realarm.endpoint_pose()
raw_input("q0")
realarm.move_to_joint_positions(realarm.convertToDict(q0))
p0 = realarm.endpoint_pose()
raw_input("p1")
realarm.set_cart_impedance_pose(p1)
raw_input("record?")
p2 = realarm.endpoint_pose()
q1 = realarm.joint_angles()
all_results = [q0, realarm.convertToList(q1), p0, p1, p2]
raw_input("next?")
break
with open('t28.npy', 'wb') as f:
plt.plot(pose_error[1,10:], label = "deviation y [m]")
plt.plot(pose_error[2,10:], label = "deviation z [m]")
plt.plot(pose_error[3,10:], label = "quaternion x")
plt.plot(pose_error[4,10:], label = "quaternion y")
plt.plot(pose_error[5,10:], label = "quaternion z")
plt.xlabel("number of iterations (adding up to 10 seconds)")
plt.legend()
plt.show()
"""
if __name__ == "__main__":
rospy.init_node("my_very_own_node")
#rospy.on_shutdown(_on_shutdown)
publish_rate = 500
rate = rospy.Rate(publish_rate)
robot = ArmInterface()
print('Moving to starting position')
robot.move_to_joint_positions(next_to_dummy)
#robot.move_to_neutral()
impedance_control(rate,K_Pt,K_Po,K_m,K_d)
plt.plot(pose_error[1,10:], label = "deviation y [m]")
plt.plot(pose_error[2,10:], label = "deviation z [m]")
plt.plot(pose_error[3,10:], label = "quaternion x")
plt.plot(pose_error[4,10:], label = "quaternion y")
plt.plot(pose_error[5,10:], label = "quaternion z")
plt.xlabel("number of iterations (adding up to 10 seconds)")
plt.legend()
plt.show()
if __name__ == "__main__":
rospy.init_node("ts_control_sim_only")
#rospy.on_shutdown(_on_shutdown)
publish_rate = 500
rate = rospy.Rate(publish_rate)
robot = ArmInterface()
print('Moving to starting position')
robot.move_to_joint_positions(new_start)
#robot.move_to_neutral()
impedance_control(rate,K_Pt,K_Po,K_m,K_d)
