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():