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 __init__(self, planner, robot_state, pre_pushing_pose=None):
super(Pusher, self).__init__()
self.planner = planner
self.collision_objects__markers_pub = rospy.Publisher(
"pusher_trajectory", Marker, tcp_nodelay=True, latch=True)
self.collision_objects_pub = rospy.Publisher("collision_object",
CollisionObject)
self.robot_state = robot_state
self.mover = pr2_control_utilities.PR2JointMover(robot_state)
self.err_msg = ""
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 __init__(self, whicharm):
self.whicharm = whicharm
self.robot_state = pr2_control_utilities.RobotState()
self.joint_controller = pr2_control_utilities.PR2JointMover(
self.robot_state)
self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller)
self.server = InteractiveMarkerServer("~trajectory_markers")
self.tf_listener = self.planner.tf_listener
self.visualizer_pub = rospy.Publisher("~trajectory_markers_path",
MarkerArray)
self.trajectory_pub = rospy.Publisher("~trajectory_poses", PoseArray)
rospy.Subscriber("~overwrite_trajectory", PoseArray,
self.overwrite_trajectory)
rospy.Service("~execute_trajectory", Empty,
self.execute_trajectory_srv)
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/base_link"
int_marker.pose.position.x = 0.5
int_marker.pose.position.z = 1.0
int_marker.name = "move_" + whicharm + "_arm"
int_marker.description = "Move the " + whicharm + " arm"
int_marker.scale = 0.2
self.server.insert(int_marker, self.main_callback)
# create the main marker shape
#color = (1,0,0,1)
color = None
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker)
#add the controls
utils.make_6DOF_marker(int_marker)
self.int_marker = int_marker
self.create_menu()
self.server.applyChanges()
self.trajectory = PoseArray()
self.trajectory.header.frame_id = "/base_link"
self.last_gripper_pose = None
if whicharm == "right":
self.ik_utils = self.planner.right_ik
else:
self.ik_utils = self.planner.left_ik
rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
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)
import roslib
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")
# POSSIBILITY OF SUCH DAMAGE.
# Author: Lorenzo Riano <*****@*****.**> (based on code from Jon Scholz)
import roslib
roslib.load_manifest("pr2_control_utilities")
import rospy
from interpolated_ik_motion_planner import ik_utilities
import pr2_control_utilities
import copy
if __name__ == "__main__":
rospy.init_node('trytest', anonymous=True)
state = pr2_control_utilities.RobotState()
mover = pr2_control_utilities.PR2JointMover(state)
ik = ik_utilities.IKUtilities("right")
angles = state.right_arm_positions
link = "r_wrist_roll_link"
pose = ik.run_fk(angles, link)
newpose = copy.deepcopy(pose)
newpose.pose.position.x += 0.3
newpose.pose.position.z += 0.0
newpose.pose.orientation.x = 0
newpose.pose.orientation.y = 0
newpose.pose.orientation.z = 0
newpose.pose.orientation.w = 1
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()
