if __name__ == "__main__": # global goal_pos, goal_ori, ctrl_thread rospy.init_node("ts_control_sim_only") # when using franka_ros_interface, the robot can be controlled through and # the robot state is directly accessible with the API # If using the panda_robot API, this will be # panda = PandaArm() robot = ArmInterface() # when using the panda_robot interface, the next 2 lines can be simplified # to: `start_pos, start_ori = panda.ee_pose()` ee_pose = robot.endpoint_pose() start_pos, start_ori = ee_pose['position'], ee_pose['orientation'] goal_pos, goal_ori = start_pos, start_ori # start controller thread rospy.on_shutdown(_on_shutdown) rate = rospy.Rate(publish_rate) ctrl_thread = threading.Thread(target=control_thread, args = [rate]) ctrl_thread.start() # ------------------------------------------------------------------------------------ server = InteractiveMarkerServer("basic_control") position = Point( start_pos[0], start_pos[1], start_pos[2]) marker = destination_marker.makeMarker( False, InteractiveMarkerControl.MOVE_ROTATE_3D, \
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: numpy.save(f, all_results)
@info: commands robot to move to neutral pose """ import rospy import copy import IPython import numpy from franka_interface import ArmInterface if __name__ == '__main__': rospy.init_node("path_testing") arm = ArmInterface() frames = arm.get_frames_interface() start_pose = arm.endpoint_pose() pose0 = copy.deepcopy(start_pose) pose1 = copy.deepcopy(pose0) pose1['position'][0] += 0.1 pose2 = copy.deepcopy(pose1) pose2['position'][1] += 0.1 pose3 = copy.deepcopy(pose2) pose3['position'][0] -= 0.1 pose4 = copy.deepcopy(pose3) pose4['position'][1] -= 0.1 path = [pose1, pose2, pose3, pose4] IPython.embed()