def __init__(self):
rospy.init_node("baxter_test")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
self._rs.enable()
self._rate = 5.0 #Hz
#IK
self.ns = "ExternalTools/left/PositionKinematicsNode/IKService"
self.hdr = Header(stamp = rospy.Time(0), frame_id = "base")
self.iksvc = rospy.ServiceProxy(self.ns, SolvePositionIK)
self.ikreq = SolvePositionIKRequest()
self._left_arm = baxter_interface.limb.Limb("left")
self._right_arm = baxter_interface.limb.Limb("right")
self._left_joint_names = self._left_arm.joint_names()
self._right_joint_names = self._right_arm.joint_names()
self._left_angles = [self._left_arm.joint_angle(joint) for joint in self._left_joint_names]
self._right_angles = [self._right_arm.joint_angle(joint) for joint in self._right_joint_names]
def __init__(self, limb, verbose=True):
self._limb_name = limb # string
self._verbose = verbose # bool
self._limb = baxter_interface.Limb(limb)
self._gripper = baxter_interface.Gripper(limb)
self._limb.set_joint_position_speed(0.1)
self._gripper.calibrate()
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def __init__(self):
self._limb = baxter_interface.limb.Limb("right")
self._angles = self._limb.joint_angles()
# open left gripper
#self.leftGripper =baxter_interface.Gripper('left')
#self.leftGripper.calibrate()
#self.leftGripper.open()
# open Right gripper
self.rightGripper = baxter_interface.Gripper('right')
self.rightGripper.calibrate()
self.rightGripper.open()
# set control params
print("Getting robot state...")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot..")
self._rs.enable()
def __init__(self, limb, hover_distance=0.15, verbose=True):
self._limb_name = limb # string
self._hover_distance = hover_distance # in meters
self._verbose = verbose # bool
self._limb = baxter_interface.Limb(limb)
self.joint_space = len(self._limb.joint_angles())
self._gripper = baxter_interface.Gripper(limb)
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self.ctr = 0
self.canSubscriber = True
def __init__(self):
# initialize interfaces
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
self.leftGripper = baxter_interface.Gripper('left', CHECK_VERSION)
self.rightGripper = baxter_interface.Gripper('right', CHECK_VERSION)
# MANUAL JOINT CONTROL
self.leftLimb = baxter_interface.Limb('left')
self.rightLimb = baxter_interface.Limb('right')
self.manualJointAccuracy = baxter_interface.settings.JOINT_ANGLE_TOLERANCE
self.manualJointTimeout = 20.0
# JOINT SPEEDS
self.manualJointSpeed = 0.3
self.leftLimb.set_joint_position_speed(self.manualJointSpeed)
self.rightLimb.set_joint_position_speed(self.manualJointSpeed)
def __init__(self):
"""
'Waves' right arm by driving the joint velocities to sinusoid functions
"""
self._pub_rate = rospy.Publisher('/robot/joint_state_publish_rate',
UInt16)
self._left_arm = baxter_interface.limb.Limb("left")
self._right_arm = baxter_interface.limb.Limb("right")
self._right_joint_names = self._right_arm.joint_names()
self._left_joint_names = self._left_arm.joint_names()
self._head = baxter_interface.Head()
# set joint state publishing to 500Hz
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
self._rs.enable()
self._pub_rate.publish(500)
def __init__(self, limb, hover_distance=0.15, verbose=True):
self._limb_name = limb # string
self._hover_distance = hover_distance # in meters
self._verbose = verbose # bool
self._limb = baxter_interface.Limb(limb)
self._gripper = baxter_interface.Gripper(limb)
ns = "ExternalTools/" + self._limb_name + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self.block_detected = False
self.block_poses = []
self.pose = Pose()
def __init__(self):
self._speak_ctr = PollySpeech()
self._mbot_relay = MultiBotInterface()
self._face_ctr = FaceController()
#enable robot
enable_robot = baxter_interface.RobotEnable()
enable_robot.enable()
rospack = rospkg.RosPack()
self._package_path = rospack.get_path(
'hri19_multirobot_transition_study')
#to deal with SST
rospy.Subscriber('stt', Speech, self._stt_callback, queue_size=1)
self._stt_toggle = rospy.ServiceProxy('toggle_stt', SetBool)
self._last_sentence = ""
self._stt_condition = threading.Condition()
self._stt_event = threading.Event()
self._pause_flag = True
self._nlu_engine = SnipsNLU()
self._head = baxter_interface.Head()
self._running = True
#memory for each session
self._temp_memory = dict()
self._memory_stack = list()
self._index_stack = list()
self._index = 0
""" The following are the list of actions/behaviors
that exist for the robot, each action's name directly correspond
to the same name of the function
"""
self._operation_list = [
'speak', 'move_to_posture', 'move_head', 'change_face',
'natural_language_understanding', 'wait_response', 'wait',
'yes_no_question', 'signal_robot', 'wait_for_signal', 'run_file',
'choice_question', 'load_param'
]
rospy.loginfo("RUN Complete Initiailization")
def main():
print("Initializing node... ")
rospy.init_node("set_joint_positions")
#################Resposible for setting joint positions for Baxter ############################################
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
def clean_shutdown():
print("\nExiting example...")
if not init_state:
print("Disabling robot...")
rs.disable()
print("Enabling robot... ")
rs.enable()
#right_e1 is special. joint limit [-0.05] is the reason. keeping it zero for now
#left_e1 is special. joint limit [-0.05] is the reason. keeping it zero for now
# joint_name_left=["left_s0", "left_s1", "left_e0", "left_e1", "left_w0", "left_w1", "left_w2"]
# joint_name_right=["right_e0", "right_e1", "right_s0", "right_s1", "right_w0", "right_w1", "right_w2"]
###################################################################################################################
# joint_name_left=["left_e0", "left_e1", "left_s0", "left_s1", "left_w0", "left_w1", "left_w2"]
# joint_desired_state_right=[0.7410703227630888, 1.5362719506658014, 0.01001108907777759, -0.7006024655803085, 0.9735489859081357, 1.447864944629445, -0.4520477771263609]
# joint_desired_state_left=[-0.02477285952504804, 1.5454976873355646, -0.34985937977411385, -0.862626895167157, -1.3115418478918244, 1.8487306917056667, -0.892602931497076]
###################################################################################################################
joint_name_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
joint_desired_state_left= [0.041363752507206364, 1.9348352917089535, -0.2726798000286923, -0.5658538514940457, -1.1000522708348006, 1.7430249026235884, -0.1936378991920895]
joint_name_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
joint_desired_state_right = [0.6068619915741271, 1.911764079997937, -0.13185736132604742, -0.4816721820621366, 0.790763174233601, 1.304727973182544, -1.0247054998116605]
joint_name_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
joint_desired_state_left= [0.4345000581685434, 1.4078108680818384, -1.117505003974524, -0.8383205005793786, -0.35128160042575973, 1.0530778108833365, 0.20401944478876002]
joint_name_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
joint_desired_state_right = [-0.33824276372873374, 1.5082866096883332, 1.035053536625683, -0.9276748814737039, 0.25080585881926515, 1.0895098545956152, -0.07209709703061444]
setJointPositions(joint_name_right, joint_name_left, joint_desired_state_right, joint_desired_state_left)
######################################################################################################################
#################Resposible for getting joint positions for Baxter from /robot/joint_states topic ############################################
joint_states_current_right, joint_states_current_left=getCurrentJointState(joint_name_right, joint_name_left)
printSetPositionResult(joint_desired_state_right, joint_states_current_right, joint_desired_state_left, joint_states_current_left,joint_name_right, joint_name_left)
def __init__(self, limb, reconfig_server):
self._dyn = reconfig_server
# control parameters
self._rate = 1000.0 # Hz
self._missed_cmds = 20.0 # Missed cycles before triggering timeout
self.joint_names = ['left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
# define start and end time to calcualte velocity
self._end = time()
self._start = time()
# create our limb instance
self._limb = baxter_interface.Limb(limb)
self._start_angles = dict()
# self._dof_names = {'x': 1, 'y': 2, 'z': 3}
self._dof_names = ['x', 'y', 'z']
# initialize parameters
self._springs = dict()
self._damping = dict()
self._start_pos = [0, 0, 0]
self._last_pos = self._start_pos
self._kin = baxter_kinematics('left')
# gravity compensation
self._gravity_comp_effort = dict()
self._gravity_comp_effort_ok = False
gravity_comp_topic = '/robot/limb/left/gravity_compensation_torques'
self._gravity_comp_sub = rospy.Subscriber(
gravity_comp_topic,
SEAJointState,
self._on_gravity_comp,
queue_size=1,
tcp_nodelay=True)
# 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 = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
self.safe_file = open('mani.csv', 'w')
def __init__(self, arm, distance):
global image_directory
baxter_interface.RobotEnable().enable()
# arm ("left" or "right")
self.limb = arm
self.distance = distance
self.limb_interface = baxter_interface.Limb(self.limb)
self.gripper = baxter_interface.Gripper(arm)
self.gripper.calibrate()
self.other_limb_interface = baxter_interface.Limb("left")
self.baxter_ik_move("left", (0.25, 0.50, 0.2, math.pi, 0.0, 0.0))
self.limb_interface.set_joint_position_speed(0.1)
self.other_limb_interface.set_joint_position_speed(0.1)
#self.open_camera(arm)
self.bridge = CvBridge()
# image directory
self.image_dir = image_directory
# flag to control saving of analysis images
self.save_images = True
# required position accuracy in metres
# self.ball_tolerance = 0.005
# self.tray_tolerance = 0.05
# number of balls found
# self.balls_found = 0
# start positions
self.origin_x = 0.6 # x = front back 0.50 0.70(good)
self.origin_y = -0.1 # y = left right 0.30 -0.1(good)
self.origin_z = 0 # z = up down 0.15
self.roll = -1.0 * math.pi # roll = horizontal
self.pitch = 0.0 * math.pi # pitch = vertical
self.yaw = 0.0 * math.pi # yaw = rotation
self.pose = [self.origin_x, self.origin_y, self.origin_z, \
self.roll, self.pitch, self.yaw]
def __init__(self):
# save the output messages into .txt file in order to draw plot
#self.saveFile = open('record.txt', 'w')
#self.saveFile1 = open('record1.txt', 'w')
self.limb_interface = baxter_interface.Limb("left")
# start positions
self.x = 0.62 # x = front back
self.y = 0.14 # y = left right
self.z = 0.14 # z = up down
self.roll = -1.0 * math.pi # roll = horizontal
self.pitch = 0.0 * math.pi # pitch = vertical
self.yaw = 0.0 * math.pi # yaw = rotation
self.pose = [self.x, self.y, self.z, self.roll, self.pitch, self.yaw]
print(self.pose)
# Enable the actuators
baxter_interface.RobotEnable().enable()
# set speed as a ratio of maximum speed
self.limb_interface.set_joint_position_speed(0.5)
# height of table
self.table = -0.077
# camera parameter
# This is a very interesting argument. Actually this is a simplification of the camera projection transformation.
# This parameter works only when the camera is vertical to the detection surface!
self.cam_calib = 0.0025 # meters per pixel at 1 meter.
self.width = 960 # Camera resolution
self.height = 600
# bridge to use opencv to process images
self.bridge = CvBridge()
# subscribe image message from webcam
self.image_sub = rospy.Subscriber("/cameras/left_hand_camera/image",
Image, self.callback)
# setup topic for images with detection results
self.image_pub = rospy.Publisher('/image_bridge',
Image,
latch=True,
queue_size=10)
self.XL_coo = rospy.Publisher('/Xcoordinate_left',
Float32,
latch=True,
queue_size=10)
self.YL_coo = rospy.Publisher('/Ycoordinate_left',
Float32,
latch=True,
queue_size=10)
def __init__(self,
shape,
desired_position,
limb,
hover_distance=0.15,
verbose=False):
self._limb_name = limb # string
self._shape_name = shape # string
self._desired_position = desired_position # list[x,y] (integer)
self._hover_distance = hover_distance # in meters
self._verbose = verbose # bool
self._limb = baxter_interface.limb.Limb(limb)
self._limb_joint_names = self._limb.joint_names()
self.prev_time = 0.0
self.control_effort_x = 0.0
self.control_effort_y = 0.0
self.object_position = Object()
self.hover_position = Pose()
self.diff_old = 0.0
self.zposi = 0.0
self.step = 0
self._rate = 10 #10Hz
self.display_pub = rospy.Publisher('/robot/xdisplay',
Image,
queue_size=60)
self.pub_grasp_now = rospy.Publisher("pump_on", Empty, queue_size=1)
self.pub_release_now = rospy.Publisher("pump_off", Empty, queue_size=1)
self.sub = rospy.Subscriber('/detected_image', Image, self.republish,
None, 1)
#msg_grasp_now = Bool(False)
#self.pub_grasp_now.publish(msg_grasp_now)
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Moving to neutral pose...")
self._limb.move_to_neutral()
def main():
# arg_fmt = argparse.RawDescriptionHelpFormatter
# parser = argparse.ArgumentParser(formatter_class=arg_fmt,
# description=main.__doc__)
# parser.add_argument(
# '-l', '--limb', choices=['left', 'right'], required=True,
# help="the limb to test"
# )
# args = parser.parse_args(rospy.myargv()[1:])
print("Getting robot state... ")
rospy.init_node("IKSolver")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
limb = baxter_interface.Limb("right")
right = baxter_interface.Gripper('right', CHECK_VERSION)
# return ik_test(args.limb)
goToOrigin(limb)
current_pose = limb.endpoint_pose()
print current_pose
print limb.joint_angles()
running = True
while (running):
x = raw_input("X Value?\n")
y = raw_input("Y Value?\n")
z = raw_input("Z Value?\n")
joints = solveIK('right', float(x), float(y), float(z),
current_pose['orientation'].x,
current_pose['orientation'].y,
current_pose['orientation'].z,
current_pose['orientation'].w)
if (x == " " or y == " " or z == " "):
running = False
else:
if (joints):
print "moving"
limb.move_to_joint_positions(joints)
def __init__(self, verbose=False):
if not ProxyTrajectoryClient._is_initialized:
ProxyTrajectoryClient._is_initialized = True
Logger.loginfo(
"Initializing proxy FollowJointTrajectory client...")
ProxyTrajectoryClient._client = ProxyActionClient({
ProxyTrajectoryClient._action_topic:
FollowJointTrajectoryAction
})
ProxyTrajectoryClient._verbose = verbose
# enable the IK Service
ik_ns = "ExternalTools/" + ProxyTrajectoryClient._limb + "/PositionKinematicsNode/IKService"
ProxyTrajectoryClient._iksvc = rospy.ServiceProxy(
ik_ns, SolvePositionIK)
rospy.wait_for_service(ik_ns, 5.0)
# enable the Baxter
ProxyTrajectoryClient._rs = baxter_interface.RobotEnable(
baxter_interface.CHECK_VERSION)
ProxyTrajectoryClient._init_state = ProxyTrajectoryClient._rs.state(
).enabled
print("Enabling robot... ")
ProxyTrajectoryClient._rs.enable()
# enable baxter limb interface
ProxyTrajectoryClient._baxter_limb_interface = baxter_interface.limb.Limb(
ProxyTrajectoryClient._limb)
# Get the names from joints
ProxyTrajectoryClient._joint_name = [
joint for joint in
ProxyTrajectoryClient._baxter_limb_interface.joint_names()
]
ProxyTrajectoryClient._gripper = baxter_interface.Gripper(
ProxyTrajectoryClient._limb)
self._goal = FollowJointTrajectoryGoal()
self._result = None
self._goal_time_tolerance = rospy.Time(0.1)
self._goal.goal_time_tolerance = self._goal_time_tolerance
def __init__(self, arm):
self.limb = arm
self.limb_interface = baxter_interface.Limb(arm)
self.gripper = baxter_interface.Gripper(arm)
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.tolerance = 0.005
self.pos_x = 0.50
self.pos_y = 0.00
self.pos_z = 0.05
self.or_x = 0.0
self.or_y = -1.0
self.or_z = 0.0
self.or_w = 0.0
self.pose = [self.pos_x, self.pos_y, self.pos_z, self.or_x, self.or_y, self.or_z, self.or_w]
self.hough_accumulator = 30
self.hough_min_radius = 35
self.hough_max_radius = 70
self.cam_calib = 0.0004
self.cam_x_offset = 0.045
self.cam_y_offset = -0.01
self.width = 960
self.height = 600
self.cv_image = cv.CreateImage((self.width, self.height), 8, 3)
baxter_interface.RobotEnable().enable()
self.limb_interface.set_joint_position_speed(0.5)
self.gripper.calibrate()
self.reset_camera(self.limb, self.width, self.height)
self.subscribe_to_camera(self.limb)
self.distance = 0
def __init__(self, limb, verbose=True):
# Initialize parameters
self._limb_name = limb
self._verbose = False
self._limb = baxter_interface.Limb(limb)
self._gripper = baxter_interface.Gripper(limb)
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
left = baxter_interface.Gripper('left', baxter_interface.CHECK_VERSION)
if self._verbose:
print("Getting robot state... ")
print("Enabling robot... ")
self._rs.enable()
if self._verbose:
print("Calibrating gripper...")
left.calibrate()
# Node cycle rate (in Hz).
self.loop_rate = rospy.Rate(100)
def main(limb):
print("Initializing node... ")
rospy.init_node("rethink_rsdk_joint_trajectory_controller_test")
print("Getting robot state... ")
rs = baxter_interface.RobotEnable()
print("Enabling robot... ")
rs.enable()
print("Running. Ctrl-c to quit")
positions = {
'left': [-0.11, -0.62, -1.15, 1.32, 0.80, 1.27, 2.39],
'right': [0.11, -0.62, 1.15, 1.32, -0.80, 1.27, -2.39],
}
p1 = positions[limb]
traj = Trajectory(limb)
traj.add_point(p1, 7.0)
traj.add_point([x * 0.75 for x in p1], 9.0)
traj.add_point([x * 1.25 for x in p1], 12.0)
traj.start()
traj.wait()
def __init__(self):
print(' initializing Tf2AnglesController node ')
rospy.init_node('Tf2AnglesController', anonymous=True)
# Enable the Robot
self.__robot = baxter_interface.RobotEnable(CHECK_VERSION)
# Robot Limbs
self.__left_limb = baxter_interface.limb.Limb('left')
self.__right_limb = baxter_interface.limb.Limb('right')
self.__rate = rospy.Rate(10)
self.__command_joint_angles = dict()
self.__enable_robot_and_move_to_neutral_pose()
self.__setup_subscribers()
self.__setup_command_joint_angles()
def main():
rospy.init_node("rsdk_test")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
def clean_shutdown():
if not init_state:
rs.disable()
rospy.on_shutdown(clean_shutdown)
rs.enable()
print('1. velocity \n 2. path')
c = raw_input()
if c == 1:
run_velocity_loop()
else if c == 2:
run_path_loop()
def __init__(self):
"""
'Wobbles' both arms by commanding joint velocities sinusoidally.
"""
self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
UInt16)
self._left_arm = baxter_interface.limb.Limb("left")
self._right_arm = baxter_interface.limb.Limb("right")
self._left_joint_names = self._left_arm.joint_names()
self._right_joint_names = self._right_arm.joint_names()
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# set joint state publishing to 100Hz
self._pub_rate.publish(100)
def main():
rospy.init_node("pick_and_place_service")
parser = argparse.ArgumentParser(
description='Process Pick and Place Command Line Arguments')
parser.add_argument('--save', '-s', type=str)
parser.add_argument('--read', '-r', type=str)
args = parser.parse_args()
rs = baxter_interface.RobotEnable()
print("Enabling Robot")
rs.enable()
pp = PickPlace('right')
# Calibrate Pickup Locations
if args.read:
print "Reading Position File: " + args.read
pp.ReadCalibration(args.read)
else:
pp.CalibrateObjects()
# Post Params
pp.PostParameters()
# Save Calibration to File
if args.save:
print "Saving Calibration File: " + args.save
pp.SaveCalibration(args.save)
# Advertise Service
s = rospy.Service('pick_and_place_object', pick_and_place,
pp.PickAndPlaceObject)
s_2 = rospy.Service('pick_and_place_check', pick_and_place,
pp.PickAndPlaceCheck)
s_3 = rospy.Service('pick_and_place_state', pick_and_place_state,
pp.PickAndPlaceState)
s_4 = rospy.Service('pick_and_place_stop', pick_and_place_stop,
pp.PickAndPlaceStop)
print "READY to PICK and Place"
rospy.spin()
def main():
global data_s, mutex
t = threading.Thread(target=receive)
t.setDaemon(True)
t.start()
print("Initializing node... ")
rospy.init_node('moveit_demo')
rospy.sleep(1)
rs = baxter_interface.RobotEnable(CHECK_VERSION)
limb = baxter_interface.Limb("right")
# limb.move_to_neutral()
moveit = MoveItDemo()
print "Waiting to receive messages..."
while True:
if data_s in [
"fight", "fight0", "handshake", "fetch", "hello", "salute"
]:
moveit.execute(data_s) #execute action
print "reset..." #reset pose
rs.reset()
print "move to natual..."
limb.move_to_neutral()
if mutex.acquire(): #clear command
data_s = ""
mutex.release()
# if data_s == "exit":
# moveit_commander.roscpp_shutdown()
# moveit_commander.os._exit(0)
# break
UDPSock.close()
os._exit(0)
def image_callback(ros_img):
pattern = cv2.imread("/home/crl7/ros_ws/marker.jpg")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
#get robot state
init_state = rs.state().enabled
#enable robot
rs.enable()
#left arm initial state
left = baxter_interface.Limb('left')
initial_left = left.endpoint_pose()['position'][:] + left.endpoint_pose()['orientation'][:]
Pose_state_left = [x for x in initial_left]
limb = 'left'
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
def listener():
rospy.init_node('end_effector_trajectory_client', anonymous=True)
fine = False
rate = rospy.Rate(1)
while not fine | rospy.is_shutdown():
try:
rs = baxter_interface.RobotEnable()
rs.enable()
fine = True
except (OSError):
rospy.logwarn("Can not enable robot.Will keep trying...")
rate.sleep()
if rospy.is_shutdown():
return
rospy.Subscriber("end_effector_command_solution", JointCommand, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def __init__(self, limb, hover_distance=0.15, verbose=True):
self._limb_name = limb # string
self._hover_distance = hover_distance # in meters
self._verbose = verbose # bool
self.cap_status = '1.0' #string
self.status = 'working' #string
self._limb = baxter_interface.Limb(limb)
self._gripper = baxter_interface.Gripper(limb)
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self.cap_sub = rospy.Subscriber('capflag', Float64,
self.callback) #initialize listener
def __init__(self, limb, amplification=1.0):
"""
Puppets one arm with the other.
@param limb: the control arm used to puppet the other
@param amplification: factor by which to amplify the arm movement
"""
puppet_arm = {"left": "right", "right": "left"}
self._control_limb = limb
self._puppet_limb = puppet_arm[limb]
self._control_arm = baxter_interface.limb.Limb(self._control_limb)
self._puppet_arm = baxter_interface.limb.Limb(self._puppet_limb)
self._amp = amplification
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def main():
# arg_fmt = argparse.RawDescriptionHelpFormatter
# parser = argparse.ArgumentParser(formatter_class=arg_fmt,
# description=main.__doc__)
# parser.add_argument(
# '-l', '--limb', choices=['left', 'right'], required=True,
# help="the limb to test"
# )
# args = parser.parse_args(rospy.myargv()[1:])
print("Getting robot state... ")
rospy.init_node("IKSolver")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
limb = baxter_interface.Limb("right")
right = baxter_interface.Gripper('right', CHECK_VERSION)
# return ik_test(args.limb)
goToOrigin(limb)
current_pose = limb.endpoint_pose()
print current_pose
print limb.joint_angles()
x = input("X Value?\n")
y = input("Y Value?\n")
z = input("Z Value?\n")
# q1 = input("Q1 Value?\n")
# q2 = input("Q2 Value?\n")
# q3 = input("Q3 Value?\n")
# q4 = input("Q4 Value?\n")
# limb.move_to_neutral()
# joints = solveIK('right', float(x), float(y), float(z), float(q1), float(q2), float(q3), float(q4));
joints = solveIK('right', x, y, z, current_pose['orientation'].x,
current_pose['orientation'].y,
current_pose['orientation'].z,
current_pose['orientation'].w)
if (joints):
limb.move_to_joint_positions(joints)
def main():
parser = argparse.ArgumentParser()
required = parser.add_argument_group('required arguments')
required.add_argument('-l',
'--limb',
required=True,
choices=['left', 'right'],
help='Tare the specified limb')
args = parser.parse_args(rospy.myargv()[1:])
limb = args.limb
print("Initializing node...")
rospy.init_node('rsdk_tare_%s' % (limb, ))
print("Preparing to tare...")
gripper_warn = ("\nIMPORTANT: Make sure to remove grippers and other"
" attachments before running tare.\n")
print(gripper_warn)
if not gripper_removed(args.limb):
return 1
rs = baxter_interface.RobotEnable(CHECK_VERSION)
rs.enable()
tt = Tare(limb)
rospy.loginfo("Running tare on %s limb" % (limb, ))
error = None
try:
tt.run()
except Exception as e:
error = e.strerror
finally:
try:
rs.disable()
except Exception:
pass
if error == None:
rospy.loginfo("Tare finished")
else:
rospy.logerr("Tare failed: %s" % (error, ))
return 0 if error == None else 1
def __init__(self):
self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
UInt16, queue_size=10)
self._left_arm = baxter_interface.limb.Limb("left")
self._right_arm = baxter_interface.limb.Limb("right")
self._left_joint_names = self._left_arm.joint_names()
self._right_joint_names = self._right_arm.joint_names()
# control parameters
self._rate = 500.0 # Hz
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# set joint state publishing to 500Hz
self._pub_rate.publish(self._rate)