def __init__(self, side='r', ctrl_mng=None, moveit_cmdr=None):
'''
Constructor
'''
if side == "right" or side == "r":
arm = "right_arm"
self.side = "r"
elif side == 'left' or side == 'l':
arm = "left_arm"
self.side = "l"
else:
rospy.logerr("Side " + side + " is not supported")
raise
#self.tf_listener = tf.TransformListener() if not tf_listener else tf_listener
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
self.ctrl_mng.switch_controllers([self.side + '_arm_controller'],
[self.side + '_cart'])
self.moveit_cmdr = MoveGroupCommander(
arm) if not moveit_cmdr else moveit_cmdr
self.gripper = Gripper(arm)
self.pose_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
self.eef_frame = self.side + "_wrist_roll_link"
self.reference_frame = "base_link"
self.approach_dist = 0.100
self.lift_dist = 0.110
self.step_size = 0.003
self.moveit_cmdr.set_pose_reference_frame(self.reference_frame)
def __init__(self, side='r', tf_listener = None, tf_broadcaster = None, ctrl_mng=None, moveit_cmdr=None):
if side == "right" or side == "r":
self.arm = "right_arm"
self.side = "r"
elif side == 'left' or side == 'l':
self.arm = "left_arm"
self.side = "l"
else:
rospy.logerr("Side " + side + " is not supported")
raise
self.tf_listener = tf.TransformListener() if not tf_listener else tf_listener
#init a TF transform broadcaster for the object frame
self.tf_broadcaster = tf.TransformBroadcaster() if not tf_broadcaster else tf_broadcaster
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
#the pretouch sensor frame
self.pretouch_sensor_frame_id = '/r_gripper_l_finger_tip_pretouch_frame'
#Gripper client and open the gripper
rospy.loginfo('open the ' + side + '_gripper')
self.gripper = Gripper(self.arm)
self.gripper.open()
#controller_magager_client
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
self.ctrl_mng.switch_controllers([self.side+'_arm_controller'], [self.side+'_cart'])
self.count = 0
#PoseStamped command publisher for jt controller
self.pose_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
self.eef_frame = self.side + "_wrist_roll_link"
self.reference_frame = "base_link"
# MoveIt! Commander
self.moveit_cmd = MoveGroupCommander(self.arm) if not moveit_cmdr else moveit_cmdr
self.moveit_cmd.set_pose_reference_frame(self.reference_frame)
self.move_arm_to_side() # Move the arm to the sidea
self.step_size = 0.0002
self.move_step_mat = np.matrix(translation_matrix(np.array([self.step_size, 0.0, 0.0])))
#pickup action server
#self.pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction)
#rospy.loginfo("waiting for PickupAction Server")
#self.pickup_client.wait_for_server()
#rospy.loginfo("PickupAction Server Connected")
#service client to /add_point service
self.add_point_client = rospy.ServiceProxy('add_point', AddPoint)
#draw_functions object for drawing stuff in rviz
self.draw_functions = draw_functions.DrawFunctions('pretouch_executor_markers')
#the transform from wrist_frame to the center of the fingers
self.gripper_to_r_finger = np.eye(4)
self.gripper_to_r_finger[0][3] = 0.1615 #x-translation from wrist_frame (0.1615)
self.gripper_to_r_finger[1][3] = -0.0400 #y-translation from wrist_frame (0.0425)
self.gripper_to_l_finger = np.eye(4)
self.gripper_to_l_finger[0][3] = 0.1615 #x-translation from wrist_frame (0.1615)
self.gripper_to_l_finger[1][3] = 0.0400 #y-translation from wrist_frame (-0.0425)
def __init__(self, side='r', ctrl_mng=None, moveit_cmdr=None):
'''
Constructor
'''
if side == "right" or side == "r":
arm = "right_arm"
self.side = "r"
elif side == 'left' or side == 'l':
arm = "left_arm"
self.side = "l"
else:
rospy.logerr("Side " + side + " is not supported")
raise
#self.tf_listener = tf.TransformListener() if not tf_listener else tf_listener
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
self.ctrl_mng.switch_controllers([self.side+'_arm_controller'], [self.side+'_cart'])
self.moveit_cmdr = MoveGroupCommander(arm) if not moveit_cmdr else moveit_cmdr
self.gripper = Gripper(arm)
self.pose_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
self.eef_frame = self.side + "_wrist_roll_link"
self.reference_frame = "base_link"
self.approach_dist = 0.100
self.lift_dist = 0.110
self.step_size = 0.003
self.moveit_cmdr.set_pose_reference_frame(self.reference_frame)
return (contact_point1, contact_point2, normal1, normal2, \\
uncertainty1, uncertainty2)
"""
# Reference frame
reference_frame_id = 'base_link'
#services
rospy.wait_for_service('tabletop_octomap')
rospy.wait_for_service('get_probabilistic_pointcloud')
rospy.wait_for_service('plan_point_cluster_grasp')
rospy.wait_for_service('set_point_cluster_grasp_params')
rospy.wait_for_service('evaluate_point_cluster_grasps')
rospy.wait_for_service('find_pretouch')
# Controller switcher
cm_client = ControllerManagerClient()
cm_client.switch_controllers(['r_arm_controller'], ['r_cart'])
# MoveIt! Commander
moveit_cmd = MoveGroupCommander("right_arm")
moveit_cmd.set_pose_reference_frame(reference_frame_id)
pose = Pose()
pose.position.x = 0.258
pose.position.y = -0.614
pose.position.z = 1.017
pose.orientation.w = 0.720
pose.orientation.x = 0.059
pose.orientation.y = 0.153
pose.orientation.z = 0.674
moveit_cmd.go(pose, wait=True)
class PretouchExecutor:
def __init__(self, side='r', tf_listener = None, tf_broadcaster = None, ctrl_mng=None, moveit_cmdr=None):
if side == "right" or side == "r":
self.arm = "right_arm"
self.side = "r"
elif side == 'left' or side == 'l':
self.arm = "left_arm"
self.side = "l"
else:
rospy.logerr("Side " + side + " is not supported")
raise
self.tf_listener = tf.TransformListener() if not tf_listener else tf_listener
#init a TF transform broadcaster for the object frame
self.tf_broadcaster = tf.TransformBroadcaster() if not tf_broadcaster else tf_broadcaster
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
#the pretouch sensor frame
self.pretouch_sensor_frame_id = '/r_gripper_l_finger_tip_pretouch_frame'
#Gripper client and open the gripper
rospy.loginfo('open the ' + side + '_gripper')
self.gripper = Gripper(self.arm)
self.gripper.open()
#controller_magager_client
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
self.ctrl_mng.switch_controllers([self.side+'_arm_controller'], [self.side+'_cart'])
self.count = 0
#PoseStamped command publisher for jt controller
self.pose_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
self.eef_frame = self.side + "_wrist_roll_link"
self.reference_frame = "base_link"
# MoveIt! Commander
self.moveit_cmd = MoveGroupCommander(self.arm) if not moveit_cmdr else moveit_cmdr
self.moveit_cmd.set_pose_reference_frame(self.reference_frame)
self.move_arm_to_side() # Move the arm to the sidea
self.step_size = 0.0002
self.move_step_mat = np.matrix(translation_matrix(np.array([self.step_size, 0.0, 0.0])))
#pickup action server
#self.pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction)
#rospy.loginfo("waiting for PickupAction Server")
#self.pickup_client.wait_for_server()
#rospy.loginfo("PickupAction Server Connected")
#service client to /add_point service
self.add_point_client = rospy.ServiceProxy('add_point', AddPoint)
#draw_functions object for drawing stuff in rviz
self.draw_functions = draw_functions.DrawFunctions('pretouch_executor_markers')
#the transform from wrist_frame to the center of the fingers
self.gripper_to_r_finger = np.eye(4)
self.gripper_to_r_finger[0][3] = 0.1615 #x-translation from wrist_frame (0.1615)
self.gripper_to_r_finger[1][3] = -0.0400 #y-translation from wrist_frame (0.0425)
self.gripper_to_l_finger = np.eye(4)
self.gripper_to_l_finger[0][3] = 0.1615 #x-translation from wrist_frame (0.1615)
self.gripper_to_l_finger[1][3] = 0.0400 #y-translation from wrist_frame (-0.0425)
def pretouch_callback(self, msg):
#self.count += 1
#print 'count = ', self.count
print 'frequency =', msg.frequency
print 'raw_freq =', msg.frequency_raw
#if self.count == 100:
#if msg.frequency < 8400 and self.count == 3: #detected
#monitor the probe time to stop probing to the air
time_limit = rospy.Duration(19.0)
time_past = rospy.Time.now() - self.start_time
print time_limit
print time_past
print time_past > time_limit
if time_past > time_limit:
self.detected = True
self.count = 0
rospy.loginfo("Nothing is here!!!")
rospy.loginfo("stop probe.....")
self.pretouch_sub.unregister()
self.ctrl_mng.switch_controllers([self.side+'_arm_controller'], [self.side+'_cart'])
print 'controller switched back to: ', self.ctrl_mng.list_controllers()
elif msg.frequency < 8650: #detected
self.detected = True
self.count = 0
rospy.loginfo("object detected by pretouch probe!!!")
rospy.loginfo("stop probe.....")
self.pretouch_sub.unregister()
self.ctrl_mng.switch_controllers([self.side+'_arm_controller'], [self.side+'_cart'])
print 'controller switched back to: ', self.ctrl_mng.list_controllers()
#draw the marker
self.draw_pretouch_marker()
#update free octomap cells
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = self.pretouch_sensor_frame_id
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose.position.x = 0
pose_stamped.pose.position.y = 0
pose_stamped.pose.position.z = 0
self.tf_listener.waitForTransform(pose_stamped.header.frame_id, 'base_link', pose_stamped.header.stamp, rospy.Duration(1.0))
print 'transform success...'
pose_stamped = self.tf_listener.transformPose('base_link', pose_stamped)
print pose_stamped
req = AddPointRequest()
req.pose_stamped = pose_stamped
req.decay_rate = 20.0 #default: 40.0
req.hit = True
resp = self.add_point_client(req)
else: #constant velocity motion
self.count += 1
print 'count= ', self.count
'''
msg = TwistStamped()
msg.header.frame_id = 'r_wrist_roll_link'
msg.header.stamp = rospy.Time.now()
msg.twist.linear.x = 0.003
#rospy.loginfo(msg)
self.twist_pub.publish(msg)
'''
# Move a step_size
#self.move_step_mat = np.matrix(translation_matrix(np.array([step_size, 0.0, 0.0])))
self.current_mat = self.current_mat * self.move_step_mat
self.pose_pub.publish(stamp_pose(mat_to_pose(self.current_mat), self.reference_frame))
#rospy.sleep(0.02)
if self.count > 10:
#update free octomap cells
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = self.pretouch_sensor_frame_id
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose.position.x = 0
pose_stamped.pose.position.y = 0
pose_stamped.pose.position.z = 0
self.tf_listener.waitForTransform(pose_stamped.header.frame_id, 'base_link', pose_stamped.header.stamp, rospy.Duration(1.0))
print 'transform success...'
pose_stamped = self.tf_listener.transformPose('base_link', pose_stamped)
print pose_stamped
req = AddPointRequest()
req.pose_stamped = pose_stamped
req.decay_rate = 40.0
req.hit = False
resp = self.add_point_client(req)
self.count = 0
def pretouch_probe(self):
#reset the self.detected flag
self.detected = False
self.start_time = rospy.Time.now()
#switch_controllers(start_controllers=[], stop_controllers=[])
rospy.loginfo("start probe.....")
self.ctrl_mng.switch_controllers([self.side+'_cart'], [self.side+'_arm_controller'])
print 'controller switched to: ', self.ctrl_mng.list_controllers()
'''
# The current pose
self.tf_listener.waitForTransform(self.eef_frame, self.reference_frame, pose_stamped.header.stamp, rospy.Duration(1.0))
pose = self.tf_listener.lookup
self.current_mat = pose_to_mat()
'''
# Start subscribing to pretouch sensor readings
self.pretouch_sub = rospy.Subscriber("/pretouch", PretouchAcoustic, self.pretouch_callback)
self.count = 0
#blocking
#wait until the object detected (self.detected)
while not self.detected:
rospy.sleep(0.1)
return True
def draw_pretouch_marker(self):
print 'drawing actual pretouch point......'
mat = np.matrix(np.identity(4))
self.draw_functions.draw_rviz_sphere(mat, 0.0035, frame = self.pretouch_sensor_frame_id, ns = 'actual_pretouch_points', id = 0, duration = 60., color = [1,0.4,0.7], opaque = 0.9, frame_locked = False)
def move_to_pose_goal(self, pose):
return self.moveit_cmd.go(pose, wait=True)
def move_to_joint_goal(self, positions):
self.moveit_cmd.go(joints = positions, wait=True)
#joint_position = [-0.817, 0.001, -1.253, -0.892, 60.854, -0.250, 3.338]
#joint_position=[0.19352680514353882, 1.122675976318801, -0.84954760489618752, -1.8022948875080473, 1.6990523534757012, -1.9945748402742813, 1.0094339881452736]
# get a joint state message already configured for this arm
js = self.arm_mover.get_joint_state(self.arm_name)
rospy.sleep(2.0)
# set desired joint positions
js.position = positions
print 'Moving to %s' % (str(positions))
# send out the command
self.moveit_cmd.go(joints = positions, wait=True)
handle = self.arm_mover.move_to_goal(self.arm_name, js)
if not handle.reached_goal():
print handle.get_errors()
return False
else:
return True
def move_arm_to_side(self):
pose = Pose()
pose.position.x = 0.258
pose.position.y = -0.614
pose.position.z = 1.017
pose.orientation.w = 0.720
pose.orientation.x = 0.059
pose.orientation.y = 0.153
pose.orientation.z = 0.674
return self.move_to_pose_goal(pose)
def __init__(self,
side='r',
tf_listener=None,
tf_broadcaster=None,
ctrl_mng=None,
moveit_cmdr=None):
if side == "right" or side == "r":
self.arm = "right_arm"
self.side = "r"
elif side == 'left' or side == 'l':
self.arm = "left_arm"
self.side = "l"
else:
rospy.logerr("Side " + side + " is not supported")
raise
self.tf_listener = tf.TransformListener(
) if not tf_listener else tf_listener
#init a TF transform broadcaster for the object frame
self.tf_broadcaster = tf.TransformBroadcaster(
) if not tf_broadcaster else tf_broadcaster
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
#the pretouch sensor frame
self.pretouch_sensor_frame_id = '/r_gripper_l_finger_tip_pretouch_frame'
#Gripper client and open the gripper
rospy.loginfo('open the ' + side + '_gripper')
self.gripper = Gripper(self.arm)
self.gripper.open()
#controller_magager_client
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
self.ctrl_mng.switch_controllers([self.side + '_arm_controller'],
[self.side + '_cart'])
self.count = 0
#PoseStamped command publisher for jt controller
self.pose_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
self.eef_frame = self.side + "_wrist_roll_link"
self.reference_frame = "base_link"
# MoveIt! Commander
self.moveit_cmd = MoveGroupCommander(
self.arm) if not moveit_cmdr else moveit_cmdr
self.moveit_cmd.set_pose_reference_frame(self.reference_frame)
self.move_arm_to_side() # Move the arm to the sidea
self.step_size = 0.0002
self.move_step_mat = np.matrix(
translation_matrix(np.array([self.step_size, 0.0, 0.0])))
#pickup action server
#self.pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction)
#rospy.loginfo("waiting for PickupAction Server")
#self.pickup_client.wait_for_server()
#rospy.loginfo("PickupAction Server Connected")
#service client to /add_point service
self.add_point_client = rospy.ServiceProxy('add_point', AddPoint)
#draw_functions object for drawing stuff in rviz
self.draw_functions = draw_functions.DrawFunctions(
'pretouch_executor_markers')
#the transform from wrist_frame to the center of the fingers
self.gripper_to_r_finger = np.eye(4)
self.gripper_to_r_finger[0][
3] = 0.1615 #x-translation from wrist_frame (0.1615)
self.gripper_to_r_finger[1][
3] = -0.0400 #y-translation from wrist_frame (0.0425)
self.gripper_to_l_finger = np.eye(4)
self.gripper_to_l_finger[0][
3] = 0.1615 #x-translation from wrist_frame (0.1615)
self.gripper_to_l_finger[1][
3] = 0.0400 #y-translation from wrist_frame (-0.0425)
class PretouchExecutor:
def __init__(self,
side='r',
tf_listener=None,
tf_broadcaster=None,
ctrl_mng=None,
moveit_cmdr=None):
if side == "right" or side == "r":
self.arm = "right_arm"
self.side = "r"
elif side == 'left' or side == 'l':
self.arm = "left_arm"
self.side = "l"
else:
rospy.logerr("Side " + side + " is not supported")
raise
self.tf_listener = tf.TransformListener(
) if not tf_listener else tf_listener
#init a TF transform broadcaster for the object frame
self.tf_broadcaster = tf.TransformBroadcaster(
) if not tf_broadcaster else tf_broadcaster
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
#the pretouch sensor frame
self.pretouch_sensor_frame_id = '/r_gripper_l_finger_tip_pretouch_frame'
#Gripper client and open the gripper
rospy.loginfo('open the ' + side + '_gripper')
self.gripper = Gripper(self.arm)
self.gripper.open()
#controller_magager_client
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
self.ctrl_mng.switch_controllers([self.side + '_arm_controller'],
[self.side + '_cart'])
self.count = 0
#PoseStamped command publisher for jt controller
self.pose_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
self.eef_frame = self.side + "_wrist_roll_link"
self.reference_frame = "base_link"
# MoveIt! Commander
self.moveit_cmd = MoveGroupCommander(
self.arm) if not moveit_cmdr else moveit_cmdr
self.moveit_cmd.set_pose_reference_frame(self.reference_frame)
self.move_arm_to_side() # Move the arm to the sidea
self.step_size = 0.0002
self.move_step_mat = np.matrix(
translation_matrix(np.array([self.step_size, 0.0, 0.0])))
#pickup action server
#self.pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction)
#rospy.loginfo("waiting for PickupAction Server")
#self.pickup_client.wait_for_server()
#rospy.loginfo("PickupAction Server Connected")
#service client to /add_point service
self.add_point_client = rospy.ServiceProxy('add_point', AddPoint)
#draw_functions object for drawing stuff in rviz
self.draw_functions = draw_functions.DrawFunctions(
'pretouch_executor_markers')
#the transform from wrist_frame to the center of the fingers
self.gripper_to_r_finger = np.eye(4)
self.gripper_to_r_finger[0][
3] = 0.1615 #x-translation from wrist_frame (0.1615)
self.gripper_to_r_finger[1][
3] = -0.0400 #y-translation from wrist_frame (0.0425)
self.gripper_to_l_finger = np.eye(4)
self.gripper_to_l_finger[0][
3] = 0.1615 #x-translation from wrist_frame (0.1615)
self.gripper_to_l_finger[1][
3] = 0.0400 #y-translation from wrist_frame (-0.0425)
def pretouch_callback(self, msg):
#self.count += 1
#print 'count = ', self.count
print 'frequency =', msg.frequency
print 'raw_freq =', msg.frequency_raw
#if self.count == 100:
#if msg.frequency < 8400 and self.count == 3: #detected
#monitor the probe time to stop probing to the air
time_limit = rospy.Duration(19.0)
time_past = rospy.Time.now() - self.start_time
print time_limit
print time_past
print time_past > time_limit
if time_past > time_limit:
self.detected = True
self.count = 0
rospy.loginfo("Nothing is here!!!")
rospy.loginfo("stop probe.....")
self.pretouch_sub.unregister()
self.ctrl_mng.switch_controllers([self.side + '_arm_controller'],
[self.side + '_cart'])
print 'controller switched back to: ', self.ctrl_mng.list_controllers(
)
elif msg.frequency < 8650: #detected
self.detected = True
self.count = 0
rospy.loginfo("object detected by pretouch probe!!!")
rospy.loginfo("stop probe.....")
self.pretouch_sub.unregister()
self.ctrl_mng.switch_controllers([self.side + '_arm_controller'],
[self.side + '_cart'])
print 'controller switched back to: ', self.ctrl_mng.list_controllers(
)
#draw the marker
self.draw_pretouch_marker()
#update free octomap cells
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = self.pretouch_sensor_frame_id
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose.position.x = 0
pose_stamped.pose.position.y = 0
pose_stamped.pose.position.z = 0
self.tf_listener.waitForTransform(pose_stamped.header.frame_id,
'base_link',
pose_stamped.header.stamp,
rospy.Duration(1.0))
print 'transform success...'
pose_stamped = self.tf_listener.transformPose(
'base_link', pose_stamped)
print pose_stamped
req = AddPointRequest()
req.pose_stamped = pose_stamped
req.decay_rate = 20.0 #default: 40.0
req.hit = True
resp = self.add_point_client(req)
else: #constant velocity motion
self.count += 1
print 'count= ', self.count
'''
msg = TwistStamped()
msg.header.frame_id = 'r_wrist_roll_link'
msg.header.stamp = rospy.Time.now()
msg.twist.linear.x = 0.003
#rospy.loginfo(msg)
self.twist_pub.publish(msg)
'''
# Move a step_size
#self.move_step_mat = np.matrix(translation_matrix(np.array([step_size, 0.0, 0.0])))
self.current_mat = self.current_mat * self.move_step_mat
self.pose_pub.publish(
stamp_pose(mat_to_pose(self.current_mat),
self.reference_frame))
#rospy.sleep(0.02)
if self.count > 10:
#update free octomap cells
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = self.pretouch_sensor_frame_id
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose.position.x = 0
pose_stamped.pose.position.y = 0
pose_stamped.pose.position.z = 0
self.tf_listener.waitForTransform(pose_stamped.header.frame_id,
'base_link',
pose_stamped.header.stamp,
rospy.Duration(1.0))
print 'transform success...'
pose_stamped = self.tf_listener.transformPose(
'base_link', pose_stamped)
print pose_stamped
req = AddPointRequest()
req.pose_stamped = pose_stamped
req.decay_rate = 40.0
req.hit = False
resp = self.add_point_client(req)
self.count = 0
def pretouch_probe(self):
#reset the self.detected flag
self.detected = False
self.start_time = rospy.Time.now()
#switch_controllers(start_controllers=[], stop_controllers=[])
rospy.loginfo("start probe.....")
self.ctrl_mng.switch_controllers([self.side + '_cart'],
[self.side + '_arm_controller'])
print 'controller switched to: ', self.ctrl_mng.list_controllers()
'''
# The current pose
self.tf_listener.waitForTransform(self.eef_frame, self.reference_frame, pose_stamped.header.stamp, rospy.Duration(1.0))
pose = self.tf_listener.lookup
self.current_mat = pose_to_mat()
'''
# Start subscribing to pretouch sensor readings
self.pretouch_sub = rospy.Subscriber("/pretouch", PretouchAcoustic,
self.pretouch_callback)
self.count = 0
#blocking
#wait until the object detected (self.detected)
while not self.detected:
rospy.sleep(0.1)
return True
def draw_pretouch_marker(self):
print 'drawing actual pretouch point......'
mat = np.matrix(np.identity(4))
self.draw_functions.draw_rviz_sphere(
mat,
0.0035,
frame=self.pretouch_sensor_frame_id,
ns='actual_pretouch_points',
id=0,
duration=60.,
color=[1, 0.4, 0.7],
opaque=0.9,
frame_locked=False)
def move_to_pose_goal(self, pose):
return self.moveit_cmd.go(pose, wait=True)
def move_to_joint_goal(self, positions):
self.moveit_cmd.go(joints=positions, wait=True)
#joint_position = [-0.817, 0.001, -1.253, -0.892, 60.854, -0.250, 3.338]
#joint_position=[0.19352680514353882, 1.122675976318801, -0.84954760489618752, -1.8022948875080473, 1.6990523534757012, -1.9945748402742813, 1.0094339881452736]
# get a joint state message already configured for this arm
js = self.arm_mover.get_joint_state(self.arm_name)
rospy.sleep(2.0)
# set desired joint positions
js.position = positions
print 'Moving to %s' % (str(positions))
# send out the command
self.moveit_cmd.go(joints=positions, wait=True)
handle = self.arm_mover.move_to_goal(self.arm_name, js)
if not handle.reached_goal():
print handle.get_errors()
return False
else:
return True
def move_arm_to_side(self):
pose = Pose()
pose.position.x = 0.258
pose.position.y = -0.614
pose.position.z = 1.017
pose.orientation.w = 0.720
pose.orientation.x = 0.059
pose.orientation.y = 0.153
pose.orientation.z = 0.674
return self.move_to_pose_goal(pose)
class GraspExecuter(object):
def __init__(self, side='r', ctrl_mng=None, moveit_cmdr=None):
'''
Constructor
'''
if side == "right" or side == "r":
arm = "right_arm"
self.side = "r"
elif side == 'left' or side == 'l':
arm = "left_arm"
self.side = "l"
else:
rospy.logerr("Side " + side + " is not supported")
raise
#self.tf_listener = tf.TransformListener() if not tf_listener else tf_listener
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
self.ctrl_mng.switch_controllers([self.side + '_arm_controller'],
[self.side + '_cart'])
self.moveit_cmdr = MoveGroupCommander(
arm) if not moveit_cmdr else moveit_cmdr
self.gripper = Gripper(arm)
self.pose_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
self.eef_frame = self.side + "_wrist_roll_link"
self.reference_frame = "base_link"
self.approach_dist = 0.100
self.lift_dist = 0.110
self.step_size = 0.003
self.moveit_cmdr.set_pose_reference_frame(self.reference_frame)
def pick(self, grasp):
'''
Given a planned grasp, pick up an object.
'''
# Open the gripper
self.gripper.open(max_effort=12)
# Compute pregrasp pose
grasp_pose = grasp.grasp_pose.pose
#pregrasp_mat = np.matrix(translation_matrix(np.array([-0.100, 0.0, 0.0]))) * pose_to_mat(grasp_pose)
pregrasp_mat = pose_to_mat(grasp_pose) * np.matrix(
translation_matrix(np.array([-0.100, 0.0, 0.0])))
pregrasp_pose = mat_to_pose(pregrasp_mat)
self.current_mat = pregrasp_mat
# Move to pregrasp pose
self.ctrl_mng.switch_controllers([self.side + '_arm_controller'],
[self.side + '_cart'])
pregrasp_pose_stamped = PoseStamped()
pregrasp_pose_stamped.pose = pregrasp_pose
pregrasp_pose_stamped.header.frame_id = grasp.grasp_pose.header.frame_id
pregrasp_pose_stamped.header.stamp = rospy.Time.now()
print "pregrasp_pose_stamped=", pregrasp_pose_stamped
result = self.moveit_cmdr.go(pregrasp_pose_stamped)
if result:
rospy.loginfo('Reached the Pre-Grasp Posture!')
else:
rospy.loginfo('failed to move to pre-grasp posture')
return False
# Approach
self.approach(self.approach_dist, self.step_size)
# Close gripper
self.gripper.close(max_effort=12)
rospy.loginfo('Closed the gripper')
rospy.sleep(1.0)
# Attach the CollisionObject
# Lift
self.lift(grasp, self.lift_dist, self.step_size)
return True
def approach(self, approach_dist, step_size):
self.ctrl_mng.switch_controllers([self.side + '_cart'],
[self.side + '_arm_controller'])
current_mat = self.current_mat
move_step_mat = np.matrix(
translation_matrix(np.array([step_size, 0.0, 0.0])))
for i in range(int(approach_dist / step_size)):
current_mat = current_mat * move_step_mat
self.pose_pub.publish(
stamp_pose(mat_to_pose(current_mat), self.reference_frame))
rospy.sleep(0.1)
self.ctrl_mng.switch_controllers([self.side + '_arm_controller'],
[self.side + '_cart'])
def lift(self, grasp, lift_dist, step_size):
self.ctrl_mng.switch_controllers([self.side + '_cart'],
[self.side + '_arm_controller'])
current_mat = pose_to_mat(grasp.grasp_pose.pose)
move_step_mat = np.matrix(
translation_matrix(np.array([0.0, 0.0, -step_size])))
for i in range(int(lift_dist / step_size)):
#current_mat = current_mat * move_step_mat
current_mat[2, 3] += step_size
self.pose_pub.publish(
stamp_pose(mat_to_pose(current_mat), self.reference_frame))
rospy.sleep(0.05)
self.ctrl_mng.switch_controllers([self.side + '_arm_controller'],
[self.side + '_cart'])
return (contact_point1, contact_point2, normal1, normal2, \\
uncertainty1, uncertainty2)
"""
# Reference frame
reference_frame_id = 'base_link'
#services
rospy.wait_for_service('tabletop_octomap')
rospy.wait_for_service('get_probabilistic_pointcloud')
rospy.wait_for_service('plan_point_cluster_grasp')
rospy.wait_for_service('set_point_cluster_grasp_params')
rospy.wait_for_service('evaluate_point_cluster_grasps')
rospy.wait_for_service('find_pretouch')
# Controller switcher
cm_client = ControllerManagerClient()
cm_client.switch_controllers(['r_arm_controller'], ['r_cart'])
# MoveIt! Commander
moveit_cmd = MoveGroupCommander("right_arm")
moveit_cmd.set_pose_reference_frame(reference_frame_id)
pose = Pose()
pose.position.x = 0.258
pose.position.y = -0.614
pose.position.z = 1.017
pose.orientation.w = 0.720
pose.orientation.x = 0.059
pose.orientation.y = 0.153
pose.orientation.z = 0.674
moveit_cmd.go(pose, wait=True)
class GraspExecuter(object):
def __init__(self, side='r', ctrl_mng=None, moveit_cmdr=None):
'''
Constructor
'''
if side == "right" or side == "r":
arm = "right_arm"
self.side = "r"
elif side == 'left' or side == 'l':
arm = "left_arm"
self.side = "l"
else:
rospy.logerr("Side " + side + " is not supported")
raise
#self.tf_listener = tf.TransformListener() if not tf_listener else tf_listener
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
self.ctrl_mng.switch_controllers([self.side+'_arm_controller'], [self.side+'_cart'])
self.moveit_cmdr = MoveGroupCommander(arm) if not moveit_cmdr else moveit_cmdr
self.gripper = Gripper(arm)
self.pose_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
self.eef_frame = self.side + "_wrist_roll_link"
self.reference_frame = "base_link"
self.approach_dist = 0.100
self.lift_dist = 0.110
self.step_size = 0.003
self.moveit_cmdr.set_pose_reference_frame(self.reference_frame)
def pick(self, grasp):
'''
Given a planned grasp, pick up an object.
'''
# Open the gripper
self.gripper.open(max_effort=12)
# Compute pregrasp pose
grasp_pose = grasp.grasp_pose.pose
#pregrasp_mat = np.matrix(translation_matrix(np.array([-0.100, 0.0, 0.0]))) * pose_to_mat(grasp_pose)
pregrasp_mat = pose_to_mat(grasp_pose) * np.matrix(translation_matrix(np.array([-0.100, 0.0, 0.0])))
pregrasp_pose = mat_to_pose(pregrasp_mat)
self.current_mat = pregrasp_mat
# Move to pregrasp pose
self.ctrl_mng.switch_controllers([self.side+'_arm_controller'], [self.side+'_cart'])
pregrasp_pose_stamped = PoseStamped()
pregrasp_pose_stamped.pose = pregrasp_pose
pregrasp_pose_stamped.header.frame_id = grasp.grasp_pose.header.frame_id
pregrasp_pose_stamped.header.stamp = rospy.Time.now()
print "pregrasp_pose_stamped=", pregrasp_pose_stamped
result = self.moveit_cmdr.go(pregrasp_pose_stamped)
if result:
rospy.loginfo('Reached the Pre-Grasp Posture!')
else:
rospy.loginfo('failed to move to pre-grasp posture')
return False
# Approach
self.approach(self.approach_dist, self.step_size)
# Close gripper
self.gripper.close(max_effort=12)
rospy.loginfo('Closed the gripper')
rospy.sleep(1.0)
# Attach the CollisionObject
# Lift
self.lift(grasp, self.lift_dist, self.step_size)
return True
def approach(self, approach_dist, step_size):
self.ctrl_mng.switch_controllers([self.side+'_cart'], [self.side+'_arm_controller'])
current_mat = self.current_mat
move_step_mat = np.matrix(translation_matrix(np.array([step_size, 0.0, 0.0])))
for i in range(int(approach_dist/step_size)):
current_mat = current_mat * move_step_mat
self.pose_pub.publish(stamp_pose(mat_to_pose(current_mat), self.reference_frame))
rospy.sleep(0.1)
self.ctrl_mng.switch_controllers([self.side+'_arm_controller'], [self.side+'_cart'])
def lift(self, grasp, lift_dist, step_size):
self.ctrl_mng.switch_controllers([self.side+'_cart'], [self.side+'_arm_controller'])
current_mat = pose_to_mat(grasp.grasp_pose.pose)
move_step_mat = np.matrix(translation_matrix(np.array([0.0, 0.0, -step_size])))
for i in range(int(lift_dist/step_size)):
#current_mat = current_mat * move_step_mat
current_mat[2,3] += step_size
self.pose_pub.publish(stamp_pose(mat_to_pose(current_mat), self.reference_frame))
rospy.sleep(0.05)
self.ctrl_mng.switch_controllers([self.side+'_arm_controller'], [self.side+'_cart'])
