def main(): """ """ # Querying the parameter server to determine Robot model and limb name(s) rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") robot_name = intera_interface.RobotParams().get_robot_name().lower( ).capitalize() # Parsing Input Arguments arg_fmt = argparse.ArgumentDefaultsHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt) parser.add_argument("-l", "--limb", dest="limb", default=valid_limbs[0], choices=valid_limbs, help='limb on which to attach joint springs') args = parser.parse_args(rospy.myargv()[1:]) # Grabbing Robot-specific parameters for Dynamic Reconfigure config_name = ''.join([robot_name, "JointSpringsExampleConfig"]) config_module = "intera_examples.cfg" cfg = importlib.import_module('.'.join([config_module, config_name])) # Starting node connection to ROS print("Initializing node... ") rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb)) dynamic_cfg_srv = Server(cfg, lambda config, level: config) js = SMC_Class(dynamic_cfg_srv, limb=args.limb) # register shutdown callback rospy.on_shutdown(js.clean_shutdown) js.move_to_neutral(0.05) rospy.sleep(3.0) js.Control_loop_Cart()
def main(): # Querying the parameter server to determine Robot model and limb name(s) rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") robot_name = intera_interface.RobotParams().get_robot_name().lower( ).capitalize() print "Robot name:", robot_name, " Valid Limbs:", valid_limbs # Grabbing Robot-specific parameters for Dynamic Reconfigure config_name = ''.join([robot_name, "JointSpringsExampleConfig"]) config_module = "intera_examples.cfg" moduleStr = '.'.join([config_module, config_name]) print "Module to import:", moduleStr cfg = importlib.import_module('.'.join([config_module, config_name])) # Starting node connection to ROS print("Initializing node... ") rospy.init_node("rsdk_ik_service_client") dynamic_cfg_srv = Server(cfg, lambda config, level: config) IK = SurfaceFollow(dynamic_cfg_srv, limb=valid_limbs[0])
def main(): """RSDK Joint Torque Example: Joint Springs Moves the default limb to a neutral location and enters torque control mode, attaching virtual springs (Hooke's Law) to each joint maintaining the start position. Run this example and interact by grabbing, pushing, and rotating each joint to feel the torques applied that represent the virtual springs attached. You can adjust the spring constant and damping coefficient for each joint using dynamic_reconfigure. """ # Querying the parameter server to determine Robot model and limb name(s) rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize() # Parsing Input Arguments arg_fmt = argparse.ArgumentDefaultsHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt) parser.add_argument( "-l", "--limb", dest="limb", default=valid_limbs[0], choices=valid_limbs, help='limb on which to attach joint springs' ) args = parser.parse_args(rospy.myargv()[1:]) # Grabbing Robot-specific parameters for Dynamic Reconfigure config_name = ''.join([robot_name,"JointSpringsExampleConfig"]) config_module = "intera_examples.cfg" cfg = importlib.import_module('.'.join([config_module,config_name])) # Starting node connection to ROS print("Initializing node... ") rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb)) dynamic_cfg_srv = Server(cfg, lambda config, level: config) js = JointSprings(dynamic_cfg_srv, limb=args.limb) # register shutdown callback rospy.on_shutdown(js.clean_shutdown) #js.move_to_neutral() # Move slowly to an user defined pose #joint_pos = np.array([0.0014580078125, -0.5757373046875, 0.0044482421875, 2.041927734375, -0.06414453125, 0.10119921875, 3.3797802734375,],dtype=np.float) ###joint_pos = np.array([ 0.01821875, -0.6355078125, -0.0242001953125, 1.66391015625, 0.036236328125, 0.526962890625, 3.286228515625],dtype=np.float) #####joint_pos = np.array([0.013896484375, -0.585494140625, -0.01746875, 1.64569140625, 0.0315244140625, 0.4926845703125, 3.2914306640625],dtype=np.float) joint_pos = np.array([0.0220263671875, -0.4779541015625, -0.0312109375, 1.3845087890625, 0.0467080078125, 0.6515537109375, 3.284783203125],dtype=np.float) js.move_to_position(joint_pos) rospy.sleep(1.0) js.Control_loop_Cart()
def main(): rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() rospy.init_node('doctor', log_level=rospy.DEBUG) moveit_commander.roscpp_initialize(sys.argv) rs = intera_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled rs.enable() rospy.logdebug("Closing gripper...") #raw_input("Press any key to close gripper") right_gripper = intera_interface.gripper.Gripper('right') right_gripper.calibrate() #rospy.sleep(2.0) #raw_input("Press any key to close gripper") right_gripper.close() #rospy.sleep(2.0) rospy.logdebug("Gripper closed") doctor = doctor_sawyer(valid_limbs[0]) doctor.find_table() while (not rospy.is_shutdown()): action = raw_input("(P)oke, (H)eartbeat, (T)emperature?") if (action == "P"): print(doctor.probe(2, 2, 0.05, 0.05)) rospy.spin()
def execute(self): """ Initializes the connection to the robot. """ robot_params = intera_interface.RobotParams() valid_limbs = robot_params.get_limb_names() if not valid_limbs: logger.error("Cannot detect any limb parameters on this robot!") return rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION) init_state = rs.state().enabled def _shutdown_hook(): logger.info("Exiting the application ...") if not init_state: logger.info("Disabling robot ...") rs.disable() rospy.on_shutdown(_shutdown_hook) logger.info("Enabling robot ...") rs.enable() if self.limb_name not in valid_limbs: logger.error("{} is not a valid limb on this robot!".format( self.limb_name)) # Initialize the limb and send it to the resting position. self.limb = intera_interface.Limb(self.limb_name) self.limb.set_joint_position_speed(0.35) self.spin()
def check(): rp = intera_interface.RobotParams() valid_cameras = rp.get_camera_names() camera = "right_hand_camera" if not valid_cameras: rp.log_message(("Cannot detect any camera_config" " parameters on this robot. Exiting."), "ERROR") return cameras = intera_interface.Cameras() if not cameras.verify_camera_exists(camera): rospy.logerr( "Could not detect the specified camera, exiting the example.") return rospy.loginfo("Opening camera '{0}'...".format(camera)) cameras.start_streaming(camera) rectify_image = not None use_canny_edge = None cameras.set_callback(camera, show_image_callback, rectify_image=rectify_image, callback_args=(True, camera)) rospy.loginfo("Camera_display node running. Ctrl-c to quit") rospy.sleep(3.0) cv2.destroyAllWindows() return decision
def main(): """RSDK Gripper Example: send a command to control the grippers. Run this example to command various gripper movements while adjusting gripper parameters, including calibration, and velocity: Uses the intera_interface.Gripper class and the helper function, intera_external_devices.getch. """ epilog = """ See help inside the example with the '?' key for key bindings. """ rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") return limb = valid_limbs[0] print("Using limb: {}.".format(limb)) arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__, epilog=epilog) parser.add_argument( "-a", "--action", dest="action", default="open", choices=["open", "close"], help="Action to perform with the gripper. Options: close or open" ) args = parser.parse_args(rospy.myargv()[1:]) print("Initializing node... ") rospy.init_node("sdk_gripper_keyboard") move_gripper(limb, args.action)
def main(): epilog = """ See help inside the example with the '?' key for key bindings. """ rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") return print("Initializing node... ") rospy.init_node("sdk_joint_position_keyboard") print("Getting robot state... ") rs = intera_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled def clean_shutdown(): print("\nExiting example.") rospy.on_shutdown(clean_shutdown) rospy.loginfo("Enabling robot...") rs.enable() move_joints(valid_limbs[0]) print("Done.")
def enable_robot(self): rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") return # arg_fmt = argparse.RawDescriptionHelpFormatter # parser = argparse.ArgumentParser(formatter_class=arg_fmt, # description=main.__doc__) # parser.add_argument( # '-l', '--limb', choices=valid_limbs, default=valid_limbs[0], # help='send joint trajectory to which limb' # ) # # args = parser.parse_args(rospy.myargv()[1:]) # limb = args.limb limb = "right" rospy.init_node("drawing".format(limb)) #print("Getting robot state... ") rs = intera_interface.RobotEnable(CHECK_VERSION) #print("Enabling robot... ") #rs.enable() limb_interface = intera_interface.Limb(limb) return limb, limb_interface
def main(): camera_name = "right_hand_camera" use_canny_edge = 1 rp = intera_interface.RobotParams() valid_cameras = rp.get_camera_names() if not valid_cameras: rp.log_message(("Cannot detect any camera_config" " parameters on this robot. Exiting."), "ERROR") return print("Initializing node... ") rospy.init_node('camera_display', anonymous=True) cameras = intera_interface.Cameras() if not cameras.verify_camera_exists(camera_name): rospy.logerr( "Could not detect the specified camera, exiting the example.") return rospy.loginfo("Opening camera '{0}'...".format(camera_name)) cameras.start_streaming(camera_name) cameras.set_cognex_strobe(False) cameras.set_callback(camera_name, show_image_callback, rectify_image=False, callback_args=None) def clean_shutdown(): print("Shutting down camera_display node.") cv2.destroyAllWindows() rospy.on_shutdown(clean_shutdown) rospy.loginfo("Camera_display node running. Ctrl-c to quit") rospy.spin()
def main(): rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") return arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) parser.add_argument( "-l", "--limb", dest="limb", default=valid_limbs[0], choices=valid_limbs, help="Limb on which to run the joint position keyboard example") args = parser.parse_args(rospy.myargv()[1:]) print("Initializing node... ") rospy.init_node("move_to_position") print("Getting robot state... ") rs = intera_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled def clean_shutdown(): print("\nExiting example.") rospy.on_shutdown(clean_shutdown) rospy.loginfo("Enabling robot...") rs.enable() move_to_position(args.limb) print("Done.")
def start_server(limb, rate, mode, valid_limbs): rospy.loginfo("Initializing node... ") rospy.init_node("sdk_{0}_joint_trajectory_action_server{1}".format( mode, "" if limb == 'all_limbs' else "_" + limb,)) rospy.loginfo("Initializing joint trajectory action server...") robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize() config_module = "intera_interface.cfg" if mode == 'velocity': config_name = ''.join([robot_name,"VelocityJointTrajectoryActionServerConfig"]) elif mode == 'position': config_name = ''.join([robot_name,"PositionJointTrajectoryActionServerConfig"]) else: config_name = ''.join([robot_name,"PositionFFJointTrajectoryActionServerConfig"]) cfg = importlib.import_module('.'.join([config_module,config_name])) dyn_cfg_srv = Server(cfg, lambda config, level: config) jtas = [] if limb == 'all_limbs': for current_limb in valid_limbs: jtas.append(JointTrajectoryActionServer(current_limb, dyn_cfg_srv, rate, mode)) else: jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode)) def cleanup(): for j in jtas: j.clean_shutdown() rospy.on_shutdown(cleanup) rospy.loginfo("Joint Trajectory Action Server Running. Ctrl-c to quit") rospy.spin()
def main(): rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") return # Add an option for starting a server for all valid limbs all_limbs = valid_limbs all_limbs.append("all_limbs") arg_fmt = argparse.ArgumentDefaultsHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt) parser.add_argument("-l", "--limb", dest="limb", default=valid_limbs[0], choices=all_limbs, help="joint trajectory action server limb") parser.add_argument("-r", "--rate", dest="rate", default=100.0, type=float, help="trajectory control rate (Hz)") parser.add_argument("-m", "--mode", default='position_w_id', choices=['position_w_id', 'position', 'velocity'], help="control mode for trajectory execution") args = parser.parse_args(rospy.myargv()[1:]) start_server(args.limb, args.rate, args.mode, valid_limbs)
def init_gripper(): rp = intera_interface.RobotParams() # For logging rp.log_message('Launch topics for gripper') rp.log_message('Please run the following command in a new terminal:') rp.log_message('roslaunch wsg_50_driver wsg_50_tcp_script.launch') time.sleep(1) return WSG50_manu.WSG50()
def main(): """Modified Joint Position Example Use your keyboard to control joint positions. Each key corresponds to increasing or decreasing the angle of a joint on Sawyer's arm. n gives the option to change the angle increment value. See help inside the example with the '?' key for key bindings. """ rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") return print("Initializing node... ") rospy.init_node("move_individual_joint_position") print("Getting robot state... ") rs = intera_interface.RobotEnable(CHECK_VERSION) def clean_shutdown(): print("\nExiting example.") rospy.on_shutdown(clean_shutdown) rospy.loginfo("Enabling robot...") rs.enable() map_keyboard(valid_limbs[0]) # i.e right print("Done.")
def main(): """RSDK Joint Torque Example: Joint Springs """ # Querying the parameter server to determine Robot model and limb name(s) rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") robot_name = intera_interface.RobotParams().get_robot_name().lower( ).capitalize() # Parsing Input Arguments arg_fmt = argparse.ArgumentDefaultsHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt) parser.add_argument("-l", "--limb", dest="limb", default=valid_limbs[0], choices=valid_limbs, help='limb on which to attach joint springs') args = parser.parse_args(rospy.myargv()[1:]) # Grabbing Robot-specific parameters for Dynamic Reconfigure config_name = ''.join([robot_name, "JointSpringsExampleConfig"]) config_module = "intera_examples.cfg" cfg = importlib.import_module('.'.join([config_module, config_name])) # Starting node connection to ROS print("Initializing node... ") rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb)) dynamic_cfg_srv = Server(cfg, lambda config, level: config) js = SMC_Class(dynamic_cfg_srv, limb=args.limb) # register shutdown callback rospy.on_shutdown(js.clean_shutdown) #js.move_to_neutral() # Move slowly to an user defined pose ##joint_pos = np.array([-0.00615625, -0.6968154296875, 0.0101787109375, 2.0974033203125, -0.0526416015625, 0.1697822265625, 3.3739580078125],dtype=np.float) joint_pos = np.array([ 0.1456845703125, -0.349845703125, -0.05283984375, 1.1399111328125, 0.068462890625, 0.7788173828125, 3.397236328125 ], dtype=np.float) #joint_pos = np.array([0.113171875, -0.478880859375, -0.0372294921875, 1.4443251953125, 0.052453125, 0.6051650390625, 3.3741640625],dtype=np.float) #joint_pos = np.array([0.2358125, -0.4979892578125, -0.046830078125, 1.4992412109375, 0.071341796875, 0.5706796875, 3.479982421875],dtype=np.float) js.move_to_position(joint_pos) rospy.sleep(5.0) js.Control_loop_Cart()
def main(): limb_name = "right" limb = init_robot(limb_name=limb_name) gripper = init_gripper() # init_cuff(limb_name=limb_name) rp = intera_interface.RobotParams() # For logging time.sleep(1) limb.set_joint_position_speed(0.12) # Let' s move slowly... # Move arm to set position des_EE_xyz = np.array([0.55, 0, 0.3]) des_orientation_EE = Orientations.DOWNWARD_ROTATED rp.log_message('Moving to x=%f y=%f' % (des_EE_xyz[0], des_EE_xyz[1])) goto_EE_xyz(limb=limb, xyz=des_EE_xyz, orientation=des_orientation_EE) #NOTE: MUST import: import getch print( "Place object beetween the fingers and press Esc to close the gripper." ) done = False while not done and not rospy.is_shutdown(): c = grip_and_record.getch.getch() if c: if c in ['\x1b', '\x03']: done = True gripper.close() # TODO: make sure that the closure is properly done! time.sleep(1) # give time to move # Raise the object goto_EE_xyz(limb=limb, xyz=des_EE_xyz + np.array([0, 0, 0.2]), orientation=des_orientation_EE) # Return object to the ground (a little bit higher) print( "If the object is no longer grasped, clean the table and press Esc. Otherwise, Press Esc to return the object to the table." ) done = False while not done and not rospy.is_shutdown(): c = grip_and_record.getch.getch() if c: if c in ['\x1b', '\x03']: done = True # Return object to the ground (a little bit higher) # TODO: return the object to a new random position ? goto_EE_xyz(limb=limb, xyz=des_EE_xyz + np.array([0, 0, 0.02]), orientation=Orientations.DOWNWARD_ROTATED) time.sleep(0.5) gripper.open() # Open gripper # data_recorder.end_processes() goto_rest_pos(limb=limb, blocking=True) # Go to a safe place before shutting down rospy.signal_shutdown("Example finished.")
def main(): """SDK Gripper Example: Joystick Control Use a game controller to control the grippers. Attach a game controller to your dev machine and run this example along with the ROS joy_node to control gripper using the joysticks and buttons. Be sure to provide the *joystick* type you are using as an argument to setup appropriate key mappings. Uses the intera_interface.Gripper class and the helper classes in intera_external_devices.Joystick. """ epilog = """ See help inside the example with the "Start" button for controller key bindings. """ rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") return arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__, epilog=epilog) required = parser.add_argument_group('required arguments') required.add_argument('-j', '--joystick', required=True, choices=['xbox', 'logitech', 'ps3'], help='specify the type of joystick to use') parser.add_argument( "-l", "--limb", dest="limb", default=valid_limbs[0], choices=valid_limbs, help="Limb on which to run the gripper joystick example") args = parser.parse_args(rospy.myargv()[1:]) joystick = None if args.joystick == 'xbox': joystick = intera_external_devices.joystick.XboxController() elif args.joystick == 'logitech': joystick = intera_external_devices.joystick.LogitechController() elif args.joystick == 'ps3': joystick = intera_external_devices.joystick.PS3Controller() else: # Should never reach this case with proper argparse usage parser.error("Unsupported joystick type '%s'" % (args.joystick)) print("Initializing node... ") rospy.init_node("sdk_gripper_joystick") map_joystick(joystick, args.limb)
def main(): """RSDK Joint Torque Example: Joint Springs Moves the default limb to a neutral location and enters torque control mode, attaching virtual springs (Hooke's Law) to each joint maintaining the start position. Run this example and interact by grabbing, pushing, and rotating each joint to feel the torques applied that represent the virtual springs attached. You can adjust the spring constant and damping coefficient for each joint using dynamic_reconfigure. """ # Querying the parameter server to determine Robot model and limb name(s) rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize() # Parsing Input Arguments arg_fmt = argparse.ArgumentDefaultsHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt) parser.add_argument( "-l", "--limb", dest="limb", default=valid_limbs[0], choices=valid_limbs, help='limb on which to attach joint springs' ) args = parser.parse_args(rospy.myargv()[1:]) # Grabbing Robot-specific parameters for Dynamic Reconfigure config_name = ''.join([robot_name, "JointSpringsExampleConfig"]) config_module = "intera_examples.cfg" cfg = importlib.import_module('.'.join([config_module,config_name])) # Starting node connection to ROS print("Initializing node... ") rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb)) dynamic_cfg_srv = Server(cfg, lambda config, level: config) dc = DataCollector(dynamic_cfg_srv, limb=args.limb) # <- MOVE! # register shutdown callback rospy.on_shutdown(dc.clean_shutdown) for i in range(30): print("Starting a new trajectory.") dc.collect_data(n_iters=10) dc.reset_to_initial_pos()
def wait_for_key(): rp = intera_interface.RobotParams() # For logging rp.log_message("Press ESC to continue...") done = False while not done and not rospy.is_shutdown(): c = grip_and_record.getch.getch() if c: if c in ['\x1b', '\x03']: done = True
def cb_camera(self, data): if self.VERBOSE: rospy.loginfo('Accessing camera') rp = intera_interface.RobotParams() valid_cameras = rp.get_camera_names() camera = intera_interface.Cameras() camera.start_streaming('right_hand_camera') rectify_image = False camera.set_callback('right_hand_camera', self.display_camera_callback)
def start(self): rp = intera_interface.RobotParams() valid_cameras = rp.get_camera_names() camera = intera_interface.Cameras() if not camera.verify_camera_exists('right_hand_camera'): rospy.logerr('Invalid camera name') camera.start_streaming('right_hand_camera') rectify_image = False camera.set_callback('right_hand_camera', self.display_camera_callback)
def __init__(self, nameObject=''): self.first = True self.rp = intera_interface.RobotParams() # For logging self.rp.log_message('') print('Make sure the correct object is printed below.') print('Object: %s' % nameObject) self.nameObject = nameObject # Make required initiations self.limb_name = "right" self.limb = None self.init_robot() self.gripper = self.init_gripper() # Requesting to start topics for KinectA self.rp.log_message('Launch topics for KinectA') self.rp.log_message( 'Please run the following command in a new terminal (in intera mode):' ) self.rp.log_message('rosrun kinect2_bridge kinect2_bridge') self.rp.log_message('') # Requesting to start topics for KinectB self.rp.log_message('Launch topics for KinectB') self.rp.log_message( 'Please run the following command in a new terminal (in intera mode) on the kinectbox02:' ) # rp.log_message('ssh k2') # rp.log_message('for pid in $(ps -ef | grep "kinect2_bridge" | awk "{print $2}"); do kill -9 $pid; done') self.rp.log_message( '/home/rail/ros_ws/src/manu_kinect/start_KinectB.sh') self.rp.log_message('') # Start Topic for the Gelsight self.rp.log_message('Launch topic for GelsightA') self.rp.log_message( 'Please run the following command in a new terminal (in intera mode):' ) self.rp.log_message('roslaunch manu_sawyer gelsightA_driver.launch') self.rp.log_message('') self.rp.log_message('Launch topic for GelsightB') self.rp.log_message( 'Please run the following command in a new terminal (in intera mode):' ) self.rp.log_message('roslaunch manu_sawyer gelsightB_driver.launch') self.gelSightA = GelSightA() self.gelSightB = GelSightB() self.kinectA = KinectA(save_init=COMPUTE_BG) self.kinectB = KinectB() time.sleep(1) # Requests the user to place the object to be griped on the table. self.rp.log_message('Place the object to grasp on the table.') wait_for_key() self.start_experiment() # Start grasping the object
def main(): """SDK Joint Trajectory Example: File Playback Plays back joint positions honoring timestamps recorded via the joint_recorder example. Run the joint_recorder.py example first to create a recording file for use with this example. Then make sure to start the joint_trajectory_action_server before running this example. This example will use the joint trajectory action server with velocity control to follow the positions and times of the recorded motion, accurately replicating movement speed necessary to hit each trajectory point on time. """ epilog = """ Related examples: joint_recorder.py; joint_position_file_playback.py. """ rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") return FILE = 'traj_final.txt' print("Initializing node... ") rospy.init_node("sdk_joint_trajectory_file_playback") print("Getting robot state... ") rs = intera_interface.RobotEnable(CHECK_VERSION) print("Enabling robot... ") rs.enable() print("Running. Ctrl-c to quit") traj = Trajectory(valid_limbs[0]) traj.parse_file(path.expanduser(FILE)) #for safe interrupt handling rospy.on_shutdown(traj.stop) result = True loop_cnt = 1 loopstr = str(1) numloops = 1 if numloops == 0: numloops = float('inf') loopstr = "forever" while (result == True and loop_cnt <= numloops and not rospy.is_shutdown()): print("Playback loop %d of %s" % ( loop_cnt, loopstr, )) traj.start() result = traj.wait() loop_cnt = loop_cnt + 1 print("Exiting - File Playback Complete")
def main(): # IK = IK_sample(dynamic_cfg_srv, limb=args.limb) # IK = IK_sample() # Querying the parameter server to determine Robot model and limb name(s) rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize() # Parsing Input Arguments arg_fmt = argparse.ArgumentDefaultsHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt) parser.add_argument( "-l", "--limb", dest="limb", default=valid_limbs[0], choices=valid_limbs, help='limb on which to attach joint springs' ) args = parser.parse_args(rospy.myargv()[1:]) # Grabbing Robot-specific parameters for Dynamic Reconfigure config_name = ''.join([robot_name,"JointSpringsExampleConfig"]) config_module = "intera_examples.cfg" cfg = importlib.import_module('.'.join([config_module,config_name])) # Starting node connection to ROS print("Initializing node... ") rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb)) dynamic_cfg_srv = Server(cfg, lambda config, level: config) IK = IK_sample(dynamic_cfg_srv, limb=args.limb) # register shutdown callback rospy.on_shutdown(IK.clean_shutdown) # rospy.init_node("rsdk_ik_service_client") if IK.ik_service_client(): rospy.loginfo("Simple IK call passed!") print "Moving to desired location" joint_pos = np.array([sub_right_j0, sub_right_j1, sub_right_j2, sub_right_j3, sub_right_j4, sub_right_j5, sub_right_j6],dtype=np.float) # joint_pos = np.array([resp.joints[0].position[0], sub_right_j1, sub_right_j2, sub_right_j3, sub_right_j4, sub_right_j5, sub_right_j6],dtype=np.float) print(joint_pos) IK.move_to_position(joint_pos) else: rospy.logerr("Simple IK call FAILED")
def set_pose(pose): rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() limb = intera_interface.Limb(valid_limbs[0]) success = False while not success and not rospy.is_shutdown(): limb.set_joint_positions(pose) joint_angles = limb.joint_angles() joint_progess = [] for key in pose.keys(): joint_progess.append(np.isclose(joint_angles[key], pose[key], atol=1e-2)) success = np.all(joint_progess)
def main(): """Camera Display Example """ rp = intera_interface.RobotParams() valid_cameras = rp.get_camera_names() if not valid_cameras: rp.log_message(("Cannot detect any camera_config" " parameters on this robot. Exiting."), "ERROR") return arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) parser.add_argument('-c', '--camera', type=str, default="head_camera", choices=valid_cameras, help='Setup Camera Name for Camera Display') parser.add_argument( '-r', '--raw', action='store_true', help='Specify use of the raw image (unrectified) topic') parser.add_argument('-e', '--edge', action='store_true', help='Streaming the Canny edge detection image') args = parser.parse_args() print("Initializing node... ") # rospy.init_node('camera_display', anonymous=True) camera = intera_interface.Cameras() if not camera.verify_camera_exists(args.camera): rospy.logerr("Invalid camera name, exiting the example.") return camera.start_streaming(args.camera) rectify_image = not args.raw use_canny_edge = args.edge camera.set_callback(args.camera, show_image_callback, rectify_image=rectify_image, callback_args=(use_canny_edge, args.camera)) global pub pub = rospy.Publisher('sawyer_cam', Image, queue_size=10) def clean_shutdown(): print("Shutting down camera_display node.") cv2.destroyAllWindows() rospy.on_shutdown(clean_shutdown) rospy.loginfo("Camera_display node running. Ctrl-c to quit") rospy.spin()
def goto_EE_xyz(limb, xyz, orientation=Orientations.DOWNWARD_ROTATED, verbosity=1, rest_pos=False): """ Move the End-effector to the desired XYZ position and orientation, using inverse kinematic :param limb: link to the limb being used :param xyz: list or array [x,y,z] with the coordinates in XYZ positions in limb reference frame :param orientation: :param verbosity: verbosity level. >0 print stuff :return: """ try: if verbosity > 0: rp = intera_interface.RobotParams() # For logging rp.log_message('Moving to x=%f y=%f z=%f' % (xyz[0], xyz[1], xyz[2])) if not rest_pos: # Make sure that the XYZ position is valid, and doesn't collide with the cage assert (xyz[0] >= bounds_table[0, 0]) and ( xyz[0] <= bounds_table[0, 1]), 'X is outside of the bounds' assert (xyz[1] >= bounds_table[1, 0]) and ( xyz[1] <= bounds_table[1, 1]), 'Y is outside of the bounds' assert (xyz[2] >= lower_bound_z), 'Z is outside of the bounds' des_pose = grip_and_record.inverse_kin.get_pose( xyz[0], xyz[1], xyz[2], orientation) curr_pos = limb.joint_angles() # Measure current position joint_positions = grip_and_record.inverse_kin.get_joint_angles( des_pose, limb.name, curr_pos, use_advanced_options=True) # gets joint positions limb.move_to_joint_positions( joint_positions, timeout=2, threshold=0.01) # Send the command to the arm # print("done moving") except UnboundLocalError: pose_dict = limb.endpoint_pose() pose_pos = pose_dict['position'] current_xyz = [pose_pos.x, pose_pos.y, pose_pos.z] halfway_xyz = ((np.array(xyz) + np.array(current_xyz)) / 2.0).tolist() if np.linalg.norm(np.array(current_xyz) - np.array(halfway_xyz)) > 0.00001: time.sleep(0.2) if rest_pos: goto_EE_xyz(limb, halfway_xyz, orientation, rest_pos=True) goto_EE_xyz(limb, xyz, orientation, rest_pos=True) else: goto_EE_xyz(limb, halfway_xyz, orientation) goto_EE_xyz(limb, xyz, orientation) else: print("WoooOooOW") goto_EE_xyz(limb, [0.60, 0.0, 0.40], orientation, rest_pos=True)
def main(): rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") return print("Initializing node... ") rospy.init_node("gripper") s = rospy.Service('grip_pls', Grip, letsgrip) print("Ready to call Grip") rospy.spin()
def main(): # Querying the parameter server to determine Robot model and limb name(s) rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") robot_name = intera_interface.RobotParams().get_robot_name().lower( ).capitalize() # Parsing Input Arguments arg_fmt = argparse.ArgumentDefaultsHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt) parser.add_argument("-l", "--limb", dest="limb", default=valid_limbs[0], choices=valid_limbs, help='limb on which to attach joint springs') args = parser.parse_args(rospy.myargv()[1:]) # Grabbing Robot-specific parameters for Dynamic Reconfigure config_name = ''.join([robot_name, "JointSpringsExampleConfig"]) config_module = "intera_examples.cfg" cfg = importlib.import_module('.'.join([config_module, config_name])) # Starting node connection to ROS print("Initializing node... ") rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb)) dynamic_cfg_srv = Server(cfg, lambda config, level: config) js = SMC_Class(dynamic_cfg_srv, limb=args.limb) # register shutdown callback rospy.on_shutdown(js.clean_shutdown) #js.move_to_neutral() # Move slowly to an user defined pose #joint_pos = np.array([-0.006259765625, -1.1291142578125, 0.0050166015625, 1.9478134765625, 0.0013515625, 0.7445380859375, 3.3209453125],dtype=np.float) # joint_pos = np.array([-0.00615625, -0.6968154296875, 0.0101787109375, 2.0974033203125, -0.0526416015625, 0.1697822265625, 3.3739580078125],dtype=np.float) # joint_pos = np.array([-0.05369245610836959, -0.5542895011336757, 0.03519333775486455, 1.9521720248440104, -0.07304541055318392, -1.4009694692718515, 3.1400003227121065],dtype=np.float) # js.move_to_position(joint_pos) rospy.sleep(1.0) js.Control_loop_Cart()