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