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 callback_func(data):
rospy.loginfo("I got the message")
try:
grip = intera_interface.Gripper('right_gripper')
except ValueError:
rospy.logerr("ERROR: Unable to detect Gripper")
return
rospy.loginfo("Gripper position is {}".format(grip.get_position()))
tmp_limb = intera_interface.Limb('right')
end_pt = tmp_limb.endpoint_pose()['position']
print("Endpoint is x: {x}\n y: {y}\n z: {z}".format(x=end_pt[0],
y=end_pt[1],
z=end_pt[2]))
# new_pt = Point(
# x = end_pt[0],
# y = end_pt[1],
# z = end_pt[2]-0.09
# )
# tmp_limb.set_joint_trajectory(["right_j6", "right_j4"], new_pt, [0.3, 0.3], [0.01, 0.01])
grip.set_position(0.01)
rospy.loginfo("Gripper position is {}".format(grip.get_position()))
rospy.signal_shutdown("Finished grip action")
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 close_jaw_incr(self, offset_pos):
gripper = intera_interface.Gripper('right_gripper')
cmd_pos = max(
min(gripper.get_position() - offset_pos, gripper.MAX_POSITION),
gripper.MIN_POSITION)
gripper.set_position(cmd_pos)
print("commanded position set to {} m".format(cmd_pos))
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 __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 __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.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, limb='right', hover_distance=0.0, tip_name="right_gripper_tip"):
self._limb_name = limb
self._limb = intera_interface.Limb(limb)
self._limb.set_joint_position_speed(0.3)
self._tip_name = tip_name
self._gripper = intera_interface.Gripper()
self._hover_distance = hover_distance
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,
initial_joint_pos,
moveit_group,
control_mode='position'):
"""
Sawyer class.
:param initial_joint_pos: {str: float}
{'joint_name': position_value}, and also
initial_joint_pos should include all of the
joints that user wants to control and observe.
:param moveit_group: str
Use this to check safety
:param control_mode: string
robot control mode: 'position' or velocity
or effort
"""
Robot.__init__(self)
self._limb = intera_interface.Limb('right')
self._gripper = intera_interface.Gripper()
self._initial_joint_pos = initial_joint_pos
self._control_mode = control_mode
self._used_joints = []
for joint in initial_joint_pos:
self._used_joints.append(joint)
self._joint_limits = rospy.wait_for_message('/robot/joint_limits',
JointLimits)
self._moveit_group = moveit_group
self._sv = StateValidity()
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 grips(b):
# initialize interfaces
gripper = None
original_deadzone = None
def clean_shutdown():
if gripper and original_deadzone:
gripper.set_dead_zone(original_deadzone)
print("Exiting example.")
try:
gripper = intera_interface.Gripper('right_gripper')
except (ValueError, OSError) as e:
rospy.logerr(
"Could not detect an electric gripper attached to the robot.")
clean_shutdown()
return 'gripper failure'
rospy.on_shutdown(clean_shutdown)
# WARNING: setting the deadzone below this can cause oscillations in
# the gripper position. However, setting the deadzone to this
# value is required to achieve the incremental commands in this example
gripper.set_dead_zone(0.001)
rospy.loginfo("Gripper deadzone set to {}".format(gripper.get_dead_zone()))
if b:
gripper.close()
else:
gripper.open()
return "Gripper Success"
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 close_gripper():
gripper = intera_interface.Gripper('right_gripper')
# calibrate gripper if necessary
if not gripper.is_calibrated():
gripper.calibrate()
gripper.close()
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 gripper_removed(side):
"""
Verify grippers are removed for calibration.
"""
try:
gripper = intera_interface.Gripper(side)
except Exception, e:
return True
def __init__(self):
global LEGO_HEIGHT
self.lego_height = LEGO_HEIGHT
self.currentPos = [None, None, None] #length must be 3, [x,y,z]
self.gripper = intera_interface.Gripper()
self.limb = intera_interface.Limb('right')
self.kinematics = sawyer_kinematics('right')
self.currentLegoPos = [None, None, None]
def __init__(self,
hover_dist=0.213,
table_level=-0.06,
lego_height=0.018,
lego_base_height=0.0,
joint_speed=0.6):
self._limb = None
self._orientation_hand_down = None
self._position_neutral = None
self._gripper = None
self._sub_open = None
self._sub_close = None
self._is_pneumatic = False
self.next_lego_target = None
self.legos_stacked_num = 0
self.base = None
self.hover_dist = hover_dist
self.table_level = table_level
self.lego_height = lego_height
self.lego_base_height = lego_base_height
self.stack_height = lego_base_height - .05
self._arm_camera_joint_angles = {
'right_j6': 4.7126953125,
'right_j5': 0.30809765625,
'right_j4': 1.6166796875,
'right_j3': 1.5452109375,
'right_j2': -1.8612998046875,
'right_j1': -0.1184775390625,
'right_j0': 0.424828125
}
rospy.init_node('Ivan_Jot_Albert_Angus')
self._limb = intera_interface.Limb('right')
self._gripper = intera_interface.Gripper()
self._limb.set_joint_position_speed(joint_speed)
self._cam = None
self.lego_locations = None
# This quaternion will have the hand face straight down (ideal for picking tasks)
self._orientation_hand_down = Quaternion()
self._orientation_hand_down.x = 0.704238785359
self._orientation_hand_down.y = 0.709956638597
self._orientation_hand_down.z = -0.00229009932359
self._orientation_hand_down.w = 0.00201493272073
# This is the default neutral position for the robot's hand (no guarantee this will move the joints to neutral though)
self._position_neutral = Point()
self._position_neutral.x = 0.449559195663
self._position_neutral.y = 0.16070379419
self._position_neutral.z = 0.212938808947
print("Waking up...")
self._limb.move_to_neutral()
rospy.sleep(2)
print("Moved to nuetral.")
def __init__(self, limb="right"):
#create our action server clients
self._limb_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#verify joint trajectory action servers are available
is_server_up = self._limb_client.wait_for_server(rospy.Duration(10.0))
if not is_server_up:
msg = ("Action server not available."
" Verify action server availability.")
rospy.logerr(msg)
rospy.signal_shutdown(msg)
sys.exit(1)
#create our goal request
self.goal = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self.arm = intera_interface.Limb(limb)
self.limb = limb
self.gripper_name = '_'.join([limb, 'gripper'])
#gripper interface - for gripper command playback
#TODO: Gripper not detected excepted
try:
self.gripper = intera_interface.Gripper(self.gripper_name)
except:
self.gripper = None
rospy.loginfo("Did not detect a connected electric gripper.")
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
# Verify Grippers 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()
#gripper goal trajectories
self.grip = FollowJointTrajectoryGoal()
#gripper control rate
self._gripper_rate = 20.0 # Hz
# Timing offset to prevent gripper playback before trajectory has started
self._slow_move_offset = 0.0
self._trajectory_start_offset = rospy.Duration(0.0)
self._trajectory_actual_offset = rospy.Duration(0.0)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
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 pick_up_and_move_block(x, y):
rospy.init_node("GraspingDemo")
global gripper
gripper = intera_interface.Gripper('right_gripper')
gripper.open()
move_to(x, y, 0.20)
move_to(x, y, 0.10)
gripper.close()
move_to(x, y, 0.30)
gripper.open()
def __init__(self):
# Intera interface for the arm and the gripper
self.limb_ = intera_interface.Limb('right')
self.gripper_ = intera_interface.Gripper()
# Verify if sawyer 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,limb = 'right', hover_distance = 0.2, tip_name = "right_gripper_tip"):
self._limb_name = limb
self._tip_name = tip_name
self._limb = intera_interface.Limb(limb)
self._gripper = intera_interface.Gripper()
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 motion_options(target):
global g_limb, g_position_neutral, g_orientation_hand_down
gripper = intera_interface.Gripper()
gripper.open()
'''
# Create a new pose (Position and Orientation) to solve for
target_pose = Pose()
target_pose.position = copy.deepcopy(g_position_neutral)
target_pose.orientation = copy.deepcopy(g_orientation_hand_down)
'''
target_pose = Pose()
target_pose.orientation = copy.deepcopy(g_orientation_hand_down)
if (target == "cup"):
target_pose.position.x = 0.649559195663 #some number
target_pose.position.y = 0.36070379419 #some number
target_pose.position.z = 0.0
elif (target == "bottle"):
target_pose.position.x = 0.649559195663 #some number
target_pose.position.y = -0.16070379419 #some number
target_pose.position.z = 0.0
# Call the IK service to solve for joint angles for the desired pose
target_joint_angles = g_limb.ik_request(target_pose, "right_hand")
# The IK Service returns false if it can't find a joint configuration
if target_joint_angles is False:
rospy.logerr("Couldn't solve for position %s" % str(target_pose))
return
# Set the robot speed (takes a value between 0 and 1)
g_limb.set_joint_position_speed(0.3)
# Send the robot arm to the joint angles in target_joint_angles, wait up to 2 seconds to finish
g_limb.move_to_joint_positions(target_joint_angles, timeout=2)
# Find the new coordinates of the hand and the angles the motors are currently at
new_hand_pose = copy.deepcopy(g_limb._tip_states.states[0].pose)
new_angles = g_limb.joint_angles()
rospy.loginfo("New Hand Pose:\n %s" % str(new_hand_pose))
rospy.loginfo("Target Joint Angles:\n %s" % str(target_joint_angles))
rospy.loginfo("New Joint Angles:\n %s" % str(new_angles))
#close gripper
gripper.close()
#return to neutral
g_limb.move_to_neutral()
#open gripper
gripper.open()
def init():
global g_limb, g_orientation_hand_down, g_position_neutral, pos, posp, gripper
global marker_p, marker_q
global square_p, square_q
global llc_p, llc_q
#Set up arm stuff
rospy.init_node('cairo_sawyer_ik_example')
g_limb = intera_interface.Limb('right')
gripper = intera_interface.Gripper()
# Straight down and 'neutral' position
g_orientation_hand_down = Quaternion()
g_orientation_hand_down.x = 0.704238785359
g_orientation_hand_down.y = 0.709956638597
g_orientation_hand_down.z = -0.00229009932359
g_orientation_hand_down.w = 0.00201493272073
g_position_neutral = Point()
g_position_neutral.x = 0.45371551183
g_position_neutral.y = 0.0663097073071
g_position_neutral.z = 0.0271459370863
#Marker position
marker_q = Quaternion()
marker_q.x = 0.704238785359
marker_q.y = 0.709956638597
marker_q.z = -0.00229009932359
marker_q.w = 0.00201493272073
marker_p = Point()
marker_p.x = 0.525423244892
marker_p.y = 0.254786824385
marker_p.z = 0.0125670410943
#Square position
square_q = Quaternion()
square_q.x = 0.704238785359
square_q.y = 0.709956638597
square_q.z = -0.00229009932359
square_q.w = 0.00201493272073
square_p = Point()
square_p.x = 0.53430244888
square_p.y = -0.152176453277
square_p.z = 0.1125670410943
#Lower Left Corner of board
llc_q = Quaternion()
llc_q.x = 0.704238785359
llc_q.y = 0.709956638597
llc_q.z = -0.00229009932359
llc_q.w = 0.00201493272073
llc_p = Point()
llc_p.x = 0.487424263171
llc_p.y = 0.254786824385 - 0.1
llc_p.z = -0.0431221623924
def __init__(self):
self.limb = intera_interface.Limb('right')
self.limb.move_to_neutral()
self.limb.set_joint_position_speed(0.10)
self.gripper = intera_interface.Gripper("right_gripper")
self.gripper.open()
self.desired_pose = Pose()
current_pose = self.limb.endpoint_pose()
self.desired_pose.orientation = current_pose['orientation']
self.target_srv = rospy.ServiceProxy("/ball_position", Target)
self.target_srv.wait_for_service()
def __init__(self, limb="right", hover_distance = 0.15, tip_name="right_gripper_tip"):
self._limb_name = limb # string
self._tip_name = tip_name # string
self._hover_distance = hover_distance # in meters
self._limb = intera_interface.Limb(limb)
self._gripper = intera_interface.Gripper()
# 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_gripper(limb_name):
try:
gripper = intera_interface.Gripper(limb_name)
except ValueError:
rospy.logerr("Could not detect a gripper attached to the robot.")
return
if gripper.has_error():
gripper.reboot()
if not gripper.is_calibrated():
gripper.calibrate()
gripper.open()
return gripper
def __init__(self, config=None):
"""Initialize.
See the parent class.
"""
super(FrankaPandaReal, self).__init__(config=config)
if rospy.get_name() == '/unnamed':
rospy.init_node('franka_panda')
assert rospy.get_name() != '/unnamed', 'Must call rospy.init_node().'
tic = time.time()
rospy.loginfo('Initializing robot...')
self._head = intera_interface.Head()
self._display = intera_interface.HeadDisplay()
self._lights = intera_interface.Lights()
self._limb = intera_interface.Limb()
self._joint_names = self._limb.joint_names()
self.cmd = []
self._command_msg = JointCommand()
self._command_msg.names = self._joint_names
self._commanders = dict(velocity=None, torque=None)
try:
self._gripper = intera_interface.Gripper()
self._has_gripper = True
except Exception:
self._has_gripper = False
self._robot_enable = intera_interface.RobotEnable(True)
self._params = intera_interface.RobotParams()
self._motion_planning = self.config.MOTION_PLANNING
if self._motion_planning == 'moveit':
rospy.loginfo('Initializing moveit toolkit...')
moveit_commander.roscpp_initialize(sys.argv)
self._scene = moveit_commander.PlanningSceneInterface()
self._group = moveit_commander.MoveGroupCommander('right_arm')
toc = time.time()
rospy.loginfo('Initialization finished after %.3f seconds.'
% (toc - tic))
