def __init__(self):
# initialize GripperCommandClient
GripperCommandClient.__init__(self)
# initialize Moveit Scene
self.scene = PlanningSceneInterface("base_link")
self.scene2 = moveit_commander.PlanningSceneInterface()
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
# creat target pose publisher
self.marker_pub = rospy.Publisher("/TargetMarker",
MarkerArray,
queue_size=1)
# creat basket pos publisher
self.marker_pub2 = rospy.Publisher("/basket",
MarkerArray,
queue_size=1)
# creat publisher for requesting grasp pose
self.request_cloud_client = rospy.Publisher("/request_cloud",
Int32,
queue_size=1)
# instantiate a RobotCommander
self.robot = moveit_commander.RobotCommander()
# instantiate two MoveGroupCommander
self.group = moveit_commander.MoveGroupCommander("arm")
# basket's parameters
self.basket_pos = 'left'
self.basket_found = False
self.basket_pos_x = 0
self.basket_pos_y = 0
self.basket_search_t = 0
def tuck():
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
rospy.loginfo("Waiting for MoveIt...")
client = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("...connected")
# Padding does not work (especially for self collisions)
# So we are adding a box above the base of the robot
scene = PlanningSceneInterface("base_link")
scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = client.moveToJointPosition(joints,
pose,
0.0,
max_velocity_scaling_factor=0.5)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
move_thread.stop()
return
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface(
"manipulator", "base_link") #MoveGroupInterface のインスタンスの作成
planning_scene = PlanningSceneInterface(
"base_link") #PlanningSceneInterface のインスタンスの作成
joint_names = [
"shoulder_pan_joint",
"shoulder_lift_joint", #ジョイントの名前を定義
"elbow_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"
]
# pose = [-1.26 , -0.64 , -2.44 , -0.66 , 1.56 , 0.007]
# move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす
# move_group.get_move_action().wait_for_result() #wait result
# result = move_group.get_move_action().get_result() #result を代入
move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
self.state = {
'default': 0,
'hold': 1,
'way_standby': 2,
'plan_standby': 3
}
# default : 何もしていない target のセットか hold button を待つ
# hold : waypoint のセットを受け付ける hold button が離れたら終了
#way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合
#plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合
self.now_state = self.state['default'] #現在フレームの状態
def gotoInit(self):
move_group = MoveGroupInterface("arm", "base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
# Plans the joints in joint_names to angles in pose
init = self.trajActionGoal.trajectory.points[0].positions
move_group.moveToJointPosition(joint_names, init, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Moved to Init")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
grasp_topic = "fetch_grasp_planner_node/plan"
rospy.loginfo("Waiting for %s..." % grasp_topic)
self.grasp_planner_client = actionlib.SimpleActionClient(
grasp_topic, GraspPlanningAction)
wait = self.grasp_planner_client.wait_for_server(rospy.Duration(5))
if (wait):
print("successfully connected to grasp server")
else:
print("failed to connect to grasp server")
rospy.Subscriber("fetch_fruit_harvest/apple_pose", Pose,
self.apple_pose_callback)
self.pub = rospy.Publisher("fetch_fruit_harvest/grasp_msg",
String,
queue_size=10)
self.object = Object()
self.grasps = Grasp()
self.ready_for_grasp = False
self.pick_place_finished = False
self.first_time_grasp = True
self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0)
self.tuck()
def __init__(self):
self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link")
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.addCube("my_front_ground", 2, 1.4, 0.0, -0.48)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -0.6)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -0.6)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -0.6)
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
# 初始化需要使用move group控制的机械臂中的arm group
self.arm = MoveGroupCommander('arm')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
self.arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
self.arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
self.arm.set_planning_time(5)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def __init__(self, boundaries=False):
self.scene = PlanningSceneInterface("/base_link")
self.robot = RobotCommander()
self.group_commander = MoveGroupCommander("manipulator")
#self.gripper = MoveGroupCommander("gripper")
self.group_commander.set_end_effector_link('ee_link')
self.get_basic_infomation()
if boundaries is True:
self.add_bounds()
self.joint_state_subscriber = rospy.Subscriber("/joint_states",
JointState,
self.joint_callback)
self.joint_pubs = [
rospy.Publisher('/%s/command' % name, Float64, queue_size=1)
for name in CONTROLLER_NAMES
]
self.gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command',
Float64,
queue_size=1)
# create a DisplayTrajectory publisher which is used later to publish trajectories for RViz to visualize
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.reset_subscriber = rospy.Subscriber("/ur/reset", String,
self.reset)
rospy.sleep(2)
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface(
"base_link") #PlanningSceneInterface のインスタンスの作成
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground") #オブジェクトの削除
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.0, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.0,
-1.0) #座標に立方体を挿入
self.planning_scene.removeCollisionObject("demo_cube")
self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除
print dir(self.planning_scene) #インスタンス内のあらゆるオブジェクトの属性とメソッドのリストを表示
import inspect
print "addBox's variable is ", inspect.getargspec(
self.planning_scene.addBox)
print "attachBox's variable is ", inspect.getargspec(
self.planning_scene.attachBox)
print "addCube's variable is ", inspect.getargspec(
self.planning_scene.addCube)
print "setColor's variable is ", inspect.getargspec(
self.planning_scene.setColor) #python関数のパラメータの名前とデフォルト値を取得
def main():
rospy.init_node('part1_obstacles')
wait_for_time()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('floor')
planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2)
table_height = 0.767
table_width = 0.7
table_x = 0.95
planning_scene.addBox('table', table_width, 2, table_height, table_x, 0,
table_height / 2)
planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37 / 2)
microwave_height = 0.28
microwave_width = 0.48
# microwave_depth = 0.33
microwave_depth = 0.27
microwave_x = 0.97
microwave_z = 0.06
microwave_y = 0.18
planning_scene.addBox('microwave', microwave_depth, microwave_width,
microwave_height, microwave_x, microwave_y,
table_height + microwave_z + microwave_height / 2)
rospy.sleep(2)
rospy.spin()
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def __init__(self):
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def arm_move(self):
move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
#planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_left_ground", 1, 1.5,
self.translation[2][0], self.translation[2][1],
self.translation[2][2])
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TF joint names
joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
# Lists of joint angles in the same order as in joint_names
disco_poses = [[0, 2.5, -0.1, 3.0, 1.5, 3.0, 1.0, 3.0]]
for pose in disco_poses:
if rospy.is_shutdown():
break
# Plans the joints in joint_names to angles in pose
move_group.moveToJointPosition(joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Disco!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
move_group.get_move_action().cancel_all_goals()
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def main(args):
rospy.init_node('simple_move')
move_group = MoveGroupInterface('manipulator', "base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = [
'shoulder_1_joint', 'shoulder_2_joint', 'elbow_1_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
# This is the wrist link not the gripper itself
gripper_frame = "wrist_3_link"
poses, pose_name = get_pose(args.reset)
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = "base_link"
while not rospy.is_shutdown():
for pose in poses:
# Finish building the Pose_stamped message
# If the message stampe is not current it could be ignored
gripper_pose_stamped.header.stamp = rospy.Time.now()
# Set the message pose
gripper_pose_stamped.pose = pose
move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
log_output = pose_name + " Done!"
rospy.loginfo(log_output)
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
move_group.get_move_action().cancel_all_goals()
def main():
rospy.init_node('a5_obstacles')
wait_for_time()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeCollisionObject('top_shelf')
planning_scene.removeCollisionObject('bottom_shelf')
planning_scene.addBox('top_shelf', 0.32, 1, 0.4, 0.99, 0, 1.64)
planning_scene.addBox('bottom_shelf', 0.5, 1, 1.09, 0.9, 0, 0.545)
rospy.sleep(2)
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
##self.objectname_sub= rospy.Subscriber("/objects",Float32MultiArray,self.callback)
##self.interestedObjectName = ""
#self.objects = []
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def set_init_pose(sefl):
#set init pose
move_group = MoveGroupInterface("manipulator","base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = ["shoulder_pan_joint", "shoulder_lift_joint",
"elbow_joint", "wrist_1_joint",
"wrist_2_joint", "wrist_3_joint"]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
move_group.get_move_action().cancel_all_goals()
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface("manipulator","base_link") #MoveGroupInterface のインスタンスの作成
planning_scene = PlanningSceneInterface("base_link") #PlanningSceneInterface のインスタンスの作成
joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", #ジョイントの名前を定義
"elbow_joint", "wrist_1_joint",
"wrist_2_joint", "wrist_3_joint"]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす
move_group.get_move_action().wait_for_result() #wait result
result = move_group.get_move_action().get_result() #result を代入
move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
def main():
rospy.init_node('a5_obstacles')
wait_for_time()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('floor')
planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2)
planning_scene.addBox('table', 0.5, 1, 0.72, 1, 0, 0.72 / 2)
rospy.sleep(2)
def __init__(self, save_file_path="food_items.pkl", nav_file_path="annotator_positions.pkl"):
self._food_items_pub = rospy.Publisher(FOOD_ITEMS_TOPIC,
FoodItems, queue_size=10, latch=True)
rospy.loginfo("Given save file path: " + save_file_path)
if os.path.isfile(save_file_path):
rospy.loginfo("File already exists, loading saved positions.")
with open(save_file_path, "rb") as save_file:
try:
self._food_items = pickle.load(save_file)
except EOFError:
# this can be caused if the file is empty.
self._food_items = {}
rospy.loginfo("File loaded...")
else:
rospy.loginfo("File doesn't exist.")
self._food_items = {}
self.__print_food_items__()
self._save_file_path = save_file_path
self.__pub_food_items__()
rospy.loginfo("initializing arm...")
self.arm = robot_api.Arm()
rospy.loginfo("initializing gripper...")
self.gripper = robot_api.Gripper()
rospy.loginfo("initializing head...")
self.head = robot_api.Head()
rospy.loginfo("initializing torso...")
self.torso = robot_api.Torso()
rospy.loginfo("initializing planning scene...")
self.planning_scene = PlanningSceneInterface('base_link')
self.curr_obstacle = None
rospy.loginfo("Starting map annotator...")
# We should expect the nav file given to contain the annotated positions:
# MICROWAVE_LOCATION_NAME - starting location in front of the microwave.
# DROPOFF_LOCATION_NAME - ending dropoff location.
# TODO: Remember to uncomment this section when we get the map working.
# self._map_annotator = Annotator(save_file_path=nav_file_path)
# if not self._map_annotator.exists(self.MICROWAVE_LOCATION_NAME):
# rospy.logwarn("Annotator is missing location '%s'" %
# (self.MICROWAVE_LOCATION_NAME))
# if not self._map_annotator.exists(self.DROPOFF_LOCATION_NAME):
# rospy.logwarn("Annotator is missing location '%s'" %
# (self.DROPOFF_LOCATION_NAME))
rospy.loginfo("Initialization finished...")
def __init__(self):
self.moving_mode = False
self.plan_only = False
self.prev_joy_buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
self.joy_subscriber = rospy.Subscriber("/fetch_joy", Joy,
self.joy_callback)
self.joints_subscriber = rospy.Subscriber("/joint_states", JointState,
self.joints_states_callback)
self.rgbimage_subscriber = rospy.Subscriber("/head_camera/depth/image",
Image,
self.depthimage_callback)
self.depthimage_subscriber = rospy.Subscriber(
"/head_camera/depth/image", Image, self.rgbimage_callback)
self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_reset_pos",
Empty,
self.arm_reset_callback)
self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_start_pos",
Empty,
self.arm_start_callback)
self.arm_effort_pub = rospy.Publisher(
"/arm_controller/weightless_torque/command",
JointTrajectory,
queue_size=2)
self.gripper_client = actionlib.SimpleActionClient(
"gripper_controller/gripper_action", GripperCommandAction)
self.arm_cartesian_twist_pub = rospy.Publisher(
"/arm_controller/cartesian_twist/command", Twist, queue_size=2)
self.head_point_client = actionlib.SimpleActionClient(
"head_controller/point_head", PointHeadAction)
self.arm_move_group = MoveGroupInterface("arm",
"base_link",
plan_only=self.plan_only)
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def run(self):
move_thread = None
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
else:
rospy.loginfo("moveit already started")
rospy.loginfo("Waiting for MoveIt...")
self.client = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("...connected")
# Padding does not work (especially for self collisions)
# So we are adding a box above the base of the robot
scene = PlanningSceneInterface("base_link")
keepout_pose = Pose()
keepout_pose.position.z = 0.375
keepout_pose.orientation.w = 1.0
ground_pose = Pose()
ground_pose.position.z = -0.03
ground_pose.orientation.w = 1.0
rospack = rospkg.RosPack()
mesh_dir = os.path.join(rospack.get_path('fetch_teleop'), 'mesh')
scene.addMesh('keepout', keepout_pose,
os.path.join(mesh_dir, 'keepout.stl'))
scene.addMesh('ground', ground_pose,
os.path.join(mesh_dir, 'ground.stl'))
joints = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.client.moveToJointPosition(
joints, pose, 0.0, max_velocity_scaling_factor=0.5)
if result and result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
scene.removeCollisionObject("ground")
if move_thread:
move_thread.stop()
# On success quit
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("done")
sys.exit(0)
return
def __init__(self):
self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action",
GripperCommandAction)
rospy.loginfo("Waiting for gripper_controller...")
self.client.wait_for_server()
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("left_arm", "left_hand", verbose = True)
self.move_group = MoveGroupInterface("left_arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server(rospy.Duration(15.0))
self.cubes = []
self.tf_listener = TransformListener()
# self.gripper_client = actionlib.SimpleActionClient("/robot/end_effector/left_gripper/gripper_action", GripperCommandAction)
# self.gripper_client.wait_for_server()
# rospy.loginfo("...connected.")
rospy.sleep(2.0)
def clean_up(self, close=False):
if close:
# get the actual list after game
self.planning_scene_interface = PlanningSceneInterface(
REFERENCE_FRAME)
for name in self.planning_scene_interface.getKnownCollisionObjects():
if self.DEBUG is 1:
print("[DEBUG] Removing object %s" % name)
self.planning_scene_interface.removeCollisionObject(name, False)
for name in self.planning_scene_interface.getKnownAttachedObjects():
if self.DEBUG is 1:
print("[DEBUG] Removing attached object %s" % name)
self.planning_scene_interface.removeAttachedObject(name, False)
if close:
self.planning_scene_interface.waitForSync(5.0)
pass
def __init__(self, tower_array, disk_array, config, debug=0):
print "[INFO] Initialise World"
self.DEBUG = debug
# initialise arrays
self.tower_array = tower_array
self.disk_array = disk_array
# configs
self.max_gripper = config.get_max_gripper()
self.disk_height = config.disk_height
self.tower_positions = config.tower_positions
self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
def __init__(self):
self.planning_scene = PlanningSceneInterface("map")
self.tf_broacaster = tf.TransformBroadcaster()
self.tf_listener = tf.TransformListener()
self.pub = rospy.Subscriber('/gazebo/link_states',
LinkStates,
self.simulated_link_state_callback,
queue_size=1)
self.update_planning_scene = True
self.last_update = rospy.Time.now()
self.update_period = rospy.Duration(10)
self.table_collision = True
self.cube_collision = False
def __init__(self):
# Create move group interface for a fetch robot
self._move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
self._planning_scene = PlanningSceneInterface("base_link")
self._planning_scene.removeCollisionObject("my_front_ground")
self._planning_scene.removeCollisionObject("my_back_ground")
self._planning_scene.removeCollisionObject("my_right_ground")
self._planning_scene.removeCollisionObject("my_left_ground")
self._planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self._planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self._planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self._planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def setBoundaries():
'''
This is a fix for the FETCH colliding with itself
Define ground plane
This creates objects in the planning scene that mimic the ground
If these were not in place gripper could hit the ground
'''
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
