def __init__(self, limb, reconfig_server, rate=100.0, mode='position_w_id', interpolation='minjerk'): self._dyn = reconfig_server self._ns = 'robot/limb/' + limb self._fjt_ns = self._ns + '/follow_joint_trajectory' self._server = actionlib.SimpleActionServer( self._fjt_ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._action_name = rospy.get_name() self._limb = intera_interface.Limb(limb, synchronous_pub=True) self._enable = intera_interface.RobotEnable() self._name = limb self._interpolation = interpolation self._cuff = intera_interface.Cuff(limb=limb) # Verify joint control mode self._mode = mode if (self._mode != 'position' and self._mode != 'position_w_id' and self._mode != 'velocity'): rospy.logerr("%s: Action Server Creation Failed - " "Provided Invalid Joint Control Mode '%s' (Options: " "'position_w_id', 'position', 'velocity')" % ( self._action_name, self._mode, )) return self._server.start() self._alive = True # Action Feedback/Result self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() # Controller parameters from arguments, messages, and dynamic # reconfigure self._control_rate = rate # Hz self._control_joints = [] self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()} self._goal_time = 0.0 self._stopped_velocity = 0.0 self._goal_error = dict() self._path_thresh = dict() # Create our PID controllers self._pid = dict() for joint in self._limb.joint_names(): self._pid[joint] = intera_control.PID() # Create our spline coefficients self._coeff = [None] * len(self._limb.joint_names()) # Set joint state publishing to specified control rate self._pub_rate = rospy.Publisher('/robot/joint_state_publish_rate', UInt16, queue_size=10) self._pub_rate.publish(self._control_rate)
def _init_nodes(self, limb, light): try: self._limb=intera_interface.Limb(limb) self._head=intera_interface.Head() self._light=SawyerLight(light) #self._head_display=intera_interface.HeadDisplay() self._display=SawyerDisplay() self._cuff=intera_interface.Cuff() self._limits=intera_interface.JointLimits() self._navigator=intera_interface.Navigator() self._joint_names=self._limb.joint_names() self._velocity_limits=self._limits.joint_velocity_limits() self._stop_cmd={} for i,name in enumerate(self._joint_names): self._stop_cmd[name]=0.0 except: print("Warning caught exception...") traceback.print_exc() pass
def __init__(self, filename, rate, side="right"): """ Records joint data to a file at a specified rate. """ self.gripper_name = '_'.join([side, 'gripper']) self._filename = filename self._raw_rate = rate self._rate = rospy.Rate(rate) self._start_time = rospy.get_time() self._done = False self._limb_right = intera_interface.Limb(side) try: self._gripper = intera_interface.Gripper(self.gripper_name) rospy.loginfo("Electric gripper detected.") except Exception as e: self._gripper = None rospy.loginfo("No electric gripper detected.") # Verify Gripper Have No Errors and are Calibrated if self._gripper: if self._gripper.has_error(): self._gripper.reboot() if not self._gripper.is_calibrated(): self._gripper.calibrate() self._cuff = intera_interface.Cuff(side)
def __init__(self, speed=0.28, accuracy=0.003726646, timeout=5): self.sawyer_arm = intera_interface.Limb('right') self.joint_speed = speed self.accuracy = accuracy self.timeout = timeout self.waypoints = list() self._is_recording_waypoints = False self.rs = intera_interface.RobotEnable() self._init_state = self.rs.state().enabled self.rs.enable() # Create Navigator self.navigator_io = intera_interface.Navigator() self.sawyer_lights = intera_interface.Lights() # Create Gripper & Cuff self.sawyer_gripper = intera_interface.Gripper() self.sawyer_cuff = intera_interface.Cuff(limb='right') self.sawyer_cuff.register_callback(self.cuff_open_gripper_callback, '{0}_button_upper'.format('right')) self.sawyer_cuff.register_callback(self.cuff_close_gripper_callback, '{0}_button_lower'.format('right')) # print(self.navigator_io.list_all_items()) self.sawyer_arm.move_to_neutral(timeout=5, speed=self.joint_speed) self.sawyer_gripper.open() self.set_light_status('red', False) self.set_light_status('green', False) self.set_light_status('blue', True)
def __init__(self): self.waypoints = Waypoints() self.rs = intera_interface.RobotEnable() self._init_state = self.rs.state().enabled self.rs.enable() # Set up arm, cuff and gripper self.sawyer_arm = intera_interface.Limb('right') self.sawyer_gripper = intera_interface.Gripper() self.sawyer_cuff = intera_interface.Cuff(limb='right') self.sawyer_lights = intera_interface.Lights() # Move to default position when the ik solver is initially launched self.sawyer_arm.move_to_neutral(timeout=10, speed=0.28) self.sawyer_gripper.open() self.gripper_dist = self.sawyer_gripper.get_position() self.sawyer_cuff.register_callback(self.cuff_open_gripper_callback, '{0}_button_upper'.format('right')) self.sawyer_cuff.register_callback(self.cuff_close_gripper_callback, '{0}_button_lower'.format('right')) self.set_light_status('red', False) self.set_light_status('green', True) self.set_light_status('blue', False)
def __init__(self): self.is_adding_new_item = False # This flag will be set to true if the user is in close proximity to the robot, flex_ik_service_client should immediatly exit self.has_returned_home = False # False implies "The user wants to create an object, but im not in the default position, so I'll move there now" node = rospy.init_node("sawyer_ik_solver_node") rate = rospy.Rate(10) # Publishing rate in Hz self.arm_speed = 0.3 self.arm_timeout = 5 self.hover_dist = 0.06 # Subscribe to topic where sortable messages arrive ik_sub_sorting = rospy.Subscriber('ui/sortable_object/sorting/execute', SortableObjectMsg, callback=self.sort_object_callback, queue_size=20) # Publish to this topic once object has been sorted, or if we got an error (data.successful_sort == False) self.ik_pub_sorted = rospy.Publisher('sawyer_ik_sorting/sortable_objects/object/sorted', SortableObjectMsg, queue_size=10) self.ik_sub_abort_sorting = rospy.Subscriber('ui/user/has_aborted', Bool, callback=None, queue_size=10) # This will suspend all sorting self.ik_sub_add_item = rospy.Subscriber('ui/user/is_moving_arm', Bool, callback=self.disable_sorting_capability_callback, queue_size=10) self.ik_pub_current_arm_pose = rospy.Publisher('sawyer_ik_sorting/sawyer_arm/pose/current', PoseGrippMessage, queue_size=10) # Publish current arm pose self.ik_sub_locating_object_done = rospy.Subscriber('ui/new_object/state/is_located', Bool, callback=self.send_current_pose_callback, queue_size=10) # Starts-up/enables the robot try: rs = intera_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled rs.enable() except Exception as ex: print(ex) error_name = "IK solver crashed!" error_msg = "The Inverse Kinematic solver has crashed. Moving the robot is no longer possible.\n\nPlese restart the program." self.ik_solver_error_msg(error_name, error_msg) # Display error if robot can't be enabled rospy.signal_shutdown("Failed to enable Robot") ik_sub_shutdown = rospy.Subscriber('sawyer_ik_solver/change_to_state/shudown', Bool, callback=self.shutdown_callback, queue_size=10) # Topic where main_gui tells solver to shutdown # Set up arm, cuff and gripper self.sawyer_arm = intera_interface.Limb('right') self.sawyer_gripper = intera_interface.Gripper() self.sawyer_cuff = intera_interface.Cuff(limb='right') self.sawyer_lights = intera_interface.Lights() # Move to default position when the ik solver is initially launched self.sawyer_arm.move_to_neutral(timeout=self.arm_timeout, speed=self.arm_speed) self.sawyer_gripper.open() self.sawyer_cuff.register_callback(self.cuff_open_gripper_callback, '{0}_button_upper'.format('right')) self.sawyer_cuff.register_callback(self.cuff_close_gripper_callback, '{0}_button_lower'.format('right')) self.set_light_status('red', False) self.set_light_status('green', False) self.set_light_status('blue', True) rospy.spin()
def __init__(self, item_id, reference_pose, base_frame="world", child_frame="right_gripper_tip", service_name="transform_lookup_service"): """ Parameters ---------- robot_id : int Id of robot assigned in the config.json configuration files. reference_pose : dict Dictionary with position and orientation fields base_frame : str The base / world frame from which to calculate the transformation to the child frame. child_frame : str The ending TF frame used to generate the pose / orientation of the robot (usually the end effector tip). service_name : str Name of transformation lookup service used by _get_transform() to calculate the transformation between world_frame and child_frame """ self.id = item_id self.reference_pose = reference_pose self._limb = intera_interface.Limb("right") self._cuff = intera_interface.Cuff("right") self._navigator = intera_interface.Navigator() try: self._gripper = intera_interface.Gripper("right") rospy.loginfo("Electric gripper detected.") if self._gripper.has_error(): rospy.loginfo("Gripper error...rebooting.") self._gripper.reboot() if not self._gripper.is_calibrated(): rospy.loginfo("Calibrating gripper.") self._gripper.calibrate() except Exception as e: self._gripper = None rospy.loginfo("No electric gripper detected.") self.base_frame = base_frame self.child_frame = child_frame self.tlc = TransformLookupClient(service_name)
import rospy from std_msgs.msg import Bool, Int32, Float64 from geometry_msgs.msg import Pose, Point, Quaternion from urdf_parser_py.urdf import URDF from pykdl_utils.kdl_kinematics import KDLKinematics import PyKDL import os os.chdir("/home/irobot/catkin_ws/src/ddpg/scripts") rospy.init_node('pose_acquisition') limb = intera_interface.Limb("right") head_display = intera_interface.HeadDisplay() head_display.display_image('/home/irobot/Downloads' + "/Cute.png") cuff = intera_interface.Cuff() tf_listenser = tf.TransformListener() endpt_ori = [0.0, 0.0, 0.0, 0.0] endpt_pos = [0.0, 0.0, 0.0] position_x = list() position_y = list() position_z = list() orient_x = list() orient_y = list() orient_z = list() orient_w = list() roll = list() pitch = list() yaw = list()
def __init__(self): # Initialize Sawyer rospy.init_node('Sawyer_comm_node', anonymous=True) intera_interface.HeadDisplay().display_image('logo.png') # Publishing topics suppress_cuff_interaction = rospy.Publisher( '/robot/limb/right/suppress_cuff_interaction', Empty, queue_size=1) self.position_topic = rospy.Publisher('/position', JointInfo, queue_size=1) self.velocity_topic = rospy.Publisher('/velocity', JointInfo, queue_size=1) self.effort_topic = rospy.Publisher('/effort', JointInfo, queue_size=1) self.dev_topic = rospy.Publisher('/dev_topic', Int32, queue_size=10) self.scroll_wheel_button_topic = rospy.Publisher( '/scroll_wheel_button_topic', Empty, queue_size=10) self.command_complete_topic = rospy.Publisher('/command_complete', Empty, queue_size=1) self.wheel_delta_topic = rospy.Publisher('/wheel_delta', Int32, queue_size=10) self.clicked = rospy.Publisher('scroll_wheel_pressed', Bool, queue_size=10) self.toggle_completed_topic = rospy.Publisher('/toggle_completed', Empty, queue_size=1) self.highTwo_success_topic = rospy.Publisher('/highTwo_success', Bool, queue_size=1) self.endpoint_topic = rospy.Publisher('/EndpointInfo', EndpointInfo, queue_size=10) self.interaction_control_topic = rospy.Publisher('/InteractionControl', InteractionControl, queue_size=10) self.pickedup_topic = rospy.Publisher('/pickedup', Bool, queue_size=1) self.pos_orient_topic = rospy.Publisher('/pos_orient', String, queue_size=1) # Subscribing topics rospy.Subscriber('/audio_duration', Float64, self.rx_audio_duration) rospy.Subscriber('/robot/joint_states', sensor_msgs.msg.JointState, self.forwardJointState) rospy.Subscriber('/GoToJointAngles', GoToJointAngles, self.cb_GoToJointAngles) rospy.Subscriber('/wheel_subscription', Bool, self.cb_WheelSubscription) rospy.Subscriber('/joint_move', joint_move, self.cb_joint_move) rospy.Subscriber('/InteractionControl', InteractionControl, self.cb_interaction) rospy.Subscriber('/JointAngle', String, self.cb_joint_angle) rospy.Subscriber('/JointImpedance', JointImpedance, self.cb_impedance) rospy.Subscriber('/multiple_choice', Bool, self.cb_multiple_choice) rospy.Subscriber('/highTwo', Bool, self.cb_highTwo) rospy.Subscriber('/robot/limb/right/endpoint_state', intera_core_msgs.msg.EndpointState, self.forwardEndpointState) rospy.Subscriber('/adjustPoseTo', adjustPoseTo, self.cb_adjustPoseTo) rospy.Subscriber('/closeGripper', Bool, self.cb_closeGripper) rospy.Subscriber('/openGripper', Bool, self.cb_openGripper) rospy.Subscriber('/GoToCartesianPose', GoToCartesianPose, self.cb_GoToCartesianPose) rospy.Subscriber('/cuffInteraction', cuffInteraction, self.cb_cuffInteraction) rospy.Subscriber('/check_pickup', Bool, self.cb_check_pickup) rospy.Subscriber('/adjustPoseBy', adjustPoseBy, self.cb_adjustPoseBy) rospy.Subscriber('/camera', Bool, self.cb_camera) rospy.Subscriber('/robot/digital_io/right_lower_button/state', intera_core_msgs.msg.DigitalIOState, self.cb_cuff_lower) rospy.Subscriber('/robot/digital_io/right_upper_button/state', intera_core_msgs.msg.DigitalIOState, self.cb_cuff_upper) # Global Vars self.audio_duration = 0 self.finished = False self.startPos = 0 self.devMode = False self.seqArr = [] self.p_command = lambda self: self.pub_num(180 / math.pi * self.limb. joint_angle(shoulder)) wheel = self.finished b_less = lambda self: self.limb.joint_angle(shoulder) < point_b[0] b_more = lambda self: self.limb.joint_angle(shoulder) > point_b[0] dot_2 = lambda self: self.limb.joint_angle(shoulder) < joint_dot_2[0] #endpoint = lambda self : self.endpoints_equal(self.limb.endpoint_pose(),dot_3,tol) or self.finished # Limb self.limb = LimbPlus() self.limb.go_to_joint_angles() # Lights self.lights = intera_interface.Lights() for light in self.lights.list_all_lights(): self.lights.set_light_state(light, False) # Navigator self.navigator = intera_interface.Navigator() self.multi_choice_callback_ids = self.BUTTON.copy() for key in self.multi_choice_callback_ids: self.multi_choice_callback_ids[key] = -1 self.wheel_callback_id = -1 self.wheel_state = self.navigator.get_wheel_state('right_wheel') self.navigator.register_callback(self.rx_finished, 'right_button_ok') self.navigator.register_callback(self.rx_cheat_code, 'head_button_ok') # Gripper self.gripper = intera_interface.get_current_gripper_interface() if isinstance(self.gripper, intera_interface.SimpleClickSmartGripper): if self.gripper.needs_init(): self.gripper.initialize() else: if not (self.gripper.is_calibrated() or self.gripper.calibrate() == True): raise self.open_gripper() # Cuff self.cuff = intera_interface.Cuff() self.cuff_callback_ids = self.CUFF_BUTTON.copy() for key in self.cuff_callback_ids: self.cuff_callback_ids[key] = -1 # Initialization complete. Spin. rospy.loginfo('Ready.') r = rospy.Rate(10) while not rospy.is_shutdown(): suppress_cuff_interaction.publish() r.sleep()
#!/usr/bin/env python2 import numpy as np import rospy import intera_interface rospy.init_node("intera-joint-recorder") # Create an object to interface with the arm limb = intera_interface.Limb('right') gripper = intera_interface.Gripper('right') cuff = intera_interface.Cuff('right') lights = intera_interface.Lights() # Move the robot to the starting point angles = limb.joint_angles() angles['right_j0'] = np.radians(0) angles['right_j1'] = np.radians(-50) angles['right_j2'] = np.radians(0) angles['right_j3'] = np.radians(120) angles['right_j4'] = np.radians(0) angles['right_j5'] = np.radians(0) angles['right_j6'] = np.radians(0) limb.move_to_joint_positions(angles) q = [] t = [] lights.set_light_state('right_hand_red_light') while not cuff.lower_button():