def __init__(self, name):
# wait for moveit
while not "/move_group/result" in dict(
rospy.get_published_topics()).keys():
rospy.sleep(2)
self.group = MoveGroupCommander("arm")
self.group.set_start_state_to_current_state()
self.group.set_planner_id("RRTConnectkConfigDefault")
self.group.set_pose_reference_frame("/base_footprint")
self.group.set_max_velocity_scaling_factor(1)
self.group.set_num_planning_attempts(50)
self.group.set_planning_time(10)
self.group.set_goal_position_tolerance(0.01)
self.group.set_goal_orientation_tolerance(0.02)
self.tf_listener = TransformListener()
self._action_name = name
self._as = actionlib.SimpleActionServer(
self._action_name,
elevator.msg.SimpleTargetAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
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)
#!/usr/bin/env python3
import rospy
from std_msgs.msg import String
from moveit_commander.move_group import MoveGroupCommander
from geometry_msgs.msg import Pose
from math import radians, cos, sin
import tf_conversions as transform
rospy.init_node("test_sub")
commander = MoveGroupCommander("r_arm")
print(commander.get_current_pose().pose)
cp = Pose()
cp.position.x = 0.000
cp.position.y = -0.006
cp.position.z = 0.273
cp.orientation.x = 0.7366
cp.orientation.y = 0.0067
cp.orientation.z = 0.0066
cp.orientation.w = 0.6762
#print(cp)
#commander.set_orientation_target(cp.orientation)
commander.set_joint_value_target(cp, True)
#commander.set_pose_target(cp) #envoie la pose au commander
plan = commander.go(wait=True) #éxécute le mouvement avec attente
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)
#reset the octomap
try:
reset_client = rospy.ServiceProxy('/tabletop_octomap_server/reset', Empty)
reset_client()
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)
