def __init__(self, trajectory_planner, limb="right", hover_distance=0.08, tip_name="right_gripper_tip"):
"""
:param limb:
:param hover_distance:
:param tip_name:
"""
self.trajectory_planner = trajectory_planner
self._limb_name = limb # string
self._tip_name = tip_name # string
self._hover_distance = hover_distance # in meters
self._limb = intera_interface.Limb(limb)
if demo_constants.is_real_robot():
self._gripper =PSGGripper()
else:
self._gripper = intera_interface.Gripper()
self._robot_enable = intera_interface.RobotEnable()
# verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def __init__(self):
self.pub = rospy.Publisher('send_next_command', Bool, queue_size=1)
self.commandPub = rospy.Publisher("/arm_motion_command",
Pose,
queue_size=10)
rospy.Subscriber("/arm_motion_command", Pose, self.recv)
rospy.Subscriber("/robot/joint_states", JointState,
self.recv_curr_position)
print("Getting robot state... ")
rs = intera_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
rs.enable()
self.pose = PoseStamped()
self.command = Pose()
#self.command.position.x = 0.75
#self.command.position.z = 0.65
self.commandRecv = False
self.sendNext = Bool()
self.sendNext.data = True
self.limb_joints = None
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, 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.rs = intera_interface.RobotEnable(CHECK_VERSION)
self.gripper = intera_interface.Gripper('right_gripper')
self.limb = intera_interface.Limb()
self.head = intera_interface.Head()
self.head_angle = .5
self.joint_names = self.limb.joint_names()
self.target_pose = Pose()
self.select_throw = 1
self.underhand_throw_speed = 0.8
self.underhand_target_angle = -1.5
self.underhand_release_angle = .75
self.overhand_throw_speed = 1
self.overhand_target_angle = -3
self.overhand_release_angle = -1.3
self.overhand_throw_offset = 0
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("pick_and_place_moveit", log_level=rospy.DEBUG)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("right_arm")
self.group.clear_pose_targets()
self.group.allow_replanning(True)
rospy.logdebug(self.group.get_current_joint_values())
self._hover_distance = rospy.get_param("~hover_distance", 0.4)
self._limb_name = rospy.get_param("~limb", 'right')
self._limb = intera_interface.Limb(self._limb_name)
self._gripper = intera_interface.Gripper(self._limb_name)
if self._gripper is None:
rospy.logerr("Gripper error")
else:
rospy.logdebug("Gripper OK")
# Enable the robot
_rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
_init_state = _rs.state().enabled
rospy.logdebug("Enabling robot... ")
_rs.enable()
rospy.Service("pick_and_place", PickAndPlace, self.execute)
rospy.Service("move_to_position", PositionMovement,
self.move_to_position)
rospy.Service("move_to_joint", JointMovement, self.move_to_joint)
rospy.Service("move_head", HeadMovement, self.move_head)
rospy.logdebug("PNP Ready")
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 __init__(self, limb="right"):
# Constants
self._key_command_positions = "cs225a::sawyer::sensors::q"
self._key_timestamp = "cs225a::sawyer::timestamp"
self._dof = 7
self._control_rate = 2500.0 # Hz
self._timeout_missed_cmds = 20.0 # Missed cycles before triggering timeout
self._joint_names = ["{0}_j{1}".format(limb, i) for i in range(self._dof)]
# Initialize limb interface
self._limb = intera_interface.Limb(limb)
# Initialize redis instance
self._redis = redis.StrictRedis(host="127.0.0.1", port=6379, db=0)
# Reset redis q values so they aren't random
self._redis.set(self._key_command_positions, "0 -0.7854 0 1.5708 0 -0.7854 0")
# Verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
# Get initial time
self._start_time = rospy.Time.from_sec(time.time())
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 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 start_robot(self):
self.rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
self.rs.enable()
self.limb = intera_interface.Limb('right')
self._sticks.set_limb(self.limb)
rospy.Subscriber("/robot/limb/right/endpoint_state", EndpointState,
self._cb_endpoint_received)
def __init__(self, reconfig_server, limb="right"):
self._dyn = reconfig_server
# create our limb instance
self._limb = intera_interface.Limb(limb)
# verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self._performedMotion = False
self.marker = Marker()
self.tf_buffer = tf2_ros.Buffer(
rospy.Duration(1200.0)) #tf buffer length
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
print("Running. Ctrl-c to quit")
self.polishTouchPointSub = rospy.Subscriber("/polishTouchPose",
PoseStamped,
self.pose_callback)
self.markerPub = rospy.Publisher("/polishTouchGotoPose", Marker)
self.flagPub = rospy.Publisher("/flag_topic", String)
rospy.on_shutdown(self.clean_shutdown)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
self.flagPub.publish(str(self._performedMotion))
if self._performedMotion:
self.markerPub.publish(self.marker)
rate.sleep()
def __init__(self,
limb="right",
hover_distance=0.16,
tip_name="right_hand"):
self._limb = intera_interface.Limb(limb)
self._gripper = intera_interface.Gripper()
self._limb_name = limb
self._process = bool
self._hover_distance = hover_distance
self._tip_name = tip_name
self._starting_pose = Pose()
self._gripper.set_cmd_velocity(0.16)
self.speed_ratio = 0.3
self.accel_ratio = 0.4
self.linear_speed = 0.3
self.linear_accel = 0.4
self._Sequence = server.Sequence()
self._camera_check = self._Sequence.poses['Camera_check']
self._zero_pose = self._Sequence._zero_pose
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def main():
"""RSDK Inverse Kinematics Example
A simple example of using the Rethink Inverse Kinematics
Service which returns the joint angles and validity for
a requested Cartesian Pose.
Run this example, the example will use the default limb
and call the Service with a sample Cartesian
Pose, pre-defined in the example code, printing the
response of whether a valid joint solution was found,
and if so, the corresponding joint angles.
"""
# Create a publisher that will publish strings to the ik_status topic
pub = rospy.Publisher('ik_status', String, queue_size=10)
rospy.init_node("rsdk_flex_ik_service_client")
rate = rospy.Rate(10) # Publishing rate in Hz
# Starts-up/enables the robot
rs = intera_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
rs.enable()
subscriber = rospy.Subscriber('move_to_dest/goal',
PoseStamped,
callback=test_callback)
rospy.spin()
def __init__(self):
rospy.init_node("pick_and_place_node", log_level=rospy.DEBUG)
self._hover_distance = rospy.get_param("~hover_distance", 0.2)
self._limb_name = rospy.get_param("~limb", 'right')
self._limb = intera_interface.Limb(self._limb_name)
self._limb.set_joint_position_speed(0.3)
self._gripper = intera_interface.Gripper(self._limb_name)
if self._gripper is None:
rospy.logerr("Gripper error")
else:
rospy.logdebug("Gripper OK")
self._head = intera_interface.Head()
self._iksvc_name = "ExternalTools/" + self._limb_name + "/PositionKinematicsNode/IKService"
rospy.wait_for_service(self._iksvc_name, 5.0)
self._iksvc = rospy.ServiceProxy(self._iksvc_name, SolvePositionIK)
# Enable the robot
_rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
_init_state = _rs.state().enabled
rospy.logdebug("Enabling robot... ")
_rs.enable()
rospy.Service("pick_and_place", PickAndPlace, self.pnp)
rospy.Service("pick_and_throw", PickAndPlace, self.pnt)
rospy.Service("move_to_position", PositionMovement,
self.move_to_position)
rospy.Service("move_to_joint", JointMovement, self.move_to_joint)
rospy.Service("move_head", HeadMovement, self.move_head)
#rospy.Service("throw", Throw, self.throw)
rospy.logdebug("PNP Ready")
def main():
try:
rospy.init_node("head_turner")
# head_turner = HeadTurner()
# #rospy.on_shutdown(head_turner.clean_shutdown)
# head_turner.set_neutral()
# head_turner.turn_head(.4)
command_rate = rospy.Rate(1)
control_rate = rospy.Rate(100)
head = intera_interface.Head()
rs = intera_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
print("Enabling robot... ")
rs.enable()
angle = 0.95
while not rospy.is_shutdown(): #and (not (abs(head.pan() - angle) <=
# intera_interface.HEAD_PAN_ANGLE_TOLERANCE)):
head.set_pan(angle, speed=0.3, timeout=0)
print("Current head angle = {}".format(head.pan()))
print("Desired head angle = {}".format(angle))
control_rate.sleep()
command_rate.sleep()
#after_turn = head_turner.get_head_pan()
#print("Angle after turn = {}".format(after_turn))
#rospy.signal_shutdown("Head finished turning")
except:
print("bad things")
def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument('-f',
'--file',
metavar='PATH',
required=True,
help='path to input file')
parser.add_argument('-n',
'--number_loops',
type=int,
default=1,
help='number of playback loops. 0=infinite.')
args = parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
rospy.init_node("taozheng_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()
traj.parse_file(path.expanduser(args.file))
def __init__(self, robot_name='sawyer', print_debug=False, email_cred_file='', log_file='', control_rate=800, gripper_attached='wsg-50'):
super(SawyerImpedanceController, self).__init__(robot_name, print_debug, email_cred_file, log_file, control_rate, gripper_attached)
self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
self._limb = intera_interface.Limb("right")
self.joint_names = self._limb.joint_names()
self._ep_handler = LatestEEObs()
self._cmd_publisher = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=100)
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 __init__(self):
"""Initializes a controller for the robot"""
print("Initializing node... ")
rospy.init_node("sawyer_custom_controller")
rospy.on_shutdown(self.clean_shutdown)
rs = intera_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
print("Robot enabled...")
self.limb = intera_interface.Limb("right")
try:
self.gripper = intera_interface.Gripper("right")
except:
self.has_gripper = False
rospy.logerr("Could not initalize the gripper.")
else:
self.has_gripper = True
self.joint_names = self.limb.joint_names()
print("Done initializing controller.")
# set gripper
try:
self.gripper = intera_interface.Gripper("right")
except ValueError:
rospy.logerr("Could not detect a gripper attached to the robot.")
return
self.gripper.calibrate()
self.gripper.set_velocity(
self.gripper.MAX_VELOCITY) #"set 100% velocity"),
self.gripper.open()
def __init__(self, params):
self._dyn = params.reconfig_server
self._freq, self._missed_cmds = 20.0, 5.0
# Control parameters
self.bounds = params.bound * np.ones([7])
# Create our limb instance
self._limb = intera_interface.Limb(params.get('limb', 'right'))
# Create cuff disable publisher
cuff_ns = "robot/limb/%s/supress_cuff_interaction" % params.get('limb', 'right')
self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
# Verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
# PD controller gains for resets
self.P = np.array([5, 5, 10, 15, 30, 20, 25]) # P controller gains
self.K = np.array([0.7, 0.2, 1.63, 0.5, 3.5, 4.7, 6.0]) # D controller gains
self._smoother_params = params.smoother_params
self._smoother_history = np.zeros(shape=[self._smoother_params.get("history_length", 1), 7])
self.sent_torques = []
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 __init__(self, ):
rospy.on_shutdown(self.clean_shutdown)
self._rate = 500.0
self._right_arm = intera_interface.limb.Limb("right")
self._right_joint_names = self._right_arm.joint_names()
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self.controller = SawyerEEFVelocityController()
self._right_arm.move_to_neutral(speed=0.1)
# Getting the base rotation in eef for coordinate frame transformations
eef_pose_in_base = self._right_arm.endpoint_pose()
self.eef_quat_in_base = np.array([
eef_pose_in_base['orientation'].x,
eef_pose_in_base['orientation'].y,
eef_pose_in_base['orientation'].z,
eef_pose_in_base['orientation'].w,
])
self.eef_rot_in_base = T.quat2mat(self.eef_quat_in_base)
self.base_rot_in_eef = self.eef_rot_in_base.T
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 main():
print_bindings()
rospy.init_node("sawyer_head_control")
head = SawyerHeadControl()
rs = intera_interface.RobotEnable(CHECK_VERSION)
rs.enable()
map_keyboard(head)
def __init__(self, reconfig_server, limb="right"):
self._dyn = reconfig_server
# control parameters
self._rate = 1000.0 # Hz
self._missed_cmds = 20.0 # Missed cycles before triggering timeout
# create our limb instance
self._limb = intera_interface.Limb(limb)
# initialize parameters
self._springs = dict()
self._damping = dict()
self._start_angles = dict()
# create cuff disable publisher
cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
# verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
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 execute(self):
"""
Initializes a connection to the gripper.
"""
self._gripper = intera_interface.Gripper()
rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
rs.enable()
self.spin()
def execute(self):
"""
Initialize the connection to the limb.
"""
self.limb = intera_interface.Limb('right')
rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
rs.enable()
self.spin()
def enabled(self):
"""
If robot is enabled.
:return: if robot is enabled.
"""
return intera_interface.RobotEnable(
intera_interface.CHECK_VERSION).state().enabled
