def __init__(self, limb, reconfig_server, rate=100.0, mode='position'):
self._dyn = reconfig_server
self._ns = 'robot/limb/' + limb + '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._action_name = rospy.get_name()
self._server.start()
self._limb = baxter_interface.Limb(limb)
self._name = limb
self._cuff = baxter_interface.DigitalIO('%s_lower_cuff' % (limb, ))
self._cuff.state_changed.connect(self._cuff_cb)
# Verify joint control mode
self._mode = mode
if self._mode != 'position' and self._mode != 'velocity':
rospy.logerr("%s: Action Server Creation Failed - "
"Provided Invalid Joint Control Mode '%s' (Options: "
"'position', 'velocity')" % (
self._action_name,
self._mode,
))
return
self._server.start()
self._alive = True
self._cuff_state = False
# 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] = baxter_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)
self._pub_rate.publish(self._control_rate)
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
self.stop=True
self._gripper_control = baxter_interface.Gripper(self._control_limb, CHECK_VERSION)
self._gripper_puppet = baxter_interface.Gripper(self._puppet_limb, CHECK_VERSION)
if self._control_limb == 'left':
self._io_control_lower = baxter_interface.DigitalIO('left_lower_button')
self._io_control_upper = baxter_interface.DigitalIO('left_upper_button')
else:
self._io_control_lower = baxter_interface.DigitalIO('right_lower_button')
self._io_control_upper = baxter_interface.DigitalIO('right_upper_button')
def __init__(self, limb='left'):
self._rp = rospkg.RosPack()
self._config_path = self._rp.get_path('baxter_mill') + '/config/'
self._limb = limb
self._baxter_limb = baxter_interface.Limb(self._limb)
self._neutral_pos = {}
self._picking_pos = {}
self._neutral_bool = False
self._picking_bool = True
self._mill_pos = {}
self._picking_pos = {}
self.br_pos = {}
self._the_pose = Pose()
self._should_io = baxter_interface.DigitalIO(self._limb +
'_shoulder_button')
self._dash_io = baxter_interface.DigitalIO(self._limb +
'_upper_button')
self._circle_io = baxter_interface.DigitalIO(self._limb +
'_lower_button')
ik_srv = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ik_srv, SolvePositionIK)
self._ikreq = SolvePositionIKRequest()
self._circle_io.state_changed.connect(self._default_points)
self.done_calibration = False
def __init__(self):
self.left_cuff_btn = baxter_interface.DigitalIO('left_lower_cuff')
self.left_dash_btn = baxter_interface.DigitalIO('left_upper_button')
self.left_circle_btn = baxter_interface.DigitalIO('left_lower_button')
self.right_cuff_btn = baxter_interface.DigitalIO('right_lower_cuff')
self.right_dash_btn = baxter_interface.DigitalIO('right_upper_button')
self.right_circle_btn = baxter_interface.DigitalIO(
'right_lower_button')
# DigitalIO('<limb_name>_upper_button' ) # 'dash' btn
# DigitalIO('<limb_name>_lower_button') # 'circle' btn
# DigitalIO('<limb_name>_lower_cuff') # cuff squeeze
#it pass the cuff sensor state to _cuff_cb
self.left_cuff_btn.state_changed.connect(self.left_cuff_callback)
#it pass the dash button state to _dash_cb
self.left_dash_btn.state_changed.connect(self.left_dash_callback)
self.left_circle_btn.state_changed.connect(self.left_circle_callback)
#it pass the cuff sensor state to _cuff_cb
self.right_cuff_btn.state_changed.connect(self.right_cuff_callback)
#it pass the dash button state to _dash_cb
self.right_dash_btn.state_changed.connect(self.right_dash_callback)
#it pass the circle button state to _circle_cb
self.right_circle_btn.state_changed.connect(self.right_circle_callback)
self.left_gripper = baxter_interface.Gripper('left', CHECK_VERSION)
self.right_gripper = baxter_interface.Gripper('right', CHECK_VERSION)
self.left_cuff_btn_state = False
self.left_dash_btn_state = False
self.left_dash_btn_value = False
self.left_circle_btn_state = False
self.right_cuff_btn_state = False
self.right_dash_btn_state = False
self.right_dash_btn_value = False
self.right_circle_btn_state = False
def __init__(self,
limb,
reconfig_server,
rate=100.0,
mode='position_w_id'):
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 = baxter_interface.Limb(limb)
self._enable = baxter_interface.RobotEnable()
self._name = limb
self._cuff = baxter_interface.DigitalIO('%s_lower_cuff' % (limb, ))
self._cuff.state_changed.connect(self._cuff_cb)
# 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
self._cuff_state = False
# 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] = baxter_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)
self._pub_ff_cmd = rospy.Publisher(self._ns +
'/inverse_dynamics_command',
JointTrajectoryPoint,
tcp_nodelay=True,
queue_size=1)
#our joint client to see the action
self._joint_client = actionlib.SimpleActionClient(
limb + 'ArmJointController', move_arm_jointsAction)
self._joint_client.wait_for_server()
self._target = None
#create a publisher for testing purposes
self._target_pub = rospy.Publisher('tarjectory_target',
JointState,
queue_size=10)
def __init__(self,
limb_name='left',
PID_gains=PID_gains_example,
rate=1000.0,
stiff_joints=stiff_joints_example):
rospy.loginfo("Initializing ZeroG node... ")
rospy.init_node("ZeroG")
self._stiff_joints = stiff_joints # {'joint_name':angle,...}
self.stiff_joints_names = stiff_joints.keys() # ['joint_name1',...]
self.stiff_joints_angles = stiff_joints.values() # [angle1,...]
self._gains = PID_gains # {'joint_name':{'kp':value,'ki':value,'kd':value},...}
self._missed_cmds = 20
self._ns = 'robot/limb/' + limb_name
self._limb = baxter_interface.Limb(limb_name)
self._joint_names = self._limb.joint_names()
self._torques = dict(
zip(self._joint_names, [0.0] * len(self._joint_names)))
self._loose_joints = []
self._enable = baxter_interface.RobotEnable()
self._name = limb_name
self._cuff = baxter_interface.DigitalIO('%s_lower_cuff' %
(limb_name, ))
self._dash = baxter_interface.DigitalIO('%s_upper_button' %
(limb_name, ))
self._circle = baxter_interface.DigitalIO('%s_lower_button' %
(limb_name, ))
# DigitalIO('%s_upper_button' % (limb,)) # 'dash' btn
# DigitalIO('%s_lower_button' % (limb,)) # 'circle' btn
# DigitalIO('%s_lower_cuff' % (limb,)) # cuff squeeze
self._cuff.state_changed.connect(
self._cuff_cb) #callback for cuff sensor
#it pass the cuff sensor state to _cuff_cb
self._dash.state_changed.connect(
self._dash_cb) #callback for dash button
#it pass the dash button state to _dash_cb
self._circle.state_changed.connect(
self._circle_cb) #callback for circle button
#it pass the circle button state to _circle_cb
self._cuff_state = False
self._dash_state = False
self._dash_value = False
self._circle_state = False
self._gripper = baxter_interface.Gripper('%s' % (limb_name, ),
CHECK_VERSION)
# Verify Grippers Have No Errors and are Calibrated
if self._gripper.error():
self._gripper.reset()
if (not self._gripper.calibrated()
and self._gripper.type() != 'custom'):
self._gripper.calibrate()
# Controller parameters
self._control_rate = rate # Hz
# for safety purposes, set the control rate command timeout.
# if the specified number of command cycles are missed, the robot
# will timeout and disable
self._limb.set_command_timeout(
(1.0 / self._control_rate) * self._missed_cmds)
# also for safety, sets the maximum speed
self._limb.set_joint_position_speed(
0.3) # it's a percentage of the arm speed
# 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)
print "ZeroG initialization completed!"
rospy.on_shutdown(self.clean_shutdown)
def __init__(self):
self._rp = rospkg.RosPack()
self.right_limb = 'right'
self.left_limb = 'left'
self._side = self.right_limb
self._limb = baxter_interface.Limb(self.right_limb)
self._left_limb = baxter_interface.Limb(self.left_limb)
self._path = self._rp.get_path('ribbon_cut') + '/config/'
self._images = (self._rp.get_path('ribbon_cut') +
'/share/images')
dash_io = baxter_interface.DigitalIO(self.right_limb + '_upper_button')
circle_io = baxter_interface.DigitalIO(self.right_limb + '_lower_button')
self._done = False
self._limbs = ('left', 'right')
self._arms = {
'left': baxter_interface.Limb('left'),
'right': baxter_interface.Limb('right'),
}
self._tuck_rate = rospy.Rate(20.0) # Hz
self._tuck_threshold = 0.2 # radians
self._peak_angle = -1.6 # radians
self._arm_state = {
'tuck': {'left': 'none', 'right': 'none'},
'collide': {'left': False, 'right': False},
'flipped': {'left': False, 'right': False}
}
self._joint_moves = {
'tuck': {
'left': [-1.0, -2.07, 3.0, 2.55, 0.0, 0.01, 0.0],
'right': [1.0, -2.07, -3.0, 2.55, -0.0, 0.01, 0.0]
},
'untuck': {
'left': [-0.08, -1.0, -1.19, 1.94, 0.67, 1.03, -0.50],
'right': [0.08, -1.0, 1.19, 1.94, -0.67, 1.03, 0.50]
}
}
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self.ribbon_location = dict()
self.cut_location = dict()
self.left_home_jp = dict()
self.right_home_jp = dict()
self.think_jp = dict()
self.shake_head_jp = dict()
self.shake_away_jp = dict()
self.safe_jp = dict()
self.ribbon_approach = dict()
self.cut_location = dict()
self.cut_approach = dict()
self.bridge = CvBridge()
self._gripper = baxter_interface.Gripper(self.right_limb, baxter_interface.CHECK_VERSION)
# connect callback fns to signals
if self._gripper.type() != 'custom':
if not (self._gripper.calibrated() or
self._gripper.calibrate() == True):
rospy.logwarn("%s (%s) calibration failed.",
self._gripper.name.capitalize(),
self._gripper.type())
else:
msg = (("%s (%s) not capable of gripper commands."
" Running cuff-light connection only.") %
(self._gripper.name.capitalize(), self._gripper.type()))
rospy.logwarn(msg)
ik_srv = "ExternalTools/" + self.right_limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ik_srv, SolvePositionIK)
self._ikreq = SolvePositionIKRequest()
circle_io.state_changed.connect(self._path_positions)
#----------------------------------------------------------------------------------
#----------------------------- Initialisation Functions----------------------------
#----------------------------------------------------------------------------------
def _check_calibration(self, value):
if self._gripper.calibrated():
return True
elif value == 'electric':
rospy.loginfo("calibrating %s...",
self._gripper.name.capitalize())
return (self._gripper.calibrate() == True)
else:
return False
def main():
#Start a node
rospy.init_node('sowrd_pulling_node')
rs = baxter_interface.RobotEnable(CHECK_VERSION)
rs.enable()
#Set up interface
left_gripper = baxter_interface.Gripper("left")
right_gripper = baxter_interface.Gripper("right")
left_button_thin = baxter_interface.DigitalIO('left_upper_button')
right_button_thin = baxter_interface.DigitalIO('right_upper_button')
left_button_circ = baxter_interface.DigitalIO('left_lower_button')
right_button_circ = baxter_interface.DigitalIO('right_lower_button')
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Put the arms aside
#left = np.array([[0.0,0.5,-0.6],[0.0,1.0,0.0,0.0]])
#right = np.array([[0.0,-0.5,-0.6],[0.0,1.0,0.0,0.0]])
#Position 1: Baoquan Action
left = np.array([[0.541,0.138,0.103],[0.479,-0.124,0.469,0.731]])
right = np.array([[0.598,-0.045,0.275],[-0.346,-0.059,-0.597,0.721]])
position(left,right,False)
rospy.sleep(1.0)
#Gripper start position
left_gripper.calibrated()
left_gripper.close()
right_gripper.calibrated()
right_gripper.open()
#Position 2: getting the sword
left = np.array([[0.947,0.26,-0.11],[0.13,0.74,-0.148,0.642]])
right = np.array([[0.0,-0.5,-0.6],[0.0,1.0,0.0,0.0]])
position(left,right)
rospy.sleep(1.0)
left_gripper.open()
while True:
#When thin button is pressed, move to position 3
if (left_button_thin.state==True):
#Position 3: start position of pulling a sword
left = np.array([[0.479,0.327,-0.211],[0.1635,0.8409,-0.04805,0.51366]])
right = np.array([[0.0,-0.5,-0.6],[0.0,1.0,0.0,0.0]])
position(left,right)
rospy.sleep(1.0)
#Catersian path plan 1: try to catch the sword with right arm
a = [0.552,0.014,-0.12]
b = [0.552,0.114,-0.12]
c = [0.552,0.214,-0.12]
o = [0.28048,-0.68855,-0.52949,-0.4085]
points = np.array([a,b,c])
quaternions = np.array([o,o,o])
execute_trajectory_from_points(points, quaternions)
#Switching the sword from left arm to right arm
right_gripper.close()
rospy.sleep(1.0)
left_gripper.open()
rospy.sleep(1.0)
#Catersian path plan 2: Pulling out the sword
a = [0.562,0.264,-0.12]
b = [0.562 + 0.01218693434*2,0.264,-0.12 + 0.09925461516*2]
c = [0.562 + 0.01218693434*4,0.264,-0.12 + 0.09925461516*4]
d = [0.562 + 0.01218693434*6,0.264,-0.12 + 0.09925461516*6]
o = [0.28048,-0.68855,-0.52949,-0.4085]
points = np.array([a,b,c,d])
quaternions = np.array([o,o,o,o])
execute_trajectory_from_points(points, quaternions)
#Position 4: Waving the sword
left = np.array([[0.0,0.5,-0.6],[0.0,1.0,0.0,0.0]])
right = np.array([[1.08,-0.33,0.437],[0.56151,0.01578,0.80809,-0.17733]])
position(left,right)
#Position 5: Getting to start position
left = np.array([[0.0,0.5,-0.6],[0.0,1.0,0.0,0.0]])
right = np.array([[0.5,-0.1,0.0],[0.5,0.5,0.5,-0.5]])
position(left,right)
break
else:
pass
def __init__(self, Baxter = True):
self.Baxter = Baxter
self.last = time.time()
if Baxter:
print 'hello '
rospy.init_node('BaxGUI')
self.pubLamp = rospy.Publisher('Lamp', String)
self.pubImage = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.Head = baxter_interface.Head()
self.headLocation = 0
self.Head.set_pan(self.headLocation)
self.Lnav = (baxter_interface.Navigator('left'))
self.Lnav.button0_changed.connect(self.scrollPush)
self.Lnav.wheel_changed.connect(self.scrolled)
self.Lnav.button1_changed.connect(self.backPush)
self.Lnav.button2_changed.connect(self.rethinkPush)
self.Rnav = (baxter_interface.Navigator('right'))
self.Rnav.button0_changed.connect(self.scrollPush)
#self.Rnav.wheel_changed.connect()
self.Rnav.button1_changed.connect(self.backPush)
self.Rnav.button2_changed.connect(self.rethinkPush)
self._close_io = baxter_interface.DigitalIO('%s_upper_button' % ('right',))
self._close_io.state_changed.connect(self.dashRightPush)
self.temp = baxter_interface.DigitalIO('%s_upper_button' % ('left',))
self.temp.state_changed.connect(self.dashLeftPush)
self.currentScreen = 'page2'
self.screenList = {}
home = Screen('home','/home/argun/BaxGUI Images/', 'bg.png')
home.addButton(10, 100, '', 'homeButton', ['screen', 'home'], 49, None)
home.addButton(10, 100, '', 'scrollUp', ['scrollup', 'options'], 2490368, None)
home.addButton(10, 150, '', 'scrollDown', ['scrolldown', 'options'], 2621440, None)
home.addButton(10, 200, '', 'select', ['select', 'options'], 13, None)
home.addOptionList(100, 100, 100, 30, 'options', ['lamp 1',
'lamp 2',
'lamp 3',
'lamp 4'],
[['screen', 'page1'],
['screen', 'page2'],
['screen', 'page3'],
['screen', 'page4']],
2, 13,
'imagewindow',
['1.png',
'2.png',
'3.png',
'4.png'])
home.addImage(500, 500, 'home.png', 'imagewindow')
self.addScreen(home)
page1 = Screen('page1','/home/argun/BaxGUI Images/', 'bg.png')
page1.addButton(10, 100, '', 'testButton', ['screen', 'home'], 49, None)
page1.addButton(500, 10, 'rotate head: wheel', 'headRotateLeft', ['head', 'left'], 2490368, imagePath = None)
page1.addButton(500, 10, 'rotate head: wheel', 'headRotateRight', ['head', 'right'], 2621440, imagePath = None)
self.addScreen(page1)
page2 = Screen('page2','/home/argun/BaxGUI Images/', 'bg.png')
page2.addButton(10, 100, '', 'testButton', ['screen', 'home'], 49, None)
page2.addButton(10, 100, '', 'rotateGripper', ['baxter', 'rotateLeftGripper'], 2490368, None)
page2.addButton(10, 100, '', 'rotateGripperR', ['baxter', 'rotateLeftGripperR'], 2621440, None)
page2.addButton(10, 100, '', 'recordNewPickup', ['baxter', 'recordNewPickup'], 13, None)
page2.addButton(10, 100, '', 'playBackPickup', ['baxter', 'playBackPickup'], 1113864, None)
page2.addButton(10, 100, '', 'toggleGripper', ['baxter', 'toggleLeftGripper'], 2, None)
self.addScreen(page2)
page3 = Screen('page3','/home/argun/BaxGUI Images/', 'bg.png')
page3.addButton(10, 100, '', 'testButton', ['screen', 'home'], 49, None)
self.addScreen(page3)
page4 = Screen('page4','/home/argun/BaxGUI Images/', 'bg.png')
page4.addButton(10, 100, '', 'testButton', ['screen', 'home'], 49, None)
self.addScreen(page4)
def __init__(self, savedRecord=None):
l_limb = "left"
r_limb = "right"
self._l_limb_name = l_limb
self._r_limb_name = r_limb
# Initialisierung der beiden Arme
self._l_limb = baxter_interface.Limb("left")
self._r_limb = baxter_interface.Limb("right")
# Liste zum speichern der waypoints
self._waypoints = [] # type: list[(str, dict)]
self._Angle = []# type: list[(str, dict)]
hover_distance = 0.15
self._hover_distance =hover_distance
verbose = True
self._verbose = verbose
self._is_recording = False
self._is_playing_back = False
# Pruefung ob robot enabled ist oder nicht
self._robot = baxter_interface.RobotEnable()
self._init_state = self._robot.state().enabled
if not self._init_state:
self._robot.enable()
# Initialisieren der Navigator Buttons --> zum lernen der Aktionen
self._l_navigator_io = baxter_interface.Navigator("left")
self._r_navigator_io = baxter_interface.Navigator("right")
# Initialisieren der Torso und Schulter Buttons
self._left_torso_navigator = baxter_interface.Navigator('torso_left')
self._right_torso_navigator = baxter_interface.Navigator('torso_right')
self._left_shoulder_button = baxter_interface.DigitalIO('left_shoulder_button')
self._right_shoulder_button = baxter_interface.DigitalIO('right_shoulder_button')
# Initialisieren der Gripper buttons
self._lelf_gripper_dash = baxter_interface.DigitalIO("left_upper_button")
self._right_gripper_dash = baxter_interface.DigitalIO("right_upper_button")
self._lelf_gripper_circle = baxter_interface.DigitalIO("left_lower_button")
self._right_gripper_circle = baxter_interface.DigitalIO("right_lower_button")
# Pruefvariablen
self._l_gripper_closed = False
self._r_gripper_closed = False
self._l_gripper = baxter_interface.Gripper("left", baxter_interface.CHECK_VERSION)
self._r_gripper = baxter_interface.Gripper("right", baxter_interface.CHECK_VERSION)
# Verbinden der Gripper buttons mit den entsprechenden Funktionen zum oeffnen und schliessen der gripper
self._lelf_gripper_dash.state_changed.connect(self._l_gripper_open)
self._right_gripper_dash.state_changed.connect(self._r_gripper_open)
self._lelf_gripper_circle.state_changed.connect(self._l_gripper_close_and_check)
self._right_gripper_circle.state_changed.connect(self._r_gripper_close_and_check)
# Verbinden der Torso buttons mit der entsprechenden Funktionen fahren in die neutrale Position
self._left_torso_navigator.button1_changed.connect(self._move_to_neutral_position)
self._right_torso_navigator.button1_changed.connect(self._move_to_neutral_position)
# kallibrieren der Gripper
self._calibrate_gripper(self._l_gripper)
self._calibrate_gripper(self._r_gripper)
def __init__(self,
arm_to_move='right',
savefoldername='data',
linear_duration=1.0,
nonzeroliststr='1',
min_vel=0.5,
max_vel=1.5,
rate=100,
reset_freq=20,
append_dt=True):
"""
Moves one/both arm in a piecewise linear trajectory. Each linear segment lasts for a fixed
amount of time during which the velocity command is held constant. Can move one, two or many joints at a single time
"""
self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
UInt16,
queue_size=10)
# Get the arms
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()
#### Limits
# Hard-code min and max angles
self._left_arm_min_angles = dict([('left_s0', -1.70168),
('left_s1', -2.147),
('left_e0', -3.05418),
('left_e1', -0.05),
('left_w0', -3.059),
('left_w1', -1.5708),
('left_w2', -3.059)])
self._left_arm_max_angles = dict([
('left_s0', 1.70168), ('left_s1', 1.047), ('left_e0', 3.05418),
('left_e1', 2.618), ('left_w0', 3.059), ('left_w1', 2.094),
('left_w2', 3.059)
])
self._right_arm_min_angles = dict([('right_s0', -1.70168),
('right_s1', -2.147),
('right_e0', -3.05418),
('right_e1', -0.05),
('right_w0', -3.059),
('right_w1', -1.5708),
('right_w2', -3.059)])
self._right_arm_max_angles = dict([('right_s0', 1.70168),
('right_s1', 1.047),
('right_e0', 3.05418),
('right_e1', 2.618),
('right_w0', 3.059),
('right_w1', 2.094),
('right_w2', 3.059)])
# Shrink limits by 0.5 radians on both min/max vals
for j in self._left_joint_names:
self._left_arm_min_angles[j] += 1.0
self._left_arm_max_angles[j] -= 1.0
for j in self._right_joint_names:
self._right_arm_min_angles[j] += 1.0
self._right_arm_max_angles[j] -= 1.0
# Passed in params
self._arm_to_move = arm_to_move # 'right', 'left' or 'both'
self._linear_duration = linear_duration
self._min_vel = min_vel
self._max_vel = max_vel
self._rate = rate # Hz
self._nonzeroliststr = nonzeroliststr
self._nonzerolist = [int(x) for x in nonzeroliststr.split(',')]
self._reset_freq = reset_freq # Reset to random pose once every this many motions
# Initialize velocities to zero
self._left_commanded_joint_velocities = dict([
(joint, 0.0) for i, joint in enumerate(self._left_joint_names)
])
self._right_commanded_joint_velocities = dict([
(joint, 0.0) for i, joint in enumerate(self._right_joint_names)
])
# Initialize robot
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 passed in rate (Default: 100 Hz)
self._pub_rate.publish(self._rate)
####### State recorder
# Get folder name
self._savefoldername = savefoldername
if append_dt:
self._savefoldername = savefoldername + time.strftime(
"_%Y_%m_%d_%H_%M_%S")
# Setup other vars
self._start_time = rospy.get_time()
# Get the gripper
self._gripper_left = baxter_interface.Gripper("left", CHECK_VERSION)
self._gripper_right = baxter_interface.Gripper("right", CHECK_VERSION)
self._io_left_lower = baxter_interface.DigitalIO('left_lower_button')
self._io_left_upper = baxter_interface.DigitalIO('left_upper_button')
self._io_right_lower = baxter_interface.DigitalIO('right_lower_button')
self._io_right_upper = baxter_interface.DigitalIO('right_upper_button')
# Verify Grippers Have No Errors and are Calibrated
if self._gripper_left.error():
self._gripper_left.reset()
if self._gripper_right.error():
self._gripper_right.reset()
if (not self._gripper_left.calibrated()
and self._gripper_left.type() != 'custom'):
self._gripper_left.calibrate()
if (not self._gripper_right.calibrated()
and self._gripper_right.type() != 'custom'):
self._gripper_right.calibrate()