def main():
"""
Register the FSMLogger with the JoyCallback.
"""
rospy.init_node("log_trypush", anonymous=True)
robot_state = pr2_control_utilities.RobotState()
joint_mover = pr2_control_utilities.PR2JointMover(robot_state)
planner = pr2_control_utilities.PR2MoveArm(joint_mover)
detector = object_detector.ObjectDetector()
base_mover = pr2_control_utilities.PR2BaseMover(planner.tf_listener,
use_controller = True,
use_move_base = False,
use_safety_dist = True,
base_controller_name = "/client/cmd_vel",
)
pusher_l = pushers.LeftArmLateralPusher(planner, robot_state)
pusher_r = pushers.RightArmLateralPusher(planner, robot_state)
obj_discovery_pub = rospy.Publisher("object_discovery", ObjectDiscovery)
fsm_factory = functools.partial(approach_fsm.create_fsm,
base_mover, detector,
joint_mover, pusher_r,
pusher_l, obj_discovery_pub)
fsm_logger = FSMLogger(detector, fsm_factory, joint_mover)
joyaction = joystick_action.JoyAction(3, fsm_logger.execute)
rospy.spin()
def main1():
robot_state = pr2_control_utilities.RobotState()
mover = pr2_control_utilities.PR2JointMover(robot_state)
planner = pr2_control_utilities.PR2MoveArm(mover)
detector = object_detector.ObjectDetector()
pusher_l = pushers.LeftArmCircularPusher(planner, robot_state)
pusher_r = pushers.RightArmCircularPusher(planner, robot_state)
push(robot_state, mover, pusher_l, pusher_r, detector)
def main():
robot_state = pr2_control_utilities.RobotState()
mover = pr2_control_utilities.PR2JointMover(robot_state)
detector = object_detector.ObjectDetector()
grabber = object_pickup.Grabber()
if not detector.search_for_object(mover, trials=15, use_random=True,
max_pan=0.5, min_pan=-0.5,
max_tilt = 1.1, min_tilt = 0.75,):
getout("No object detected by the wide stereo")
grabber.pickup_object(detector.last_collision_processing_msg,
"right_arm", index = 0,
min_approach_distance=0.05,
lift_desired_distance=0.1)
def test():
robot_state = pr2_control_utilities.RobotState()
joint_mover = pr2_control_utilities.PR2JointMover(robot_state)
planner = pr2_control_utilities.PR2MoveArm(joint_mover)
detector = object_detector.ObjectDetector()
base_mover = pr2_control_utilities.PR2BaseMover(planner.tf_listener,
use_controller=True,
use_move_base=False,
use_safety_dist=True)
pusher_l = pushers.LeftArmLateralPusher(planner, robot_state)
pusher_r = pushers.RightArmLateralPusher(planner, robot_state)
obj_discovery_pub = rospy.Publisher("object_discovery", ObjectDiscovery)
fsm = create_fsm(base_mover, detector, joint_mover, pusher_r, pusher_l,
obj_discovery_pub)
fsm.execute()
def main():
robot_state = pr2_control_utilities.RobotState()
joint_mover = pr2_control_utilities.PR2JointMover(robot_state)
planner = pr2_control_utilities.PR2MoveArm(joint_mover)
detector = object_detector.ObjectDetector()
arm = "left"
#arm = "right"
service_name = "/pr2_trajectory_markers_" + arm + "/execute_trajectory"
logger = learn_actions.LogTrajectoryResult(detector,
joint_mover,
service_name,
arm,
planner,
tf_listener=planner.tf_listener)
raw_input("press a key to start...")
logger.publish_object_changes()
def main():
robot_state = pr2_control_utilities.RobotState()
joint_mover = pr2_control_utilities.PR2JointMover(robot_state)
planner = pr2_control_utilities.PR2MoveArm(joint_mover)
detector = object_detector.ObjectDetector()
pusher_l = pushers.LeftArmLateralPusher(planner, robot_state)
pusher_r = pushers.RightArmLateralPusher(planner, robot_state)
base_mover = pr2_control_utilities.PR2BaseMover(planner.tf_listener,
use_controller=True,
use_move_base=False,
use_safety_dist=True)
logger = learn_actions.LogPushingResult(detector,
pusher_l,
pusher_r,
joint_mover,
base_mover,
tf_listener=planner.tf_listener)
joyaction = learn_actions.JoyAction(
3, logger.search_and_detect_changes_publisher)
roslib.load_manifest(PKG)
import rospy
import pr2_control_utilities
import tabletop_actions.object_detector as object_detector
import tf
from tf import transformations
import sys
if __name__ == "__main__":
rospy.init_node(PKG, anonymous=True)
robot_state = pr2_control_utilities.RobotState()
joint_mover = pr2_control_utilities.PR2JointMover(robot_state)
detector = object_detector.ObjectDetector()
if not detector.search_for_object(
joint_mover,
trials=15,
cluster_choser="find_closest_cluster",
max_pan=0.5,
min_pan=-0.5,
max_tilt=1.1,
min_tilt=0.6,
):
rospy.logerr("No object detected by the stereo")
else:
rospy.loginfo("Object detected")
table = detector.last_detection_msg.detection.table
def main():
rospy.init_node(PKG, anonymous=True)
ik = pr2_control_utilities.IKUtilities("right")
robot_state = pr2_control_utilities.RobotState()
mover = pr2_control_utilities.PR2JointMover(robot_state)
planner = pr2_control_utilities.PR2MoveArm(ik, mover)
detector = object_detector.ObjectDetector()
collision_objects_pub = rospy.Publisher("collision_object",
CollisionObject)
initial_head = robot_state.head_positions[:]
mover.time_to_reach = 1
if not detector.point_head_at(mover):
getout("No object detected by the wide stereo")
box = detector.detect_bounding_box()
if box is None:
getout("No box found for pre-pushing!")
primary_axis_prepush = find_primary_axis(box)
prev_box_position = (box.pose.pose.position.x, box.pose.pose.position.y,
box.pose.pose.position.z)
rospy.loginfo("Box pose before pushing: %s" % str(prev_box_position))
rospy.loginfo("Primary axis before pushing: %s" %
str(primary_axis_prepush))
#adding the object
object_id = "graspable_bottle"
collision_object_msg = utils.build_object_from_box(box, object_id)
collision_objects_pub.publish(collision_object_msg)
draw_object(object_id)
frame = box.pose.header.frame_id
traj_poses = get_circular_pushing_poses(box, 10, z_offset=0.2)
draw_poses(traj_poses, frame)
starting_angles, end_angles = gripper_down_horizontal_angles()
collision_operation = utils.build_collision_operations(
"right_arm", object_id)
allowed_contact_specification = utils.build_allowed_contact_specification(
box)
previous_pose = pr2_control_utilities.PR2JointMover(robot_state)
previous_pose.store_targets()
#moving to pre_push position
mover.close_right_gripper(True)
start_push = traj_poses[0]
if planner.move_right_arm(
start_push,
starting_angles,
frame,
waiting_time=30,
ordered_collision_operations=collision_operation):
rospy.loginfo("Planning ok")
else:
getout("Planning not done")
whole_angles = [starting_angles] * len(traj_poses)
planner.joint_mover.time_to_reach = 5.0
mover.set_head_state(initial_head)
if planner.move_right_arm_trajectory_non_collision(
traj_poses,
whole_angles,
frame,
max_vel=0.4,
ignore_errors=False,
):
rospy.loginfo("Pushing ok")
else:
getout("Pushing not done")
rospy.loginfo("Moving back to the original place")
previous_pose.time_to_reach = 10
previous_pose.execute_and_wait()
head_aim = (traj_poses[-1][0], traj_poses[-1][1], traj_poses[-1][2] - 0.4)
previous_pose.point_head_to(head_aim, frame)
if not detector.point_head_at(mover):
getout("No object detected by the wide stereo")
box = detector.detect_bounding_box()
primary_axis_prepush = find_primary_axis(box)
rospy.loginfo("Primary axis after pushing: %s" % str(primary_axis_prepush))
next_box_position = (box.pose.pose.position.x, box.pose.pose.position.y,
box.pose.pose.position.z)
dist = math.sqrt((next_box_position[0] - prev_box_position[0])**2 +
(next_box_position[1] - prev_box_position[1])**2 +
(next_box_position[2] - prev_box_position[2])**2)
rospy.loginfo("Distance travelled: %f" % dist)
rospy.loginfo("Done")
rospy.spin()