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
