def main(): rospy.init_node("apNet_demo", anonymous=True) gripper_controller = GripperController() ur5_controller = UR5Controller() # init pose gripper_controller.open() ur5_controller.home() print('Starting APCNet Demo') vel_controller = APCNetDemo() while not rospy.is_shutdown(): try: if vel_controller.state == 'home': arm_is_home = ur5_controller.home() rospy.sleep(1) if arm_is_home: print('home --> reach') gripper_controller.open() vel_controller.state = 'reach' elif vel_controller.state == 'reach': vel_controller.joint_command_pub.publish(vel_controller.joint_traj) rospy.sleep(0.008) # 125 Hz elif vel_controller.state == 'grasp': gripper_controller.close() vel_controller.state = 'place' print('grasp --> place') rospy.sleep(1) elif vel_controller.state == 'place': is_arrived = ur5_controller.move2rest_pose() # rospy.sleep(1) if is_arrived: print('place --> drop') gripper_controller.open() vel_controller.state = 'home' print('drop --> home') rospy.sleep(0.5) except KeyboardInterrupt: print "Pressed \'Ctrl + c\', Shutting down ..." sys.exit()
def __init__(self, event_detector=False, tf_listener=None): if tf_listener == None: self.tf_listener = tf.TransformListener() else: self.tf_listener = tf_listener self.arms = ['r', 'l'] self.arm_names = {'l': 'left_arm', 'r': 'right_arm'} self.cart_client = {} self.joint_client = {} self.controllers = {} self.using_joint_controller = {} self.jl = joint_listener() self.cartl = cartesian_pose_listener(tf_listener) self.gripper = GripperController(event_detector) self.tool_ik = tool_frame_ik(tf_listener) self.control= { 'l': ee_cart_imped_action.EECartImpedClient("left_arm"), 'r': ee_cart_imped_action.EECartImpedClient("right_arm") } self.pr2_controller_manager = Pr2ControllerManager() for arm in self.arms: self.pr2_controller_manager.start_controller(arm, "joint") self.using_joint_controller[arm] = False controller_names= [ "_shoulder_pan_joint", "_shoulder_lift_joint", "_upper_arm_roll_joint", "_elbow_flex_joint", "_forearm_roll_joint", "_wrist_flex_joint", "_wrist_roll_joint"] for arm in self.arms: self.cart_client[arm] = actionlib.SimpleActionClient(\ arm+"_arm_ik", ArmMoveIKAction) self.cart_client[arm]=\ actionlib.SimpleActionClient\ ('/move_'+self.arm_name,\ arm_navigation_msgs.msg.MoveArmAction) rospy.loginfo('Waiting for move arm server') self.move_arm_control.wait_for_server() self.joint_client[arm] = actionlib.SimpleActionClient(\ arm+"_arm_controller/joint_trajectory_action", \ JointTrajectoryAction) self.controllers[arm] = [arm +name for name in controller_names] rospy.sleep(1)
def __init__(self, event_detector=False, tf_listener=None): if tf_listener == None: self.tf_listener = tf.TransformListener() else: self.tf_listener = tf_listener self.arms = ['r', 'l'] self.cart_client = {} self.joint_client = {} self.controllers = {} self.using_joint_controller = {} self.jl = joint_listener() self.cartl = cartesian_pose_listener(tf_listener) self.gripper = GripperController(event_detector) self.tool_ik = tool_frame_ik(tf_listener) self.control= { 'l': ee_cart_imped_action.EECartImpedClient("left_arm"), 'r': ee_cart_imped_action.EECartImpedClient("right_arm") } self.tc = torso_controller() self.pr2_controller_manager = Pr2ControllerManager() controller_names= [ "_shoulder_pan_joint", "_shoulder_lift_joint", "_upper_arm_roll_joint", "_elbow_flex_joint", "_forearm_roll_joint", "_wrist_flex_joint", "_wrist_roll_joint"] self.controller_names= {} for arm in self.arms: self.joint_client[arm] = actionlib.SimpleActionClient( '/%s_arm_controller/joint_trajectory_action' % arm, JointTrajectoryAction) self.joint_client[arm].wait_for_server() self.pr2_controller_manager.start_controller(arm, "cartesian") self.using_joint_controller[arm] = False self.controller_names[arm] = [ arm + x for x in controller_names] for arm in self.arms: self.cart_client[arm] = actionlib.SimpleActionClient(\ arm+"_arm_ik", ArmMoveIKAction)
class ArmController: def __init__(self, event_detector=False, tf_listener=None): if tf_listener == None: self.tf_listener = tf.TransformListener() else: self.tf_listener = tf_listener self.arms = ['r', 'l'] self.cart_client = {} self.joint_client = {} self.controllers = {} self.using_joint_controller = {} self.jl = joint_listener() self.cartl = cartesian_pose_listener(tf_listener) self.gripper = GripperController(event_detector) self.tool_ik = tool_frame_ik(tf_listener) self.control= { 'l': ee_cart_imped_action.EECartImpedClient("left_arm"), 'r': ee_cart_imped_action.EECartImpedClient("right_arm") } self.tc = torso_controller() self.pr2_controller_manager = Pr2ControllerManager() controller_names= [ "_shoulder_pan_joint", "_shoulder_lift_joint", "_upper_arm_roll_joint", "_elbow_flex_joint", "_forearm_roll_joint", "_wrist_flex_joint", "_wrist_roll_joint"] self.controller_names= {} for arm in self.arms: self.joint_client[arm] = actionlib.SimpleActionClient( '/%s_arm_controller/joint_trajectory_action' % arm, JointTrajectoryAction) self.joint_client[arm].wait_for_server() self.pr2_controller_manager.start_controller(arm, "cartesian") self.using_joint_controller[arm] = False self.controller_names[arm] = [ arm + x for x in controller_names] for arm in self.arms: self.cart_client[arm] = actionlib.SimpleActionClient(\ arm+"_arm_ik", ArmMoveIKAction) def command_torso(self, pose): self.tc.command_torso(pose) ## Gripper functions def open_gripper(self, whicharm): self.gripper.open(whicharm) def close_gripper(self, whicharm): self.gripper.close(whicharm) ## Current Arm State Functions def get_cartesian_pose(self, frame_id="base_link"): poses = {} for arm in self.arms: poses[arm] = self.cartl.return_cartesian_pose(arm, frame_id) return poses def get_joint_angles(self): current_joint_angles = self.jl.get_joint_angles() return current_joint_angles def is_moving(self, whicharm): velocities = self.jl.get_joint_velocities()[whicharm] abs_v = [abs(v) for v in velocities] total_v = sum(abs_v) print total_v return total_v > .3 ## Joint movement commands def joint_traj_movearm(self, whicharm, joint_angles_list, \ move_duration=5, blocking=True): if not self.using_joint_controller[whicharm]: self.pr2_controller_manager.start_controller(whicharm, "joint") self.using_joint_controller[whicharm] = True goal = JointTrajectoryGoal() goal.trajectory.joint_names = self.controllers[whicharm] for joint_angles in joint_angles_list: point = JointTrajectoryPoint() point.positions = joint_angles point.time_from_start=rospy.Duration(move_duration) goal.trajectory.points.append(point) self.joint_client[whicharm].send_goal(goal) if blocking: self.joint_client[whicharm].wait_for_result(rospy.Duration(move_duration)) return self.joint_client[whicharm].get_state() >= 3 ## Joint movement commands def joint_movearm(self, whicharm, joint_angles, \ move_duration=5, blocking=True): if not self.using_joint_controller[whicharm]: self.pr2_controller_manager.start_controller(whicharm, "joint") self.using_joint_controller[whicharm] = True goal = JointTrajectoryGoal() goal.trajectory.joint_names = self.controller_names[whicharm] point = JointTrajectoryPoint() point.positions = joint_angles point.time_from_start=rospy.Duration(move_duration) goal.trajectory.points.append(point) self.joint_client[whicharm].send_goal(goal) if blocking: self.joint_client[whicharm].wait_for_result(rospy.Duration(move_duration)) return self.joint_client[whicharm].get_state() >= 3 def joint_movearms(self, poses, move_duration=5, blocking=True): for arm in self.arms: self.joint_movearm(arm, poses[arm], move_duration, False) if blocking: result = True for arm in self.arms: self.joint_client[arm].wait_for_result() result &= self.joint_client[arm].get_state()>=3 return result def cart_freeze_arm(self, whicharm): curr = self.get_cartesian_pose() if not self.using_joint_controller[whicharm]: self.cancel_goal(whicharm) self.cart_movearm(whicharm, curr[whicharm], "base_link") def freeze_arm(self, whicharm): curr_angles = self.get_joint_angles()[whicharm] self.joint_movearm(whicharm, curr_angles) def cancel_goal(self,whicharm): self.control[whicharm].cancelGoal() self.control[whicharm].resetGoal() def near_current_pose(self, whicharm, pose): current_pose = self.get_cartesian_pose()[whicharm] #XXX return check_cartesian_near_pose(current_pose, pose, .2, .2, self.tf_listener) def cart_movearm(self,whicharm, poses, frame_id, blocking=False): self.control[whicharm].cancelGoal() self.control[whicharm].resetGoal() rospy.sleep(.1) if self.using_joint_controller[whicharm]: self.pr2_controller_manager.start_controller(whicharm, "cartesian") self.using_joint_controller[whicharm]= False for pose in poses: pose = list(pose) + [ frame_id] self.control[whicharm].addTrajectoryPoint(*pose) self.control[whicharm].sendGoal(wait=blocking) if blocking: self.control[whicharm].cancelGoal() self.control[whicharm].resetGoal() return def cart_movearm_old(self,whicharm, pose, frame_id, \ move_duration=5.0, ik_timeout=5.0, blocking=False): self.control[whicharm].cancelGoal() self.control[whicharm].resetGoal() if self.using_joint_controller[whicharm]: self.pr2_controller_manager.start_controller(whicharm, "cartesian") self.using_joint_controller[whicharm]= False pose = list(pose) + [500]*3 + [30]*3 + [False]*6 + [move_duration, frame_id] self.control[whicharm].addTrajectoryPoint(*pose) self.control[whicharm].sendGoal(wait=blocking) if blocking: self.control[whicharm].cancelGoal() self.control[whicharm].resetGoal() return self.near_current_pose(whicharm, pose) ## Cartesian Move arm commands # pose is a 7 element list def cart_movearms(self, poses, frame_id, \ move_duration=5.0, ik_timeout=5.0,blocking=True): for arm in self.arms: self.cart_movearm(arm, poses[arm], frame_id, move_duration,\ ik_timeout, False) if blocking: result = True for arm in self.arms: self.cart_client[arm].wait_for_result() #result &= self.cart_client[arm].get_state()==3 return result