def run(self):
move_thread = None
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
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")
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 = 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")
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 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 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 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 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()
class GraspingClient(object):
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 updateScene(self):
# find objects
rospy.loginfo("Add collision objects")
self.scene.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.removeCollisionObject("box_1")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.scene.addBox("box_1", 0.44, 0.43, 0.68, 0.7, -0.4, .34)
self.scene.waitForSync()
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def zero(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
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")
scene.addBox("keepout", 0.3, 0.5, 0.05, 0.12, 0.0, 0.375)
scene.addBox("ground", 1.5, 1.5, 0.05, 0.5, 0.0, 0.0)
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 main():
rospy.init_node('part2_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)
rospy.sleep(2)
rospy.spin()
class UR_Moveit_API:
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 get_basic_infomation(self):
# We can get the name of the reference frame for this robot:
planning_frame = self.group_commander.get_planning_frame()
print("============ Reference frame: %s" % planning_frame,
"============")
# We can also print the name of the end-effector link for this group:
eef_link = self.group_commander.get_end_effector_link()
print("============ End effector: %s" % eef_link, "============")
# We can get a list of all the groups in the robot:
group_names = self.robot.get_group_names()
print("============ Robot Groups:", group_names, "============")
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("============ Printing robot state")
print(self.robot.get_current_state(), "============")
print("================================================")
def joint_callback(self, joint_state):
self.joint_state = joint_state
def plan_cartesian_path(self, scale=1):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
group = self.group_commander
waypoints = []
wpose = group.get_current_pose().pose
wpose.position.z -= scale * 0.1 # First move up (z)
wpose.position.y += scale * 0.2 # and sideways (y)
waypoints.append(copy.deepcopy(wpose))
wpose.position.x += scale * 0.1 # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))
wpose.position.y -= scale * 0.1 # Third move sideways (y)
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
# Note: We are just planning, not asking move_group to actually move the robot yet:
return plan, fraction
def display_trajectory(self, plan):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
robot = self.robot
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory)
'''
def open_gripper(self, drop=False):
plan = self.gripper.plan(GRIPPER_DROP if drop else GRIPPER_OPEN)
return self.gripper.execute(plan, wait=True)
'''
def add_bounds(self):
print("Complete to add_bounds")
# size_x, size_y, size_z, x, y, z
self.scene.addBox('table', 1.0, 1.0, 1.0, .0, .0, -0.5)
# add a cube of 0.1m size, at [1, 0, 0.5] in the base_link frame
#self.scene.addCube("my_cube", 0.1, 1, 0, 0.5)
# Remove the cube
#self.scene.removeCollisionObject("my_cube")
def remove_bounds(self):
for obj in self.scene.get_objects().keys():
self.scene.remove_world_object(obj)
'''
def close_gripper(self):
plan = self.gripper.plan(GRIPPER_CLOSED)
return self.gripper.execute(plan, wait=True)
'''
def get_ik_client(self, request):
rospy.wait_for_service('/compute_ik')
inverse_ik = rospy.ServiceProxy('/compute_ik', GetPositionIK)
ret = inverse_ik(request)
if ret.error_code.val != 1:
return None
return ret.solution.joint_state
def get_fk_client(self, header, link_names, robot_state):
rospy.wait_for_service('/compute_fk')
fk = rospy.ServiceProxy('/compute_fk', GetPositionFK)
ret = fk(header, link_names, robot_state)
if ret.error_code.val != 1:
return None
return ret.pose_stamped
def orient_to_target(self, x=None, y=None, angle=None):
current = self.get_joint_values()
if x or y:
angle = np.arctan2(y, x)
if angle >= 3.1:
angle = 3.1
elif angle <= -3.1:
angle = -3.1
current[0] = angle
plan = self.group_commander.plan(current)
return self.group_commander.execute(plan, wait=True)
def wrist_rotate(self, angle):
rotated_values = self.group_commander.get_current_joint_values()
rotated_values[4] = angle - rotated_values[0]
if rotated_values[4] > np.pi / 2:
rotated_values[4] -= np.pi
elif rotated_values[4] < -(np.pi / 2):
rotated_values[4] += np.pi
plan = self.group_commander.plan(rotated_values)
return self.group_commander.execute(plan, wait=True)
def get_joint_values(self):
return self.group_commander.get_current_joint_values()
def get_current_pose(self):
return self.group_commander.get_current_pose()
def move_to_neutral(self):
print('Moving to neutral...')
plan = self.group_commander.plan(NEUTRAL_VALUES)
return self.group_commander.execute(plan, wait=True)
def move_to_home(self):
print('Moving to home...')
plan = self.group_commander.plan(HOME)
return self.group_commander.execute(plan, wait=True)
def move_to_up(self):
print('Moving to up...')
plan = self.group_commander.plan(UP)
return self.group_commander.execute(plan, wait=True)
def reset(self, data):
print("data: ", data)
self.move_to_neutral()
rospy.sleep(0.5)
def orient_to_pregrasp(self, x, y):
angle = np.arctan2(y, x)
return self.move_to_drop(angle)
def move_to_grasp(self, x, y, z, angle, compensate_control_noise=True):
if compensate_control_noise:
x = (x - CONTROL_NOISE_COEFFICIENT_BETA
) / CONTROL_NOISE_COEFFICIENT_ALPHA
y = (y - CONTROL_NOISE_COEFFICIENT_BETA
) / CONTROL_NOISE_COEFFICIENT_ALPHA
current_p = self.group_commander.get_current_pose().pose
p1 = Pose(position=Point(x=x, y=y, z=z), orientation=DOWN_ORIENTATION)
plan, f = self.group_commander.compute_cartesian_path([current_p, p1],
0.001, 0.0)
joint_goal = list(plan.joint_trajectory.points[-1].positions)
first_servo = joint_goal[0]
joint_goal[4] = (angle - first_servo) % np.pi
if joint_goal[4] > np.pi / 2:
joint_goal[4] -= np.pi
elif joint_goal[4] < -(np.pi / 2):
joint_goal[4] += np.pi
try:
plan = self.group_commander.plan(joint_goal)
except MoveItCommanderException as e:
print('Exception while planning')
traceback.print_exc(e)
return False
return self.group_commander.execute(plan, wait=True)
def move_to_vertical(self, z, force_orientation=True, shift_factor=1.0):
current_p = self.group_commander.get_current_pose().pose
current_angle = self.get_joint_values()[4]
orientation = current_p.orientation if force_orientation else None
p1 = Pose(position=Point(x=current_p.position.x * shift_factor,
y=current_p.position.y * shift_factor,
z=z),
orientation=orientation)
waypoints = [current_p, p1]
plan, f = self.group_commander.compute_cartesian_path(
waypoints, 0.001, 0.0)
if not force_orientation:
return self.group_commander.execute(plan, wait=True)
else:
if len(plan.joint_trajectory.points) > 0:
joint_goal = list(plan.joint_trajectory.points[-1].positions)
else:
return False
joint_goal[4] = current_angle
plan = self.group_commander.plan(joint_goal)
return self.group_commander.execute(plan, wait=True)
def move_to_target(self, target):
assert len(target) >= 6, 'Invalid target command'
for i, pos in enumerate(target):
self.joint_pubs[i].publish(pos)
def move_to_joint_position(self, joints):
"""
Adds the given joint values to the current joint values, moves to position
"""
joint_state = self.joint_state
joint_dict = dict(zip(joint_state.name, joint_state.position))
for i in range(len(JOINT_NAMES)):
joint_dict[JOINT_NAMES[i]] += joints[i]
joint_state = JointState()
joint_state.name = JOINT_NAMES
joint_goal = [joint_dict[joint] for joint in JOINT_NAMES]
joint_goal = np.clip(np.array(joint_goal), JOINT_MIN, JOINT_MAX)
joint_state.position = joint_goal
header = Header()
robot_state = RobotState()
robot_state.joint_state = joint_state
link_names = ['ee_link']
position = self.get_fk_client(header, link_names, robot_state)
target_p = position[0].pose.position
x, y, z = target_p.x, target_p.y, target_p.z
conditions = [
x <= BOUNDS_LEFTWALL, x >= BOUNDS_RIGHTWALL, y <= BOUNDS_BACKWALL,
y >= BOUNDS_FRONTWALL, z <= BOUNDS_FLOOR, z >= 0.15
]
print("Target Position: %0.4f, %0.4f, %0.4f" % (x, y, z))
for condition in conditions:
if not condition:
return
self.move_to_target(joint_goal)
rospy.sleep(0.15)
def scenario_plan(self):
plan = self.group_commander.plan(PREDISCARD_VALUES)
self.group_commander.execute(plan, wait=True)
plan = self.group_commander.plan(DISCARD_VALUES)
self.group_commander.execute(plan, wait=True)
#self.open_gripper(drop=True)
plan = self.group_commander.plan(PREDISCARD_VALUES)
self.group_commander.execute(plan, wait=True)
self.move_to_neutral()
def execute_plan(self, plan):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
group = self.group_commander
## Use execute if you would like the robot to follow
## the plan that has already been computed:
group.execute(plan, wait=True)
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
import argparse
import rospy
from moveit_python import PlanningSceneInterface
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Add a box to the MoveIt planning scene.")
parser.add_argument("name", help="Name of the box to add")
parser.add_argument("x", type=float, help="X coordinate of center of box")
parser.add_argument("y", type=float, help="Y coordinate of center of box")
parser.add_argument("z", type=float, help="Z coordinate of center of box")
parser.add_argument("size_x", type=float, help="Size of box in x dimension")
parser.add_argument("size_y", type=float, help="Size of box in y dimension")
parser.add_argument("size_z", type=float, help="Size of box in z dimension")
args = parser.parse_args()
if args.name:
rospy.init_node("add_box")
scene = PlanningSceneInterface("/base_link")
print("Adding Box with name: %s" % args.name)
scene.addBox(args.name, args.size_x, args.size_y, args.size_z,
args.x, args.y, args.z, use_service=True)
else:
parser.print_help()
class GraspingClient(object):
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 apple_pose_callback(self, message):
if self.pick_place_finished or self.first_time_grasp:
self.first_time_grasp = False
self.pick_place_finished = False
print("apple_pose_callback")
apple = SolidPrimitive()
apple.type = SolidPrimitive.SPHERE
apple.dimensions = [0.03, 0.03, 0.03]
self.object.name = "apple"
self.object.primitives = [apple]
self.object.primitive_poses = [message]
# add stamp and frame
self.object.header.stamp = rospy.Time.now()
self.object.header.frame_id = "base_link"
goal = GraspPlanningGoal()
goal.object = self.object
self.grasp_planner_client.send_goal(goal)
self.grasp_planner_client.wait_for_result(rospy.Duration(5.0))
grasp_planner_result = self.grasp_planner_client.get_result()
self.grasps = grasp_planner_result.grasps
self.ready_for_grasp = True
def updateScene(self):
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0)
self.scene.addSolidPrimitive(self.object.name,
self.object.primitives[0],
self.object.primitive_poses[0])
self.scene.waitForSync()
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name, places, scene=self.scene)
return success
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def stow(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def intermediate_stow(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
target, origin, wrist, group_name, group_joint_name, base_link = pd.show_states()
final_trans = pd.calculate_transform(target, origin)
final_grasp = pd.grasp_to_body(final_trans, grasp)
final_msg_trans = pd.convert_to_message(final_grasp)
get_ik = pd.IK(final_msg_trans)
tablemsg = pd.show_states_table()
tablest = pd.calculate_transform(tablemsg, origin)
table = pd.convert_to_message(tablest)
planning_scene.removeCollisionObject('table')
table_size_x = 0.913
table_size_y = 0.913
table_size_z = 0.040
table_x = table.position.x
table_y = table.position.y
table_z = table.position.z + 0.74
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z,
table_x, table_y, table_z)
target = pd.convert_to_message(final_trans)
planning_scene.removeCollisionObject('cube')
cube_size_x = 0.044
cube_size_y = 0.044
cube_size_z = 0.180
cube_x = target.position.x
cube_y = target.position.y
cube_z = target.position.z
planning_scene.addBox('cube', cube_size_x, cube_size_y, cube_size_z,
cube_x, cube_y, cube_z)
final_grasp = pd.grasp_to_body(final_trans,grasp)
fmt = pd.convert_to_message(final_grasp)
pose_goal = geometry_msgs.msg.Pose()
pose_goal.position.x = fmt.position.x
pose_goal.position.y = fmt.position.y
class UR_Moveit_API:
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('eef_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 get_basic_infomation(self):
# We can get the name of the reference frame for this robot:
planning_frame = self.group_commander.get_planning_frame()
print("============ Reference frame: %s" % planning_frame,
"============")
# We can also print the name of the end-effector link for this group:
eef_link = self.group_commander.get_end_effector_link()
print("============ End effector: %s" % eef_link, "============")
# We can get a list of all the groups in the robot:
group_names = self.robot.get_group_names()
print("============ Robot Groups:", group_names, "============")
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("============ Printing robot state")
print(self.robot.get_current_state(), "============")
print("================================================")
def joint_callback(self, joint_state):
self.joint_state = joint_state
def open_gripper(self, drop=False):
plan = self.gripper.plan(GRIPPER_DROP if drop else GRIPPER_OPEN)
return self.gripper.execute(plan, wait=True)
def add_bounds(self):
print("Complete to add_bounds")
# size_x, size_y, size_z, x, y, z
self.scene.addBox('table', 1.0, 1.0, 1.0, .0, .0, -0.5)
# add a cube of 0.1m size, at [1, 0, 0.5] in the base_link frame
#self.scene.addCube("my_cube", 0.1, 1, 0, 0.5)
# Remove the cube
#self.scene.removeCollisionObject("my_cube")
def remove_bounds(self):
for obj in self.scene.get_objects().keys():
self.scene.remove_world_object(obj)
def close_gripper(self):
plan = self.gripper.plan(GRIPPER_CLOSED)
return self.gripper.execute(plan, wait=True)
def get_ik_client(self, request):
rospy.wait_for_service('/compute_ik')
inverse_ik = rospy.ServiceProxy('/compute_ik', GetPositionIK)
ret = inverse_ik(request)
if ret.error_code.val != 1:
return None
return ret.solution.joint_state
def get_fk_client(self, header, link_names, robot_state):
rospy.wait_for_service('/compute_fk')
fk = rospy.ServiceProxy('/compute_fk', GetPositionFK)
ret = fk(header, link_names, robot_state)
if ret.error_code.val != 1:
return None
return ret.pose_stamped
def joint_limits(self, header, link_names, robot_state):
rospy.wait_for_service('/compute_fk')
fk = rospy.ServiceProxy('/compute_fk', GetPositionFK)
ret = fk(header, link_names, robot_state)
if ret.error_code.val != 1:
return None
return ret.pose_stamped
def eval_grasp(self, threshold=.0001, manual=False):
if manual:
user_input = None
while user_input not in ('y', 'n', 'r'):
user_input = raw_input(
'Successful grasp? [(y)es/(n)o/(r)edo]: ')
if user_input == 'y':
return 1, None
elif user_input == 'n':
return 0, None
else:
return -1, None
else:
current = np.array(self.gripper.get_current_joint_values())
target = np.array(GRIPPER_CLOSED)
error = current[0] - target[0]
return error > threshold, error
def orient_to_target(self, x=None, y=None, angle=None):
current = self.get_joint_values()
if x or y:
angle = np.arctan2(y, x)
if angle >= 3.1:
angle = 3.1
elif angle <= -3.1:
angle = -3.1
current[0] = angle
plan = self.group_commander.plan(current)
return self.group_commander.execute(plan, wait=True)
def wrist_rotate(self, angle):
rotated_values = self.group_commander.get_current_joint_values()
rotated_values[4] = angle - rotated_values[0]
if rotated_values[4] > np.pi / 2:
rotated_values[4] -= np.pi
elif rotated_values[4] < -(np.pi / 2):
rotated_values[4] += np.pi
plan = self.group_commander.plan(rotated_values)
return self.group_commander.execute(plan, wait=True)
def get_joint_values(self):
return self.group_commander.get_current_joint_values()
def get_current_pose(self):
return self.group_commander.get_current_pose()
def move_to_neutral(self):
print('Moving to neutral...')
plan = self.group_commander.plan(NEUTRAL_VALUES)
return self.group_commander.execute(plan, wait=True)
def move_to_drop(self, angle=None):
drop_positions = DROPPING_VALUES[:]
if angle:
drop_positions[0] = angle
plan = self.group_commander.plan(drop_positions)
return self.group_commander.execute(plan, wait=True)
def move_to_empty(self):
plan = self.group_commander.plan(EMPTY_VALUES)
return self.group_commander.execute(plan, wait=True)
def move_to_reset(self):
print('Moving to reset...')
plan = self.group_commander.plan(RESET_VALUES)
return self.group_commander.execute(plan, wait=True)
def reset(self, data):
print("data: ", data)
self.move_to_reset()
rospy.sleep(0.5)
def orient_to_pregrasp(self, x, y):
angle = np.arctan2(y, x)
return self.move_to_drop(angle)
def move_to_grasp(self, x, y, z, angle, compensate_control_noise=True):
if compensate_control_noise:
x = (x - CONTROL_NOISE_COEFFICIENT_BETA
) / CONTROL_NOISE_COEFFICIENT_ALPHA
y = (y - CONTROL_NOISE_COEFFICIENT_BETA
) / CONTROL_NOISE_COEFFICIENT_ALPHA
current_p = self.group_commander.get_current_pose().pose
p1 = Pose(position=Point(x=x, y=y, z=z), orientation=DOWN_ORIENTATION)
plan, f = self.group_commander.compute_cartesian_path([current_p, p1],
0.001, 0.0)
joint_goal = list(plan.joint_trajectory.points[-1].positions)
first_servo = joint_goal[0]
joint_goal[4] = (angle - first_servo) % np.pi
if joint_goal[4] > np.pi / 2:
joint_goal[4] -= np.pi
elif joint_goal[4] < -(np.pi / 2):
joint_goal[4] += np.pi
try:
plan = self.group_commander.plan(joint_goal)
except MoveItCommanderException as e:
print('Exception while planning')
traceback.print_exc(e)
return False
return self.group_commander.execute(plan, wait=True)
def move_to_vertical(self, z, force_orientation=True, shift_factor=1.0):
current_p = self.group_commander.get_current_pose().pose
current_angle = self.get_joint_values()[4]
orientation = current_p.orientation if force_orientation else None
p1 = Pose(position=Point(x=current_p.position.x * shift_factor,
y=current_p.position.y * shift_factor,
z=z),
orientation=orientation)
waypoints = [current_p, p1]
plan, f = self.group_commander.compute_cartesian_path(
waypoints, 0.001, 0.0)
if not force_orientation:
return self.group_commander.execute(plan, wait=True)
else:
if len(plan.joint_trajectory.points) > 0:
joint_goal = list(plan.joint_trajectory.points[-1].positions)
else:
return False
joint_goal[4] = current_angle
plan = self.group_commander.plan(joint_goal)
return self.group_commander.execute(plan, wait=True)
def move_to_target(self, target):
assert len(target) >= 6, 'Invalid target command'
for i, pos in enumerate(target):
self.joint_pubs[i].publish(pos)
def move_to_joint_position(self, joints):
"""
Adds the given joint values to the current joint values, moves to position
"""
joint_state = self.joint_state
joint_dict = dict(zip(joint_state.name, joint_state.position))
for i in range(len(JOINT_NAMES)):
joint_dict[JOINT_NAMES[i]] += joints[i]
joint_state = JointState()
joint_state.name = JOINT_NAMES
joint_goal = [joint_dict[joint] for joint in JOINT_NAMES]
joint_goal = np.clip(np.array(joint_goal), JOINT_MIN, JOINT_MAX)
joint_state.position = joint_goal
header = Header()
robot_state = RobotState()
robot_state.joint_state = joint_state
link_names = ['eef_link']
position = self.get_fk_client(header, link_names, robot_state)
target_p = position[0].pose.position
x, y, z = target_p.x, target_p.y, target_p.z
conditions = [
x <= BOUNDS_LEFTWALL, x >= BOUNDS_RIGHTWALL, y <= BOUNDS_BACKWALL,
y >= BOUNDS_FRONTWALL, z <= BOUNDS_FLOOR, z >= 0.15
]
print("Target Position: %0.4f, %0.4f, %0.4f" % (x, y, z))
for condition in conditions:
if not condition:
return
self.move_to_target(joint_goal)
rospy.sleep(0.15)
def sweep_arena(self):
self.remove_bounds()
self.move_to_drop(.8)
plan = self.group_commander.plan(TL_CORNER[0])
self.group_commander.execute(plan, wait=True)
plan = self.group_commander.plan(TL_CORNER[1])
self.group_commander.execute(plan, wait=True)
plan = self.group_commander.plan(L_SWEEP[0])
self.group_commander.execute(plan, wait=True)
plan = self.group_commander.plan(L_SWEEP[1])
self.group_commander.execute(plan, wait=True)
self.move_to_drop(-.8)
plan = self.group_commander.plan(BL_CORNER[0])
self.group_commander.execute(plan, wait=True)
plan = self.group_commander.plan(BL_CORNER[1])
self.group_commander.execute(plan, wait=True)
self.move_to_drop(-2.45)
plan = self.group_commander.plan(BR_CORNER[0])
self.group_commander.execute(plan, wait=True)
plan = self.group_commander.plan(BR_CORNER[1])
self.group_commander.execute(plan, wait=True)
self.move_to_drop(2.3)
plan = self.group_commander.plan(TR_CORNER[0])
self.group_commander.execute(plan, wait=True)
plan = self.group_commander.plan(TR_CORNER[1])
self.group_commander.execute(plan, wait=True)
self.add_bounds()
def discard_object(self):
plan = self.group_commander.plan(PREDISCARD_VALUES)
self.group_commander.execute(plan, wait=True)
plan = self.group_commander.plan(DISCARD_VALUES)
self.group_commander.execute(plan, wait=True)
self.open_gripper(drop=True)
plan = self.group_commander.plan(PREDISCARD_VALUES)
self.group_commander.execute(plan, wait=True)
self.move_to_neutral()
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.27
microwave_x = 0.97
microwave_y = 0.18
microwave_z = 0.06
microwave_side_height = 0.2
microwave_r_width = 0.135
microwave_r_y = microwave_y - 0.175
microwave_l_width = 0.035
microwave_l_y = microwave_y + 0.222
microwave_bottom_height = 0.05
microwave_top_height = 0.04
microwave_back_depth = 0.03
microwave_back_x = table_x + (microwave_depth /
2) + (microwave_back_depth / 2)
microwave_door_width = 0.09
microwave_door_x = microwave_x - 0.33
microwave_door_y = microwave_l_y + 0.027
planning_scene.addBox(
'microwave_top', microwave_depth, microwave_width,
microwave_top_height, microwave_x, microwave_y,
table_height + microwave_z + microwave_bottom_height +
microwave_side_height + (microwave_top_height / 2))
planning_scene.addBox(
'microwave_bottom', microwave_depth, microwave_width,
microwave_bottom_height, microwave_x, microwave_y,
table_height + microwave_z + (microwave_bottom_height / 2))
planning_scene.addBox(
'microwave_side_r', microwave_depth, microwave_r_width,
microwave_side_height, microwave_x, microwave_r_y, table_height +
microwave_z + microwave_bottom_height + (microwave_side_height / 2))
planning_scene.addBox(
'microwave_side_l', microwave_depth, microwave_l_width,
microwave_side_height, microwave_x, microwave_l_y, table_height +
microwave_z + microwave_bottom_height + microwave_side_height / 2)
planning_scene.addBox('microwave_back', microwave_back_depth,
microwave_width, microwave_height, microwave_back_x,
microwave_y,
table_height + microwave_z + microwave_height / 2)
planning_scene.addBox(
'microwave_door', 0.39, microwave_door_width, microwave_height + 0.01,
microwave_door_x, microwave_door_y,
table_height + microwave_z + microwave_height / 2 + 0.005)
rospy.sleep(2)
rospy.spin()
def main():
rospy.init_node('pour_scene')
wait_for_time()
target_name = 'cup1'
x_pose = 0.329202820349
y_pose = -0.01
z_pose = 0.060
x_ori, y_ori, z_ori, w_ori = cvt_e2q(0, 0, 0)
x, y, z, _ = getState('cup2')
head = fetch_api.Head()
arm = fetch_api.Arm()
torso = fetch_api.Torso()
torso.set_height(fetch_api.Torso.MAX_HEIGHT)
head.look_at('base_link', x, y, z)
sess = tensorflow.Session()
model = load_model(sess)
x, y, z, _ = getState(target_name)
# observation test
get_observation()
move_arm_to(arm, x_pose, y + y_pose, z + z_pose, x_ori, y_ori, z_ori,
w_ori)
x, y, z, _ = getState('table1')
base = fetch_api.Base()
if x > 1:
base.go_forward(0.6, speed=0.2)
# collision in motion planning
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
length_table = 1
width_table = 1
height_table = 0.04
x_table, y_table, z_table, _ = getState('table1')
z_table = z + 0.7
planning_scene.addBox('table', length_table, width_table, height_table,
x_table, y_table, z_table)
length_box = 0.05
width_box = 0.05
height_box = 0.2
x, y, z, _ = getState('cup1')
x_box = x
y_box = y
z_box = z
planning_scene.addBox('mug_1', length_box, width_box, height_box, x_box,
y_box, z_box)
length_box = 0.03
width_box = 0.05
height_box = 0.2
x, y, z, _ = getState('cup2')
x_box = x
y_box = y
z_box = z
planning_scene.addBox('mug_2', length_box, width_box, height_box, x_box,
y_box, z_box)
# the initial position of gripper is (-0.5, 0, 0), and
# the final position of gripper is (-0.5+x_pose, y_pose, z_pose).
x, y, z, _ = getState(target_name)
move_arm_to(arm, x - 0.5 + x_pose, y + y_pose, z + z_pose, x_ori, y_ori,
z_ori, w_ori)
planning_scene.removeCollisionObject('mug_1')
# planning_scene.removeCollisionObject('mug_2')
# planning_scene.removeCollisionObject('mug')
# planning_scene.removeCollisionObject('table')
gripper = fetch_api.Gripper()
effort = gripper.MAX_EFFORT
gripper.close(effort)
x, y, z, _ = getState(target_name)
move_arm_to(arm, x - 0.5 + x_pose, y + y_pose, z + z_pose + 0.06, x_ori,
y_ori, z_ori, w_ori)
x, y, z, _ = getState('cup2')
head.look_at('base_link', x, y, z)
for _ in xrange(50):
obs = get_observation()
act = model.choose_action(obs)
print('obs: {}, action: {}'.format(obs, act))
execute_action(act, arm)
time.sleep(0.5)
class FakePerceptionNode:
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 update(self):
pass
def simulated_link_state_callback(self, linksmsg):
"""
string[] name
geometry_msgs/Pose[] pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
geometry_msgs/Twist[] twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
"""
cube_transforms = self.propagate_link_states_to_tf(
linksmsg, "cube", "cube_", "map")
table_transforms = self.propagate_link_states_to_tf(
linksmsg, "table", "table_", "map")
ellapsed = rospy.Time.now() - self.last_update
if ellapsed > self.update_period:
# if self.update_planning_scene:
rospy.logdebug("updating planning scene")
attached_objects = self.planning_scene.getKnownAttachedObjects()
rospy.loginfo(attached_objects)
#self.update_planning_scene = False
if self.table_collision and not "cube_0" in attached_objects:
for i, table_transf in enumerate(table_transforms):
#self.planning_scene.removeCollisionObject("table_" + str(i))
thickness = 0.12
pos = table_transf[0]
self.planning_scene.addBox("table_" + str(i), 1.2, 1.3,
0.001 + thickness, pos[0],
pos[1],
0.7 - thickness * 0.5) # 0.68
if self.cube_collision and not "cube_0" in attached_objects:
for i, cube_transf in enumerate(cube_transforms):
#self.planning_scene.removeCollisionObject("cube_" + str(i))
pos = cube_transf[0]
self.planning_scene.addCube("cube_" + str(i), 0.06, pos[0],
pos[1], pos[2])
#self.cube_collision = False
def propagate_link_states_to_tf(self, linksmsg, link_name_filter,
object_prefix, global_frame):
filteredLinkPoses = [
b for i, b in enumerate(linksmsg.pose)
if link_name_filter in linksmsg.name[i]
]
transforms = []
for i, cubepose in enumerate(filteredLinkPoses):
quat = [
cubepose.orientation.x, cubepose.orientation.y,
cubepose.orientation.z, cubepose.orientation.w
]
trans = [
cubepose.position.x, cubepose.position.y, cubepose.position.z
]
self.tf_broacaster.sendTransform(trans, quat, rospy.Time.now(),
object_prefix + str(i),
global_frame)
transforms.append([trans, quat])
return transforms
# rightgripper.open()
scene = moveit_commander.PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
rospy.sleep(2)
# box_pose = geometry_msgs.msg.PoseStamped()
# box_pose.header.frame_id = robot.get_planning_frame()
# box_pose.pose.position.x = 1.
# box_pose.pose.position.y = 0.5
# box_pose.pose.position.z = 0.5
# scene.add_box("table", box_pose, (0.5, 1.5, 0.6))
# name size and position
p.addBox("first part", 0.8, 0.43, 0.04, 1.23, 0, 0.36)
p.addBox("under part", 0.8, 0.43, 0.04, 1.23, 0, 0.0)
p.addBox("second part", 0.8, 0.01, 1.6, 1.23, -0.22, 0)
p.addBox("third part", 0.8, 0.01, 1.6, 1.23, 0.22, 0)
p.waitForSync()
def load_gazebo_models(table_pose=Pose(position=Point(x=0.7, y=0.0, z=0.0)),
table_reference_frame="world",
block_pose1=Pose(position=Point(x=0.6725, y=0.0765, z=-0.135)),
block_pose2=Pose(position=Point(x=0.55, y=-0.2, z=-0.135)),
block_pose3=Pose(position=Point(x=0.7, y=-0.1, z=-0.135)),
block_pose4=Pose(position=Point(x=0.58, y=-0.03, z=-0.135)),
block_reference_frame="base"):
pass
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
import argparse
import rospy
from moveit_python import PlanningSceneInterface
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Remove objects from the MoveIt planning scene.")
parser.add_argument("--name", nargs="?", help="Name of object to remove")
args = parser.parse_args()
if args.name:
rospy.init_node("add_boxes")
scene = PlanningSceneInterface("/base_link")
print("Adding Box with name: %s" % args.name)
scene.addBox(args.name, 0.1, 0.1, 0.1, 3, 3, 3, True)
scene.waitForSync()
else:
parser.print_help()
def main():
print('We are running the main of arm_obstable_demo')
rospy.init_node('arm_obstacle_demo')
wait_for_time()
print(
"If planning scene fails to init, make sure you have launched move_group.launch"
)
planning_scene = PlanningSceneInterface("base_link")
# Create table object
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z,
table_x, table_y, table_z)
# Create divider object
planning_scene.removeCollisionObject('divider')
size_x = 0.3
size_y = 0.01
size_z = 0.2
x = table_x - (table_size_x / 2) + (size_x / 2)
y = 0
z = table_z + (table_size_z / 2) + (size_z / 2)
planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
# Before moving to the first pose
planning_scene.removeAttachedObject('tray')
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.75
pose2.pose.orientation.w = 1
arm = fetch_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
kwargs = {
'allowed_planning_time': 15,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': False
}
error = arm.move_to_pose(pose1, **kwargs)
# If the robot reaches the first pose successfully, then "attach" an object there
# Of course, you would have to close the gripper first and ensure that you grasped the object properly
if error is not None:
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to,
frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
rospy.sleep(1)
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
# At the end of your program
planning_scene.removeAttachedObject('tray')
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True) #ノードの初期化 reaching ていう名前
self.robot = moveit_commander.RobotCommander() # インスタンスの作成
self.model_pose = [0.0, 0.0, 0.0] #モデルの姿勢
self.obstacle_pose = [0.0, 0.0, 0.0] #障害物の位置
self.subsc_pose = [0.0, 0.0, 0.0] #購読した座標
self.subsc_orientation = [0.0, 0.0, 0.0, 1.0] #購読した四元数
self.set_forbidden() #禁止エリアの初期化
self.set_init_pose() #位置姿勢の初期化
self.before_pose = [0, 0, 0] #前回の場所
self.pcarray = 0 #pointcloud を np array にしたものを入れる
self.executeFlag = False #unity コントローラ B ボタンと対応
self.before_executeFlag = False #前フレームの executeFlag
self.waypoints = []
self.way_flag = False #unity コントローラの 中指のボタンに対応
self.before_wayflag = False #前フレームのway_flag
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 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.addCube("targetOBJ", 0.5, 0.0, 1.0, 2.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 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 callback(self, data): #トピックにデータが追加されるたびに呼ばれる
x = data.pose[1].position.x
y = data.pose[1].position.y
z = data.pose[1].position.z
self.model_pose = [x, y, z] #modelの姿勢を代入
x = data.pose[2].position.x
y = data.pose[2].position.y
z = data.pose[2].position.z
self.obstacle_pose = [x, y, z] #障害物の姿勢を代入
def callbacksub(self, data):
x = data.x
y = data.y
z = data.z
self.subsc_pose = [x, y, z] #購読した座標を代入
def callbackunity(self, data):
x = data.pose.position.x
y = data.pose.position.y
z = data.pose.position.z
self.subsc_pose = [y, -x, z] #unity からの座標 座標変換も行っている
x = data.pose.orientation.x
y = data.pose.orientation.y
z = data.pose.orientation.z
w = data.pose.orientation.w
self.subsc_orientation = [y, -x, z, w] #unity からの四元数
def rtabcallback(self, data):
dtype_list = [(f.name, np.float32) for f in data.fields]
cloud_arr = np.fromstring(data.data, dtype_list)
self.pcarray = np.reshape(cloud_arr, (data.height, data.width))
def excallback(self, data):
self.executeFlag = data.data
def waycallback(self, data):
self.way_flag = data.data
def start_subscriber(self): #購読をスタート
rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
# /gazebo/model_states から gazebo_msgs/ModelStates 型のトピックを購読 トピックが更新されるたびに self.callback を呼ぶ
#rospy.Subscriber("/UR/reaching/pose",Vector3,self.callbacksub)
rospy.Subscriber("/unity/target", PoseStamped,
self.callbackunity) #unityから配信しているトピックを購読
rospy.Subscriber('/rtabmap/cloud_map', PointCloud2,
self.rtabcallback) #zed で slam したpointcloud を購読
rospy.Subscriber('/unity/execute', Bool, self.excallback)
rospy.Subscriber('/unity/wayflag', Bool, self.waycallback)
def change_state(self, target):
#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 を使わなかった場合
set_flag = (self.before_pose != target.position
) # target の位置が変化している = トリガーが押された
hold_flag = self.way_flag # hold button が押されている
exe_flag = (self.before_executeFlag != self.executeFlag
) # executeFlag が前フレームと違う = execute button が押された
print('set ' + str(set_flag))
print('hold ' + str(hold_flag))
print('exe ' + str(exe_flag))
if self.now_state == self.state['default']:
if set_flag:
self.now_state = self.state['plan_standby']
elif hold_flag:
self.now_state = self.state['hold']
elif self.now_state == self.state['hold']:
if not hold_flag:
self.now_state = self.state['way_standby']
elif self.now_state == self.state['way_standby']:
if exe_flag:
self.now_state = self.state['default']
elif self.now_state == self.state['plan_standby']:
if exe_flag:
self.now_state = self.state['default']
def target(self):
rate = rospy.Rate(1) #Rateクラスのインスタンス rateを１で
rospy.sleep(1) #1秒スリープ
useway = False
while not rospy.is_shutdown(): #シャットダウンフラグが立っていなければ、
self.planning_scene.removeCollisionObject("demo_cube") #オブジェクトの削除
self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除
self.planning_scene.addCube(
"demo_cube", 0.06, self.model_pose[0], self.model_pose[1],
self.model_pose[2]) #一辺が0.06の正方形をmodel_pose(座標)に追加
self.planning_scene.addBox(
"obstacle", 0.540614, 0.083603, 0.54373, self.obstacle_pose[0],
self.obstacle_pose[1],
self.obstacle_pose[2]) #---の大きさの箱をobstacle_pose(座標)に追加
print self.model_pose #model_poseを表示
group = moveit_commander.MoveGroupCommander(
"manipulator") #MoveGroupCommander クラスのインスタンス
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose #エンドエフェクタの位置姿勢
pose_target = geometry_msgs.msg.Pose(
) #geometry_msgs.pose のインスタンス ここにターゲット設定
pose_target.orientation.x = self.subsc_orientation[0]
pose_target.orientation.y = self.subsc_orientation[1]
pose_target.orientation.z = self.subsc_orientation[2]
pose_target.orientation.w = self.subsc_orientation[
3] #トピックから 四元数を代入
pose_target.position.x = self.subsc_pose[0] #
pose_target.position.y = self.subsc_pose[1] #
pose_target.position.z = self.subsc_pose[2] #トピックから 座標を代入
#self.planning_scene.removeCollisionObject("targetOBJ")
#self.planning_scene.addCube("targetOBJ", 0.005,self.subsc_pose[0] ,self.subsc_pose[1] ,self.subsc_pose[2] )
self.way_flag = False
#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.change_state(pose_target)
exe_flag = (self.before_executeFlag != self.executeFlag
) # executeFlag が前フレームと違う = execute button が押された
if self.now_state == self.state['default']:
if self.before_pose != pose_target.position:
group.set_pose_target(pose_target)
group.plan()
if self.now_state == self.state['hold']:
if self.before_pose != pose_target.position:
self.waypoints.append(copy.deepcopy(pose_target))
if self.now_state == self.state['way_standby']:
plan, fraction = group.compute_cartesian_path(
self.waypoints, 0.01, 0.0)
if exe_flag:
group.execute(plan)
self.waypoints = []
if self.now_state == self.state['plan_standby']:
if exe_flag:
group.go()
self.before_executeFlag = self.executeFlag
if self.before_pose != pose_target.position: #前フレームと座標が変化した
if self.way_flag: #waypoint を使用したティーチング(グリップボタン押してる)
self.waypoints.append(copy.deepcopy(pose_target))
useway = True
else:
group.set_pose_target(pose_target) #ターゲットをセット
useway = False
# group.set_pose_target(pose_target)#毎フレーム必要っぽい！！！！！！！！！！！！！
if not useway and self.before_pose != pose_target.position and not self.before_wayflag:
group.plan()
if self.before_wayflag != self.way_flag and not self.way_flag:
#前のフレームが true で false に変わったフレーム
print(self.waypoints)
plan, fraction = group.compute_cartesian_path(
self.waypoints, 0.01, 0.0)
self.before_pose = pose_target.position
self.before_wayflag = self.way_flag
if self.executeFlag != self.before_executeFlag:
if useway:
group.execute(plan)
self.before_executeFlag = self.executeFlag
self.waypoints = []
else:
group.go()
self.before_executeFlag = self.executeFlag
rospy.sleep(1) #1秒スリープ
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True)#ノードの初期化 reaching ていう名前
self.robot = moveit_commander.RobotCommander()# インスタンスの作成
self.model_pose = [0.0,0.0,0.0] #モデルの姿勢
self.obstacle_pose = [0.0,0.0,0.0] #障害物の位置
self.set_forbidden() #禁止エリアの初期化
self.set_init_pose() #位置姿勢の初期化
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 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 callback(self,data): #トピックにデータが追加されるたびに呼ばれる
x = data.pose[1].position.x
y = data.pose[1].position.y
z = data.pose[1].position.z
self.model_pose = [x,y,z] #modelの姿勢を代入
x = data.pose[2].position.x
y = data.pose[2].position.y
z = data.pose[2].position.z
self.obstacle_pose = [x,y,z] #障害物の姿勢を代入
def start_subscriber(self): #購読をスタート
rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
# /gazebo/model_states から gazebo_msgs/ModelStates 型のトピックを購読 トピックが更新されるたびに self.callback を呼ぶ
def target(self):
rate = rospy.Rate(1) #Rateクラスのインスタンス rateを１で
rospy.sleep(1) #1秒スリープ
while not rospy.is_shutdown(): #シャットダウンフラグが立っていなければ、
self.planning_scene.removeCollisionObject("demo_cube") #オブジェクトの削除
self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除
self.planning_scene.addCube("demo_cube", 0.06,self.model_pose[0],self.model_pose[1],self.model_pose[2]) #一辺が0.06の正方形をmodel_pose(座標)に追加
self.planning_scene.addBox("obstacle", 0.540614, 0.083603, 0.54373, self.obstacle_pose[0],self.obstacle_pose[1],self.obstacle_pose[2])#---の大きさの箱をobstacle_pose(座標)に追加
print self.model_pose #model_poseを表示
group = moveit_commander.MoveGroupCommander("manipulator") #MoveGroupCommander クラスのインスタンス
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose #エンドエフェクタの位置姿勢
pose_target = geometry_msgs.msg.Pose() #geometry_msgs.pose のインスタンス これに入れるとトピックに反映される？
pose_target.orientation.x = pose.orientation.x #
pose_target.orientation.y = pose.orientation.y #
pose_target.orientation.z = pose.orientation.z #
pose_target.orientation.w = pose.orientation.w #トピックに 四元数を代入
#pose_target.position.x = pose.position.x
#pose_target.position.y = pose.position.y
#pose_target.position.z = pose.position.z
pose_target.position.x = self.model_pose[0] #
pose_target.position.y = self.model_pose[1] #
pose_target.position.z = self.model_pose[2]+0.05 #トピックに 座標を代入
group.set_pose_target(pose_target) #ターゲットをセット
group.go() #移動
rospy.sleep(1) #1秒スリープ
def main():
rospy.init_node("arm_obstacle_demo")
wait_for_time()
argv = rospy.myargv()
planning_scene = PlanningSceneInterface("base_link")
# Create table obstacle
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z,
table_x, table_y, table_z)
# Create divider obstacle
planning_scene.removeCollisionObject('divider')
size_x = 0.3
size_y = 0.01
size_z = 0.2
x = table_x - (table_size_x / 2) + (size_x / 2)
y = 0
z = table_z + (table_size_z / 2) + (size_z / 2)
planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.75
pose2.pose.orientation.w = 1
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
gripper = robot_api.Gripper()
arm = robot_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
kwargs = {
'allowed_planning_time': 15,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': True,
'orientation_constraint': oc
}
planning_scene.removeAttachedObject('tray')
gripper.open()
error = arm.move_to_pose(pose1, **kwargs)
if error is not None:
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
rospy.sleep(2)
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to, frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
gripper.close()
rospy.sleep(2)
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
gripper.open()
planning_scene.removeAttachedObject('tray')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
return
def main():
rospy.init_node('arm_obstacle_demo')
wait_for_time()
planning_scene = PlanningSceneInterface('base_link')
# Create table obstacle
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z,
table_x, table_y, table_z)
# Create divider obstacle
planning_scene.removeCollisionObject('divider')
size_x = 0.3
size_y = 0.01
size_z = 0.4
x = table_x - (table_size_x / 2) + (size_x / 2)
y = 0
z = table_z + (table_size_z / 2) + (size_z / 2)
#planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.75
pose2.pose.orientation.w = 1
torso = fetch_api.Torso()
torso.set_height(fetch_api.Torso.MAX_HEIGHT)
arm = fetch_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
# and add an orientation constraint to pose 2:
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
kwargs = {
'allowed_planning_time': 100,
'execution_timeout': 100,
'num_planning_attempts': 15,
'replan': True,
'replan_attempts': 10
}
# Before moving to the first pose
planning_scene.removeAttachedObject('tray')
error = arm.move_to_pose(pose1, **kwargs)
# If the robot reaches the first pose successfully, then "attach" an object there
# Of course, you would have to close the gripper first and ensure that you grasped the object properly
if error is not None:
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to,
frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
rospy.sleep(1)
kwargs['orientation'] = oc
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
# At the end of your program
planning_scene.removeAttachedObject('tray')
def main():
# Create table obstacle
rospy.init_node('arm_obstacle_demo')
planning_scene = PlanningSceneInterface('base_link')
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z)
# Create divider obstacle
# planning_scene.removeCollisionObject('divider')
# size_x = 0.3
# size_y = 0.01
# size_z = 0.4
# x = table_x - (table_size_x / 2) + (size_x / 2)
# y = 0
# z = table_z + (table_size_z / 2) + (size_z / 2)
# planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.75
pose2.pose.orientation.w = 1
arm = fetch_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
#Orientation constraint
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
# move to pose args
kwargs = {
'allowed_planning_time': 20,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': True,
'orientation_constraint': oc
}
# Before moving to the first pose
planning_scene.removeAttachedObject('tray')
error = arm.move_to_pose(pose1, **kwargs)
if error is not None:
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to, frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
rospy.sleep(1)
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
# At the end of your program
planning_scene.removeAttachedObject('tray')
# Was working without this but now its needed
planning_scene.clear()
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.model_pose = [0.0, 0.0, 0.0]
self.obstacle_pose = [0.0, 0.0, 0.0]
self.set_forbidden()
self.set_init_pose()
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)
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)
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 callback(self, data):
x = data.pose[1].position.x
y = data.pose[1].position.y
z = data.pose[1].position.z
self.model_pose = [x, y, z]
x = data.pose[2].position.x
y = data.pose[2].position.y
z = data.pose[2].position.z
self.obstacle_pose = [x, y, z]
def start_subscriber(self):
rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
def target(self):
rate = rospy.Rate(1)
rospy.sleep(1)
while not rospy.is_shutdown():
self.planning_scene.removeCollisionObject("demo_cube")
self.planning_scene.removeCollisionObject("obstacle")
self.planning_scene.addCube("demo_cube", 0.06, self.model_pose[0],
self.model_pose[1], self.model_pose[2])
self.planning_scene.addBox("obstacle", 0.540614, 0.083603, 0.54373,
self.obstacle_pose[0],
self.obstacle_pose[1],
self.obstacle_pose[2])
print self.model_pose
group = moveit_commander.MoveGroupCommander("manipulator")
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = pose.orientation.x
pose_target.orientation.y = pose.orientation.y
pose_target.orientation.z = pose.orientation.z
pose_target.orientation.w = pose.orientation.w
#pose_target.position.x = pose.position.x
#pose_target.position.y = pose.position.y
#pose_target.position.z = pose.position.z
pose_target.position.x = self.model_pose[0]
pose_target.position.y = self.model_pose[1]
pose_target.position.z = self.model_pose[2] + 0.05
group.set_pose_target(pose_target)
group.go()
rospy.sleep(1)
class World:
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)
"""Calls a method to display the collision objects.
"""
def create_planning_scene(self):
print "[INFO] Create_planning_scene"
self.display_towers_and_disks()
"""This method collects all needed information and
publish them to other methods.
"""
def display_towers_and_disks(self):
print "[INFO] Display towers and disks"
for tower in self.tower_array:
# call method to publish new tower
self.publish_scene(tower.get_position(), tower)
# set color by name
self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0, 1.0)
# publish color
self.planning_scene_interface.sendColors()
for disk in self.disk_array:
self.publish_scene(disk.get_position(), None, disk)
self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1],
disk.get_color()[2], disk.get_color()[3])
self.planning_scene_interface.sendColors()
# wait for sync after publishing collision objects
self.planning_scene_interface.waitForSync(5.0)
rospy.sleep(5)
"""This method creates a box or a cylinder with methods of
the planning scene interface.
:param position: the position of the new collision object
:param tower: the tower object
:param disk: the disk object
"""
def publish_scene(self, position, tower=None, disk=None):
if tower is not None:
self.planning_scene_interface.addBox(tower.get_name(), self.max_gripper / 100.0, self.max_gripper / 100.0,
(self.tower_positions[tower.get_id() - 1][2] * 2), position[0],
position[1], position[2])
else:
self.planning_scene_interface.addCylinder(disk.get_id(), self.disk_height, disk.get_diameter() / 2,
position[0], position[1], position[2])
"""This method cleans the planning scene.
:param close: with this flag a new planning scene objects will be removed
in sync otherwise the object will be deleted without syncing the scene
"""
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
"""This method corrects the position of the moved disk.
:param tower: parent tower of the disk
"""
def refresh_disk_pose(self, tower):
print "[INFO] Refresh disk pose"
disk = tower.get_last()
if self.DEBUG is 1:
print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\
"tower pose", tower.get_position()
# remove the disk from planning scene
self.planning_scene_interface.removeCollisionObject(disk.get_id(), False)
# publish collision object and set old color
self.publish_scene(disk.get_position(), None, disk)
self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1],
disk.get_color()[2], disk.get_color()[3])
self.planning_scene_interface.sendColors()
def main():
rospy.init_node('arm_obstacle_demo')
wait_for_time()
# argv = rospy.myargv()
planning_scene = PlanningSceneInterface(frame='base_link')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
planning_scene.removeAttachedObject('tray')
# Create table obstacle
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z,
table_x, table_y, table_z)
# Create divider obstacle
# size_x = 0.3
# size_y = 0.01
# size_z = 0.4
# x = table_x - (table_size_x / 2) + (size_x / 2)
# y = 0
# z = table_z + (table_size_z / 2) + (size_z / 2)
# planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
# add orientation constraint
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.75
pose2.pose.orientation.w = 1
# # set torso to max height
torso = robot_api.Torso()
torso.set_height(robot_api.Torso.MAX_HEIGHT)
arm = robot_api.Arm()
# register shutdown method
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
kwargs = {
'allowed_planning_time': 20,
'execution_timeout': 20,
'num_planning_attempts': 10,
'replan': True
}
error = arm.move_to_pose(pose1, **kwargs)
if error is not None:
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
# attach an object if pose1 is successfully reached
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to,
frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
# close the gripper
# gripper = robot_api.Gripper()
# gripper.close()
rospy.sleep(1)
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
# At the end of the program
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
planning_scene.removeAttachedObject('tray')
class ArmController(object):
def __init__(self):
# set arm and gripper, and fiducial id
self.arm = fetch_api.Arm()
self.gripper = fetch_api.Gripper()
# Will need to change this to something real at some point
#self.target_id = None# target_id
self.planning_scene = PlanningSceneInterface('base_link')
# First, load the poses for the fiducial insert movement
self.sequence = pickle.load(open(INSERT_GRASP_POSES, "rb"))
self.gripper_open = True
# # Init the reader for the tags
self.reader = ArTagReader()
self.sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers,
self.reader.callback)
def open_gripper(self):
self.gripper.open()
self.gripper_open = True
def add_bounding_box(self):
# Init the planning scene for collisions
print "waiting for service...."
rospy.wait_for_service('get_spines')
print "found service!"
get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations)
response = get_spine_poses() # 0.17
self.planning_scene.addBox('surface', (response.surface_x_size - 0.19),
response.surface_y_size,
response.surface_z_size,
response.surface_pose.position.x,
response.surface_pose.position.y,
response.surface_pose.position.z)
# Adding a bounding box for the delivery table
def add_delivery_bounding_box(self):
print "waiting for service...."
rospy.wait_for_service('get_spines')
print "found service!"
get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations)
response = get_spine_poses()
self.planning_scene.addBox('surface', (response.surface_x_size + 0.05),
response.surface_y_size,
response.surface_z_size,
response.surface_pose.position.x,
response.surface_pose.position.y,
response.surface_pose.position.z)
def grab_tray(self, target_id, target_marker_pose):
# This is the same as the pbd action stuff, not making any changes at the moment
self.gripper.open()
self.gripper_open = True
# target_marker_pose = None
# check = 0
# print "Searching for fiducial...."
# while target_marker_pose == None and check < 100:
# # If the fiducial was not seen, try again
# rospy.sleep(0.1)
# check += 1
# for marker in self.reader.markers:
# if target_id == marker.id:
# target_marker_pose = marker.pose.pose
# if target_marker_pose == None:
# print "Fiducial not found :("
# return False
# else:
# print "Fiducial Found :)"
# target_marker_pose.orientation.x = 0.0
# target_marker_pose.orientation.y = 0.0
# target_marker_pose.orientation.z = 0.0
# target_marker_pose.orientation.w = 1.0
everError = None
# This is the same as the pbd action stuff, not making any changes at the moment
for pbd_pose in self.sequence:
move_pose = PoseStamped()
move_pose.header.frame_id = 'base_link'
if pbd_pose.frame == 'base_link':
move_pose.pose = pbd_pose.pose
print "Default pose"
print pbd_pose.pose
else:
# for marker in reader.markers:
# if target_id == marker.id:
print "Calculating pose relative to marker...."
# Transform the pose to be in the base_link frame
pose_in_tag_frame = pose_to_transform(pbd_pose.pose)
#tag_in_base_frame = pose_to_transform(marker.pose.pose)
tag_in_base_frame = pose_to_transform(target_marker_pose)
target_matrix = np.dot(tag_in_base_frame, pose_in_tag_frame)
target_pose = transform_to_pose(target_matrix)
move_pose.pose = target_pose
#rospy.sleep(1)
err = self.arm.move_to_pose(move_pose,
num_planning_attempts=2) # 3
print "Error in move to pose: ", err
temp = 0
while temp < 3 and err != None:
#if err != None:
#return False
print "Trying This pose again"
err = self.arm.move_to_pose(move_pose,
num_planning_attempts=3,
tolerance=0.03)
print "Error in move to pose: ", err
temp += 1
if err != None:
print "Arm failed to move to pose"
print "Returning to Default Pose"
default_pose = PoseStamped()
default_pose.header.frame_id = 'base_link'
default_pose.pose.position.x = 0.0
default_pose.pose.position.y = 0.467635764991
default_pose.pose.position.z = 0.743876436337
default_pose.pose.orientation.x = 0.0
default_pose.pose.orientation.y = 0.0
default_pose.pose.orientation.z = 0.0
default_pose.pose.orientation.w = 1.0
err = self.arm.move_to_pose(default_pose)
print "returned to default pose"
return False
# Check the gripper to open/close
if pbd_pose.gripper_open != self.gripper_open:
if self.gripper_open == True:
self.gripper.close()
self.gripper_open = False
else:
self.gripper.open()
self.gripper_open = True
return True
def find_grasp_pose(self, target_id, target_marker_pose):
# target_fiducial = None
# check = 0
# while target_fiducial == None and check < 100:
# # If the fiducial was not seen, try again
# rospy.sleep(0.1)
# check += 1
# for marker in self.reader.markers:
# if marker.id == target_id:
# target_fiducial = marker
# if target_fiducial == None:
# print "Failed to find fiducial :("
# else:
# print "Found fidcuail!"
check = 0
found_good_pose = False
closest_pose = None
print "waiting for service...."
rospy.wait_for_service('get_spines')
print "found service!"
try:
while check < 20 and found_good_pose == False:
get_spine_poses = rospy.ServiceProxy('get_spines',
GetSpineLocations)
response = get_spine_poses()
spine_poses = response.spine_poses
min_dist = float('inf')
for pose in spine_poses:
#distance = calculate_euclidean_distance(pose, target_fiducial.pose.pose)
distance = calculate_euclidean_distance(
pose, target_marker_pose)
if distance < min_dist:
min_dist = distance
closest_pose = pose
check += 1
if closest_pose != None:
#if closest_pose.position.x > target_fiducial.pose.pose.position.x and closest_pose.position.y < (target_fiducial.pose.pose.position.y + 0.025) and closest_pose.position.y > (target_fiducial.pose.pose.position.y - 0.025):
if closest_pose.position.x > target_marker_pose.position.x and closest_pose.position.y < (
target_marker_pose.position.y +
0.025) and closest_pose.position.y > (
target_marker_pose.position.y - 0.025):
found_good_pose = True
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# print target_fiducial.id
# closest_pose = None
# min_dist = float('inf')
# for pose in spine_poses:
# distance = calculate_euclidean_distance(pose, target_fiducial.pose.pose)
# if distance < min_dist:
# min_dist = distance
# closest_pose = pose
if found_good_pose == False:
print "Failed to find good pose"
return None
else:
print "Found good pose"
#print "Pose closest to target fiducial"
#print closest_pose
return closest_pose
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True)
self.robot = moveit_commander.RobotCommander()
# self.model_pose = [0.0,0.2,0.2 , 0,0,0,1]
self.model_pose = [0.0, 0.3, 0.1, 0, 0, 0, 1]
self.obstacle_pose = [0.0, 0.0, 0.0]
self.set_forbidden()
self.set_init_pose()
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.removeCollisionObject("my_ground")
self.planning_scene.addCube("my_ground", 2, 0, 0, -1.04)
self.planning_scene.addBox("obstacle", 0.1, 0.5, 1, -0.3, 0, 0.5)
self.planning_scene.addBox("obstacle2", 0.5, 0.1, 1, 0, -0.3, 0.5)
#self.planning_scene.addCube("demo_cube", 0.06,0,0.3,0)
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)
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 = [-1.26, -0.64, -2.44, -0.66, 1.56, 0.007]
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 callback(self, data):
x = data.pose[1].position.x
y = data.pose[1].position.y
z = data.pose[1].position.z
def start_subscriber(self):
rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
def reaching_pose(self):
print self.model_pose
group = moveit_commander.MoveGroupCommander("manipulator")
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = self.model_pose[3]
pose_target.orientation.y = self.model_pose[4]
pose_target.orientation.z = self.model_pose[5]
pose_target.orientation.w = self.model_pose[6]
pose_target.position.x = self.model_pose[0]
pose_target.position.y = self.model_pose[1]
pose_target.position.z = self.model_pose[2]
group.set_pose_target(pose_target)
group.go()
#raw_input("Enter -->")
def gripper_close(self):
print("Gripper_Close")
gripper_pose = Char()
gripper_pose = 255
pub.publish(gripper_pose)
#raw_input("Enter -->")
def gripper_open(self):
print("Gripper_Open")
gripper_pose = Char()
gripper_pose = 0
pub.publish(gripper_pose)
#raw_input("Enter -->")
def move_45(self):
rate = rospy.Rate(1)
rospy.sleep(1)
a = 0.065
b = 0.040
c = 0.012
pattern_45 = [
[-0.10, 0.31, 0.015, 0, 0, 0, 1], #B
'c',
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
#[0.0 ,0.31 ,0.15 , 0,0,0,1], #A90
[0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
[0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795],
'o',
[0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
#[0.0 ,0.31 ,0.1 , 0,0,0,1], #A90
[-0.10, 0.401, 0.041, 0, 0, 0, 1],
'c',
[-0.10, 0.401, 0.100, 0, 0, 0, 1],
[0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795],
[0, 0.5 - b, 0.037 + b, 0.3826834, 0, 0, 0.9238795],
[0, 0.5 - c, 0.037 + c, 0.3826834, 0, 0, 0.9238795],
'o',
'c',
[0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795],
[-0.10, 0.401, 0.100, 0, 0, 0, 1],
[-0.10, 0.401, 0.050, 0, 0, 0, 1],
'o',
[-0.10, 0.401, 0.100, 0, 0, 0, 1],
[0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
[0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795],
'c',
[0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
[-0.10, 0.31, 0.015, 0, 0, 0, 1], #B
'o',
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
]
while not rospy.is_shutdown():
for pose in pattern_45:
if pose == 'c':
self.gripper_close()
rospy.sleep(1)
elif pose == 'o':
self.gripper_open()
rospy.sleep(1)
else:
self.model_pose = pose
self.reaching_pose()
if rospy.is_shutdown():
return 0
def move_90(self):
rate = rospy.Rate(1)
rospy.sleep(1)
a = 0.065
b = 0.040
c = 0.012
pattern_45 = [
[-0.10, 0.31, 0.015, 0, 0, 0, 1], #B
'c',
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
[0.0, 0.300, 0.1, 0, 0, 0, 1], #A90
[0.0, 0.315, 0.0358, 0, 0, 0, 1], #A90
'o',
[0.0, 0.315, 0.1, 0, 0, 0, 1], #A90
[-0.10, 0.40, 0.041, 0, 0, 0, 1], #negi
'c',
[-0.10, 0.40, 0.1, 0, 0, 0, 1],
[-0.001, 0.316, 0.1, 0, 0, 0, 1], #A90
[-0.001, 0.316, 0.053, 0, 0, 0, 1], #A90
'o',
'c',
[0.0, 0.314, 0.1, 0, 0, 0, 1], #A90
[-0.10, 0.40, 0.1, 0, 0, 0, 1],
[-0.10, 0.40, 0.05, 0, 0, 0, 1], #negi
'o',
#[-0.10,0.40 ,0.1 , 0,0,0,1],
[0.0, 0.300, 0.1, 0, 0, 0, 1], #A90
[0.0, 0.315, 0.036, 0, 0, 0, 1], #A90
'c',
[0.0, 0.300, 0.1, 0, 0, 0, 1], #A90
[-0.10, 0.320, 0.1, 0, 0, 0, 1], #B
[-0.10, 0.31, 0.0153, 0, 0, 0, 1], #B
'o',
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
]
while not rospy.is_shutdown():
for pose in pattern_45:
if pose == 'c':
self.gripper_close()
rospy.sleep(1)
elif pose == 'o':
self.gripper_open()
rospy.sleep(1)
else:
self.model_pose = pose
self.reaching_pose()
if rospy.is_shutdown():
return 0
def main():
rospy.init_node("book_grasp_procedure")
wait_for_time()
# First, load the poses for the fiducial insert movement
sequence = pickle.load( open(INSERT_GRASP_POSES, "rb") )
gripper = fetch_api.Gripper()
arm = fetch_api.Arm()
gripper_open = True
# # Init the reader for the tags
reader = ArTagReader()
sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback)
rospy.sleep(0.5)
# Init the planning scene for collisions
planning_scene = PlanningSceneInterface('base_link')
print "waiting for service...."
rospy.wait_for_service('get_spines')
print "found service!"
get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations)
response = get_spine_poses()
planning_scene.addBox('surface', (response.surface_x_size - 0.17), (response.surface_y_size), response.surface_z_size,
response.surface_pose.position.x, response.surface_pose.position.y, response.surface_pose.position.z)
gripper.open()
gripper_open = True
target_marker_pose = None
check = 0
print "Searching for fiducial...."
while target_marker_pose == None and check < 100:
# If the fiducial was not seen, try again
rospy.sleep(0.1)
check += 1
for marker in reader.markers:
if TARGET_ID == marker.id:
target_marker_pose = marker.pose.pose
if target_marker_pose == None:
print "Failed to find fiducial"
print "Surface position z"
print response.surface_pose.position.z
print "target marker position z"
print target_marker_pose.position.z
# This is the same as the pbd action stuff, not making any changes at the moment
for pbd_pose in sequence:
move_pose = PoseStamped()
move_pose.header.frame_id = 'base_link'
if pbd_pose.frame == 'base_link':
move_pose.pose = pbd_pose.pose
else:
# for marker in reader.markers:
# if TARGET_ID == marker.id:
print "Calculating pose relative to marker...."
# Transform the pose to be in the base_link frame
pose_in_tag_frame = pose_to_transform(pbd_pose.pose)
#tag_in_base_frame = pose_to_transform(marker.pose.pose)
tag_in_base_frame = pose_to_transform(target_marker_pose)
target_matrix = np.dot(tag_in_base_frame, pose_in_tag_frame)
target_pose = transform_to_pose(target_matrix)
move_pose.pose = target_pose
rospy.sleep(1)
err = arm.move_to_pose(move_pose, replan=True)
print "Error in move to pose: ", err
# Check the gripper to open/close
if pbd_pose.gripper_open != gripper_open:
if gripper_open == True:
gripper.close()
gripper_open = False
else:
gripper.open()
gripper_open = True
print "Take this opportunity to change to a different mock point cloud, if necessary"
user_input = raw_input("Press enter to continue")
# After this we calculate the spine_pose closest to the fiducial. I will test that if I can get the service call working
target_fiducial = None
check = 0
print "Searching for fiducial"
while target_fiducial == None and check < 100:
# If the fiducial was not seen, try again
rospy.sleep(0.1)
check += 1
for marker in reader.markers:
if marker.id == TARGET_ID:
target_fiducial = marker
if target_fiducial == None:
print "Failed to find fiducial"
print target_fiducial.id
# Now, we make a request to the perception side of things
spine_poses = []
# code for checking
check = 0
found_good_pose = False
closest_pose = None
print "waiting for service...."
rospy.wait_for_service('get_spines')
print "found service!"
print "Searching for a good grasp pose...."
try:
while check < 20 and found_good_pose == False:
get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations)
response = get_spine_poses()
spine_poses = response.spine_poses
min_dist = float('inf')
for pose in spine_poses:
distance = calculate_euclidean_distance(pose, target_fiducial.pose.pose)
if distance < min_dist:
min_dist = distance
closest_pose = pose
check += 1
if closest_pose.position.x > target_fiducial.pose.pose.position.x and closest_pose.position.y < (target_fiducial.pose.pose.position.y + 0.025) and closest_pose.position.y > (target_fiducial.pose.pose.position.y - 0.025):
found_good_pose = True
# debugging line
# for pose in spine_poses:
# print pose
except rospy.ServiceException, e:
print "Service call failed: %s"%e
class World:
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)
"""Calls a method to display the collision objects.
"""
def create_planning_scene(self):
print "[INFO] Create_planning_scene"
self.display_towers_and_disks()
"""This method collects all needed information and
publish them to other methods.
"""
def display_towers_and_disks(self):
print "[INFO] Display towers and disks"
for tower in self.tower_array:
# call method to publish new tower
self.publish_scene(tower.get_position(), tower)
# set color by name
self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0,
1.0)
# publish color
self.planning_scene_interface.sendColors()
for disk in self.disk_array:
self.publish_scene(disk.get_position(), None, disk)
self.planning_scene_interface.setColor(disk.get_id(),
disk.get_color()[0],
disk.get_color()[1],
disk.get_color()[2],
disk.get_color()[3])
self.planning_scene_interface.sendColors()
# wait for sync after publishing collision objects
self.planning_scene_interface.waitForSync(5.0)
rospy.sleep(5)
"""This method creates a box or a cylinder with methods of
the planning scene interface.
:param position: the position of the new collision object
:param tower: the tower object
:param disk: the disk object
"""
def publish_scene(self, position, tower=None, disk=None):
if tower is not None:
self.planning_scene_interface.addBox(
tower.get_name(), self.max_gripper / 100.0,
self.max_gripper / 100.0,
(self.tower_positions[tower.get_id() - 1][2] * 2), position[0],
position[1], position[2])
else:
self.planning_scene_interface.addCylinder(disk.get_id(),
self.disk_height,
disk.get_diameter() / 2,
position[0], position[1],
position[2])
"""This method cleans the planning scene.
:param close: with this flag a new planning scene objects will be removed
in sync otherwise the object will be deleted without syncing the scene
"""
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
"""This method corrects the position of the moved disk.
:param tower: parent tower of the disk
"""
def refresh_disk_pose(self, tower):
print "[INFO] Refresh disk pose"
disk = tower.get_last()
if self.DEBUG is 1:
print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\
"tower pose", tower.get_position()
# remove the disk from planning scene
self.planning_scene_interface.removeCollisionObject(
disk.get_id(), False)
# publish collision object and set old color
self.publish_scene(disk.get_position(), None, disk)
self.planning_scene_interface.setColor(disk.get_id(),
disk.get_color()[0],
disk.get_color()[1],
disk.get_color()[2],
disk.get_color()[3])
self.planning_scene_interface.sendColors()
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True) #ノードの初期化 reaching ていう名前
self.robot = moveit_commander.RobotCommander() # インスタンスの作成
self.model_pose = [0.0, 0.0, 0.0] #モデルの姿勢
self.obstacle_pose = [0.0, 0.0, 0.0] #障害物の位置
self.subsc_pose = [0.0, 0.0, 0.0] #購読した座標
self.subsc_orientation = [0.0, 0.0, 0.0, 1.0] #購読した四元数
self.set_forbidden() #禁止エリアの初期化
self.set_init_pose() #位置姿勢の初期化
self.before_pose = [0, 0, 0] #前回の場所
self.pcarray = 0 #pointcloud を np array にしたものを入れる
self.executeFlag = False #unity コントローラ B ボタンと対応
self.before_executeFlag = False #前フレームの executeFlag
self.waypoints = []
self.way_flag = False #unity コントローラの 中指のボタンに対応
self.before_wayflag = False #前フレームのway_flag
self.infirst_frame = True #最初のフレーム
self.calc_way_flag = False
self.frame = 0
# def set_ZEDbase(self):
# listener = tf.TransformListener()
# listener.waitForTransform('world','ee_link',rospy.Time(0),rospy.Duration(2.0))
# (self.trans,rot)=listener.lookupTransform('/world','/ee_link',rospy.Time(0)) # trans(x,y,z) rot (x,y,z,w)
# print('trans = '+str(self.trans))
# br = tf.TransformBroadcaster()
# br.sendTransform((self.trans[0],self.trans[1],self.trans[2]),(0.0,0.0,0.0,1.0),rospy.Time.now(),'base_forZED','world')
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface(
"base_link") #PlanningSceneInterface のインスタンスの作成
self.planning_scene.removeCollisionObject("my_ground")
self.planning_scene.addCube("my_ground", 2, 0, 0, -1.0)
self.planning_scene.addBox("obstacle", 0.1, 0.5, 1, -0.3, 0, 0.5)
self.planning_scene.addBox("obstacle2", 0.5, 0.1, 1, 0, -0.3, 0.5)
self.planning_scene.addCube("demo_cube", 0.06, 0, 0.3, 0)
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 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'] #現在フレームの状態
# self.set_ZEDbase()
def callback(self, data): #トピックにデータが追加されるたびに呼ばれる
x = data.pose[1].position.x
y = data.pose[1].position.y
z = data.pose[1].position.z
self.model_pose = [x, y, z] #modelの姿勢を代入
x = data.pose[2].position.x
y = data.pose[2].position.y
z = data.pose[2].position.z
self.obstacle_pose = [x, y, z] #障害物の姿勢を代入
def callbacksub(self, data):
x = data.x
y = data.y
z = data.z
self.subsc_pose = [x, y, z] #購読した座標を代入
def callbackunity(self, data):
x = data.pose.position.x
y = data.pose.position.y
z = data.pose.position.z
self.subsc_pose = [y, -x, z] #unity からの座標 座標変換も行っている
x = data.pose.orientation.x
y = data.pose.orientation.y
z = data.pose.orientation.z
w = data.pose.orientation.w
self.subsc_orientation = [y, -x, z, w] #unity からの四元数
def rtabcallback(self, data):
# dtype_list = [(f.name, np.float32) for f in data.fields]
# cloud_arr = np.fromstring(data.data, dtype_list)
# self.pcarray = np.reshape(cloud_arr, (data.height, data.width))
# print(self.pcarray)
print('rtab subscribed ')
def excallback(self, data):
self.executeFlag = data.data
#print(data.data)
def waycallback(self, data):
self.way_flag = data.data
self.wayflagcalc()
def wayflagcalc(self):
if self.before_wayflag != self.way_flag:
if self.calc_way_flag:
self.calc_way_flag = False
else:
self.calc_way_flag = True
self.before_wayflag = self.way_flag
def start_subscriber(self): #購読をスタート
rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
# /gazebo/model_states から gazebo_msgs/ModelStates 型のトピックを購読 トピックが更新されるたびに self.callback を呼ぶ
#rospy.Subscriber("/UR/reaching/pose",Vector3,self.callbacksub)
rospy.Subscriber("/unity/target", PoseStamped,
self.callbackunity) #unityから配信しているトピックを購読
#rospy.Subscriber('/rtabmap/cloud_map', PointCloud2, self.rtabcallback) #zed で slam したpointcloud を購読
rospy.Subscriber('/unity/execute', Bool, self.excallback)
rospy.Subscriber('/unity/wayflag', Bool, self.waycallback)
rospy.Subscriber('/pcl/near_points', PointCloud2, self.create_object)
def change_state(self, target):
#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 の設置をした場合 target が指定されたら plan_standby に以降
#plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合
set_flag = (self.before_pose != target.position
) # target の位置が変化している = トリガーが押された
#self.calc_way_flag # hold button が押されている
exe_flag = (self.before_executeFlag != self.executeFlag
) # executeFlag が前フレームと違う = execute button が押された
print('set ' + str(set_flag))
print('hold ' + str(self.calc_way_flag))
print('exe ' + str(exe_flag))
print('state ' + str(self.now_state))
if self.now_state == self.state['default']:
if set_flag:
self.now_state = self.state['plan_standby']
elif self.calc_way_flag:
self.now_state = self.state['hold']
elif self.now_state == self.state['hold']:
if not self.calc_way_flag:
self.now_state = self.state['way_standby']
elif self.now_state == self.state['way_standby']:
if set_flag:
self.now_state = self.state['plan_standby']
elif exe_flag:
self.now_state = self.state['default']
elif self.now_state == self.state['plan_standby']:
if exe_flag:
self.now_state = self.state['default']
elif self.calc_way_flag:
self.now_state = self.state['hold']
def create_object(self, data):
self.Points = [data[0:3] for data in pc2.read_points(data)]
def target(self):
rate = rospy.Rate(1) #Rateクラスのインスタンス rateを１で
rospy.sleep(1) #1秒スリープ
useway = False
while not rospy.is_shutdown(): #シャットダウンフラグが立っていなければ、
print self.model_pose #model_poseを表示
group = moveit_commander.MoveGroupCommander(
"manipulator") #MoveGroupCommander クラスのインスタンス
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose #エンドエフェクタの位置姿勢
pose_target = geometry_msgs.msg.Pose(
) #geometry_msgs.pose のインスタンス ここにターゲット設定
pose_target.orientation.x = self.subsc_orientation[0]
pose_target.orientation.y = self.subsc_orientation[1]
pose_target.orientation.z = self.subsc_orientation[2]
pose_target.orientation.w = self.subsc_orientation[
3] #トピックから 四元数を代入
pose_target.position.x = self.subsc_pose[0] #
pose_target.position.y = self.subsc_pose[1] #
pose_target.position.z = self.subsc_pose[2] #トピックから 座標を代入
# br = tf.TransformBroadcaster()
# br.sendTransform((self.trans[0],self.trans[1],self.trans[2]),(0.0,0.0,0.8509035,0.525322),rospy.Time.now(),'base_forZED','world')
# #逐一ブロードキャストする
if self.infirst_frame:
self.before_pose = pose_target.position
self.infirst_frame = False
#self.planning_scene.removeCollisionObject("targetOBJ")
#self.planning_scene.addCube("targetOBJ", 0.005,self.subsc_pose[0] ,self.subsc_pose[1] ,self.subsc_pose[2] )
self.way_flag = False
#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 を使わなかった場合
exe_flag = (self.before_executeFlag != self.executeFlag
) # executeFlag が前フレームと違う = execute button が押された
if self.now_state == self.state['default']:
if self.before_pose != pose_target.position:
group.set_pose_target(pose_target)
print(' Set Target !!!\n')
print(pose_target)
if self.now_state == self.state['hold']:
if self.before_pose != pose_target.position:
self.waypoints.append(copy.deepcopy(pose_target))
print(' Append Target !!!')
if self.now_state == self.state['way_standby']:
plan, fraction = group.compute_cartesian_path(
self.waypoints, 0.01, 0.0)
if self.before_pose != pose_target.position:
group.set_pose_target(pose_target)
print(' Set Target !!!\n')
print(pose_target)
elif exe_flag:
group.execute(plan)
print(' Planning Execute !!!')
print(self.waypoints)
self.waypoints = []
if self.now_state == self.state['plan_standby']:
group.set_pose_target(pose_target)
group.plan()
if self.before_pose != pose_target.position:
group.set_pose_target(pose_target)
print(' Set Target !!!\n')
print(pose_target)
elif exe_flag:
group.go()
print(' Planning Go !!!')
self.change_state(pose_target)
self.before_executeFlag = self.executeFlag
self.before_pose = pose_target.position
self.frame += 1
rospy.sleep(2) #2秒スリープ
