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)