def _initialize(self):
'''Connect up all services and action clients.
'''
self._world_interface = WorldInterface()
self._controller_manager = ControllerManagerClient()
if self._arm_name in ['right_arm', 'left_arm']:
self._group_name = self._arm_name
self._planner = ArmPlanner(self._arm_name)
self._hand_description = HandDescription(self._arm_name)
arm_abbr = self._arm_name[0]
self._joint_controller = '%s_arm_controller' % arm_abbr
self._cartesian_controller = '%s_cart' % arm_abbr
self._move_arm_client = actionlib.SimpleActionClient(
'move_%s' % self._arm_name,
arm_navigation_msgs.msg.MoveArmAction)
self._wait_for_action_server(self._move_arm_client)
#jt_action_name = '/%s_arm_controller/joint_trajectory_action' % arm_abbr
#self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, JointTrajectoryAction)
#self._wait_for_action_server(self._joint_trajectory_client)
jt_action_name = '/%s_arm_controller/follow_joint_trajectory' % arm_abbr
self._joint_trajectory_client = actionlib.SimpleActionClient(
jt_action_name, FollowJointTrajectoryAction)
self._wait_for_action_server(self._joint_trajectory_client)
self._cart_interface = CartesianControllerInterface(self._arm_name)
elif self._arm_name == 'both':
self._joint_controller = 'two_arm_controller'
#jt_two_arm_action_name = '/two_arm_controller/joint_trajectory_action'
#self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, JointTrajectoryAction)
jt_two_arm_action_name = '/two_arm_controller/follow_joint_trajectory'
self._joint_trajectory_client = actionlib.SimpleActionClient(
jt_two_arm_action_name, FollowJointTrajectoryAction)
self._wait_for_action_server(self._joint_trajectory_client)
else:
raise ValueError('Invalid arm name for worker: %s' %
self._arm_name)
#sleep(.04)
#except Exception:
#cmclient.switch_controllers(["l_arm_controller"], ["l_cart"])
#cmclient.switch_controllers(["r_arm_controller"], ["r_cart"])
#traceback.print_exc()
#exit(0)
# TODO: set saturation arguments for velocity and torque
# also reduce max effort arguments
rospy.init_node("knot_tying", disable_signals=True)
comm.initComm()
rope_getter = comm.LatestFileGetter("rope_model", "txt", comm.ArrayMessage)
library = rope_library.RopeTrajectoryLibrary(args.dbname, "write")
brett = pr2.PR2()
cmclient = ControllerManagerClient()
while True:
rope_k3 = rope_getter.recv_latest().data
draw_curve(rope_k3)
_, rars = library.get_closest_and_warp(rope_k3)
left_used = (rars["grab_l"] > -1).any()
right_used = (rars["grab_r"] > -1).any()
l_quat = rars["quat_l"]
l_pos = rars["xyz_l"]
r_quat = rars["quat_r"]
r_pos = rars["xyz_r"]
