def __init__(self):
self.LEFT = 1
self.RIGHT = 0
self.arm = self.RIGHT # default is right arm
self.use_slip_detection = 0
self.tf_listener = tf.TransformListener()
self.cms = [
controller_manager.ControllerManager('r', self.tf_listener,
self.use_slip_detection),
controller_manager.ControllerManager('l', self.tf_listener,
self.use_slip_detection)
]
self.left_actions = []
self.right_actions = []
def __init__(self):
self.LEFT = 1
self.RIGHT = 0
self.use_slip_detection = 0
self.tf_listener = tf.TransformListener()
self.cms = controller_manager.ControllerManager('l',self.tf_listener, self.use_slip_detection)
rospy.Subscriber('/pressure/l_gripper_motor',PressureState, self.state_callback)
#rospy.Subscriber('/pressure/l_gripper_motor_info',PressureInfo, self.info_callback)
self.display_state = -1
self.init_values = [[[0.0]*22],[[0.0]*22]]
self.cur_values = [[[0.0]*22],[[0.0]*22]]
self.initialized = False
self.left_hit = False
self.right_hit = False
def __init__(self):
self.LEFT = 1
self.RIGHT = 0
self.arm_above_and_to_side_angles = [[
-0.968, 0.729, -0.554, -1.891, -1.786, -1.127, 0.501
], [0.968, 0.729, 0.554, -1.891, 1.786, -1.127, 0.501]]
self.arm_to_side_angles = [[
-2.135, 0.803, -1.732, -1.905, -2.369, -1.680, 1.398
], [2.135, 0.803, 1.732, -1.905, 2.369, -1.680, 1.398]]
self.traj_from_above_to_side = lambda whicharm: [
self.arm_above_and_to_side_angles[whicharm], self.
arm_to_side_angles[whicharm]
]
self.use_slip_detection = 0
self.tf_listener = tf.TransformListener()
self.cms = [
controller_manager.ControllerManager('r', self.tf_listener,
self.use_slip_detection),
controller_manager.ControllerManager('l', self.tf_listener,
self.use_slip_detection)
]
def __init__(self,
arm,
pr2,
tf_listener,
using_slip_controller=1,
using_slip_detection=1):
if tf_listener == None:
self.tf_listener = tf.TransformListener()
else:
self.tf_listener = tf_listener
self.pr2 = pr2
if arm == 'l':
self.ik_frame = 'l_wrist_roll_link'
self.tool_frame = 'l_gripper_tool_frame'
self.arm_obj = self.pr2.left
ptopic = '/pressure/l_gripper_motor'
else:
self.ik_frame = 'r_wrist_roll_link'
self.tool_frame = 'r_gripper_tool_frame'
self.arm_obj = self.pr2.right
ptopic = '/pressure/r_gripper_motor'
self.pressure_listener = pm.PressureListener(ptopic, 5000)
print 'INITIALIZING CONTROLLER MANAGER'
self.cman = con.ControllerManager(arm, self.tf_listener,
using_slip_controller,
using_slip_detection)
print 'INITIALIZAING REACTIVE GRASPER'
self.reactive_gr = rgr.ReactiveGrasper(self.cman)
#self.collision_monitor = cmon.CollisionClient(arm)
#cpy from kaijen code
#gripper_event_detector_action_name = arm+'_gripper_sensor_controller/event_detector'
#self.gripper_event_detector_action_client = actionlib.SimpleActionClient(gripper_event_detector_action_name, \
# PR2GripperEventDetectorAction)
self.movement_mode = 'cart' #or cart
#self.cman.start_joint_controllers()
#self.cman.start_gripper_controller()
self.timeout = 50.
def __init__(self, which_arm): #which_arm is 'r' or 'l'
self.which_arm = which_arm
self._node_name = which_arm + '_reactive_grasp'
if which_arm == 'r':
self._action_name = 'reactive_grasp/right'
self._lift_action_name = 'reactive_lift/right'
self._approach_action_name = 'reactive_approach/right'
self._place_action_name = 'reactive_place/right'
else:
self._action_name = 'reactive_grasp/left'
self._lift_action_name = 'reactive_lift/left'
self._approach_action_name = 'reactive_approach/left'
self._place_action_name = 'reactive_place/left'
#params for reactive approach/grasp
self.side_step = rospy.get_param("~side_step", .015)
self.back_step = rospy.get_param("~back_step", .03)
self.approach_num_tries = rospy.get_param("~approach_num_tries", 5)
self.goal_pos_thres = rospy.get_param("~goal_pos_thres", .01)
#params for reactive grasp/grasp adjustment
self.min_contact_row = rospy.get_param("~min_contact_row", 1)
self.min_gripper_opening = rospy.get_param("~min_gripper_opening",
.0021)
self.max_gripper_opening = rospy.get_param("~max_gripper_opening", .1)
self.grasp_adjust_x_step = rospy.get_param("~grasp_adjust_x_step", .02)
self.grasp_adjust_z_step = rospy.get_param("~grasp_adjust_z_step",
.015)
self.grasp_adjust_num_tries = rospy.get_param(
"~grasp_adjust_num_tries", 3)
#params for reactive grasp
self.grasp_num_tries = rospy.get_param("~grasp_num_tries", 2)
self.forward_step = rospy.get_param("~forward_step",
0) #select based on top/side
self.close_force = rospy.get_param("~close_force", 100)
self.use_slip_controller = rospy.get_param("~use_slip_controller", 0)
self.use_slip_detection = rospy.get_param("~use_slip_detection", 0)
#params for reactive place
self.place_overshoot = rospy.get_param("~place_overshoot", .01)
self.place_angle_max_diff = rospy.get_param("~place_angle_max_diff",
math.pi / 10.)
#params for reactive lift
self.shake_object = rospy.get_param("~shake_object", 0)
rospy.loginfo("reactive_grasp_server: using_slip_controller:" +
str(self.use_slip_controller))
rospy.loginfo("reactive_grasp_server: using_slip_detection:" +
str(self.use_slip_detection))
#param for recording in grasp_playpen
self.pause_for_recording = rospy.get_param("~pause_for_recording", 0)
if self.pause_for_recording:
rospy.loginfo("pausing for recording is turned on")
#start action servers for reactive grasp, lift, and approach
self._as = actionlib.SimpleActionServer(self._action_name, ReactiveGraspAction, \
execute_cb=self.reactive_grasp_cb)
self._lift_as = actionlib.SimpleActionServer(self._lift_action_name, ReactiveLiftAction, \
execute_cb=self.reactive_lift_cb)
self._approach_as = actionlib.SimpleActionServer(self._approach_action_name, ReactiveGraspAction, \
execute_cb = self.reactive_approach_cb)
self._place_as = actionlib.SimpleActionServer(self._place_action_name, ReactivePlaceAction, \
execute_cb = self.reactive_place_cb)
#advertise services for compliant grasp and grasp adjustment
rospy.Service(self._node_name+'/compliant_close', \
Empty, self.compliant_close_callback)
rospy.Service(self._node_name+'/grasp_adjustment', \
Empty, self.grasp_adjustment_callback)
#initialize controler manager and reactive grasper
self.cm = controller_manager.ControllerManager(which_arm, \
using_slip_controller = self.use_slip_controller, \
using_slip_detection = self.use_slip_detection)
self.rg = ReactiveGrasperWithPreempt(self.cm, self._as, self._lift_as,
self._approach_as, self._place_as)
#shove the flag into the reactive grasper
self.rg.pause_for_recording = self.pause_for_recording
rospy.loginfo("reactive grasp action found")
rospy.loginfo("waiting for reactive approach action")
ra_ac.wait_for_server()
rospy.loginfo("reactive approach action found")
rospy.loginfo("waiting for reactive lift action")
rl_ac.wait_for_server()
rospy.loginfo("reactive lift action found")
rospy.loginfo("waiting for reactive place action")
rp_ac.wait_for_server()
rospy.loginfo("reactive place action found")
cm = controller_manager.ControllerManager('r', \
using_slip_controller = use_slip_controller, \
using_slip_detection = use_slip_detection)
#joint names for the right arm
joint_names = [
"r_shoulder_pan_joint", "r_shoulder_lift_joint",
"r_upper_arm_roll_joint", "r_elbow_flex_joint", "r_forearm_roll_joint",
"r_wrist_flex_joint", "r_wrist_roll_joint"
]
#reactive approach from above
#table_height = .55 #simulated table
table_height = .7239 #round table
tip_dist_to_table = .165
wrist_height = table_height + tip_dist_to_table + .02
def __init__(self):
self.GD = controller_manager.ControllerManager(
'l', using_slip_controller=False, using_slip_detection=False)