예제 #1
0
 def __init__(self):
     self.arm_planner = ArmPlanner('right_arm')
     self.arm_mover = ArmMover()
     self.base_plan_service = rospy.ServiceProxy('/move_base/make_plan',
                                                 GetPlan)
     self.psi = get_planning_scene_interface()
     self.viz_pub = rospy.Publisher('lower_bound', MarkerArray)
     self.pose_pub = rospy.Publisher('/initialpose',
                                     PoseWithCovarianceStamped)
예제 #2
0
 def __init__(self):
     arms = ['right_arm', 'left_arm']
     self.grippers = {}
     self.arm_planners = {}
     for arm in arms:
         self.grippers[arm] = Gripper(arm)
         self.arm_planners[arm] = ArmPlanner(arm)
     self.arm_mover = ArmMover()
     self.arm_tasks = ArmTasks()
     self.base = Base()
     self.wi = WorldInterface()
     self.psi = get_planning_scene_interface()
     self.base_action = SimpleActionClient('base_trajectory_action',
                                           BaseTrajectoryAction)
     rospy.loginfo('Waiting for base trajectory action')
     self.base_action.wait_for_server()
     rospy.loginfo('Found base trajectory action')
     rospy.loginfo('DARRT trajectory executor successfully initialized!')
예제 #3
0
# 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.
#
# Author: Sachin Chitta, Jon Binney

import roslib; 
roslib.load_manifest('pr2_python')
import rospy

from geometry_msgs.msg import PoseStamped
from pr2_python.arm_mover import ArmMover

# initialize
rospy.init_node('is_in_collision', anonymous=True)
right_arm = ArmMover('right_arm')

if right_arm.is_current_state_in_collision():
    print 'Right arm in collision'
else:
    print 'Right arm not in collision'

js = right_arm.get_joint_state()
js.position[0] = 1.0 #set shoulder pan (first joint) to swing into left arm

if right_arm.is_in_collision(js):
    print 'Right arm in collision'
else:
    print 'Right arm not in collision'
예제 #4
0
class Executor:
    def __init__(self):
        arms = ['right_arm', 'left_arm']
        self.grippers = {}
        self.arm_planners = {}
        for arm in arms:
            self.grippers[arm] = Gripper(arm)
            self.arm_planners[arm] = ArmPlanner(arm)
        self.arm_mover = ArmMover()
        self.arm_tasks = ArmTasks()
        self.base = Base()
        self.wi = WorldInterface()
        self.psi = get_planning_scene_interface()
        self.base_action = SimpleActionClient('base_trajectory_action',
                                              BaseTrajectoryAction)
        rospy.loginfo('Waiting for base trajectory action')
        self.base_action.wait_for_server()
        rospy.loginfo('Found base trajectory action')
        rospy.loginfo('DARRT trajectory executor successfully initialized!')

    def execute_trajectories(self, result):
        rospy.loginfo('There are ' + str(len(result.primitive_names)) +
                      ' segments')
        for (i, t) in enumerate(result.primitive_types):
            rospy.loginfo('Executing trajectory ' + str(i) + ' of type ' + t +
                          ' and length ' + str(
                              len(result.primitive_trajectories[i].
                                  joint_trajectory.points)))
            #print 'Trajectory is\n', result.primitive_trajectories[i]
            splits = t.split('-')
            if splits[0] == 'BaseTransit':
                self.execute_base_trajectory(result.primitive_trajectories[i])
                continue
            if splits[0] == 'BaseWarp':
                self.arm_tasks.move_arm_to_side('right_arm')
                self.arm_tasks.move_arm_to_side('left_arm')
                self.execute_warp_trajectory(result.primitive_trajectories[i])
                continue
            if splits[0] == 'ArmTransit':
                self.execute_arm_trajectory(splits[1],
                                            result.primitive_trajectories[i])
                continue
            #if splits[0] == 'Approach':
            #gripper.open()
            if splits[0] == 'Pickup':
                try:
                    self.grippers[splits[1]].close()
                except ActionFailedError:
                    pass
                self.wi.attach_object_to_gripper(splits[1], splits[2])
                #so the trajectory filter doesn't complain
                ops = OrderedCollisionOperations()
                op = CollisionOperation()
                op.operation = op.DISABLE

                op.object2 = splits[2]
                for t in self.wi.hands[splits[1]].touch_links:
                    op.object1 = t
                    ops.collision_operations.append(copy.deepcopy(op))
                self.psi.add_ordered_collisions(ops)
            self.execute_straight_line_arm_trajectory(
                splits[1], result.primitive_trajectories[i])
            if splits[0] == 'Place':
                self.grippers[splits[1]].open()
                self.wi.detach_and_add_back_attached_object(
                    splits[1], splits[2])
                self.psi.reset()

    def execute_warp_trajectory(self, trajectory):
        #figure out the last point on the trajectory
        tp = trajectory.multi_dof_joint_trajectory.points[-1]
        (phi, theta, psi) = gt.quaternion_to_euler(tp.poses[0].orientation)
        self.base.move_to(tp.poses[0].position.x, tp.poses[0].position.y, psi)

    def execute_base_trajectory(self, trajectory):
        goal = BaseTrajectoryGoal()
        #be a little slower and more precise
        goal.linear_velocity = 0.2
        goal.angular_velocity = np.pi / 8.0
        goal.linear_error = 0.01
        goal.angular_error = 0.01
        joint = -1
        for (i, n) in enumerate(
                trajectory.multi_dof_joint_trajectory.joint_names):
            if n == 'world_joint' or n == '/world_joint':
                goal.world_frame = trajectory.multi_dof_joint_trajectory.frame_ids[
                    i]
                goal.robot_frame = trajectory.multi_dof_joint_trajectory.child_frame_ids[
                    i]
                joint = i
                break
        if joint < 0:
            raise ActionFailedError('No world joint found in base trajectory')
        for p in trajectory.multi_dof_joint_trajectory.points:
            (phi, theta,
             psi) = gt.quaternion_to_euler(p.poses[joint].orientation)
            goal.trajectory.append(
                Pose2D(x=p.poses[joint].position.x,
                       y=p.poses[joint].position.y,
                       theta=psi))
        self.base_action.send_goal_and_wait(goal)

    def execute_arm_trajectory(self, arm_name, trajectory):
        arm_trajectory = self.arm_planners[arm_name].joint_trajectory(
            trajectory.joint_trajectory)
        arm_trajectory.header.stamp = rospy.Time.now()
        #try:
        #    arm_trajectory = self.arm_planners[arm_name].filter_trajectory(arm_trajectory)
        #except ArmNavError, e:
        #rospy.logwarn('Trajectory filter failed with error '+str(e)+'.  Executing anyway')
        arm_trajectory = tt.convert_path_to_trajectory(arm_trajectory,
                                                       time_per_pt=0.35)
        self.arm_mover.execute_joint_trajectory(arm_name, arm_trajectory)

    def execute_straight_line_arm_trajectory(self, arm_name, trajectory):
        arm_trajectory = self.arm_planners[arm_name].joint_trajectory(
            trajectory.joint_trajectory)
        arm_trajectory.header.stamp = rospy.Time.now()
        arm_trajectory = tt.convert_path_to_trajectory(arm_trajectory,
                                                       time_per_pt=0.4)
        self.arm_mover.execute_joint_trajectory(arm_name, arm_trajectory)
예제 #5
0
# 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.
#
# Author: Jon Binney

import roslib; 
roslib.load_manifest('pr2_python')
import rospy

from geometry_msgs.msg import PoseStamped

from pr2_python.arm_mover import ArmMover

rospy.init_node('test_arm_control', anonymous=True)
arm_mover = ArmMover()

# Tell the left arm to move to a cartesian goal
goal_pose = PoseStamped()
goal_pose.pose.position.x = 0.65
goal_pose.pose.position.y = 0.2
goal_pose.pose.orientation.w = 1.0
goal_pose.header.frame_id = '/torso_lift_link'
handle1 = arm_mover.move_to_goal('left_arm', goal_pose, blocking=False)

# Tell the right arm to move to a joint space goal
joint_position = [-0.817, 0.001, -1.253, -0.892, 60.854, -0.250, 3.338]
js = arm_mover.get_joint_state('right_arm')
js.position = joint_position
handle2 = arm_mover.move_to_goal('right_arm', js, blocking=False)
예제 #6
0
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Jon Binney

import roslib; 
roslib.load_manifest('pr2_python')
import rospy

from geometry_msgs.msg import PoseStamped

from pr2_python.arm_mover import ArmMover

arm_name = 'right_arm'

# initialize
rospy.init_node('test_arm_control', anonymous=True)
arm_mover = ArmMover()

# generate desired arm pose
goal_pose = PoseStamped()
goal_pose.pose.position.x = 0.65
goal_pose.pose.orientation.w = 1.0
goal_pose.header.frame_id = '/torso_lift_link'

handle = arm_mover.move_to_goal(arm_name, goal_pose)
if handle.reached_goal():
    print 'Reached the goal!'
else:
    print handle.get_errors()

예제 #7
0
# 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.
#
# Author: Jon Binney

import roslib
roslib.load_manifest('pr2_python')
import rospy

from pr2_python.arm_mover import ArmMover

# initialize
rospy.init_node('test_arm_control', anonymous=True)
arm_mover = ArmMover()

# parameters
arm_name = 'right_arm'
joint_position = [-0.817, 0.001, -1.253, -0.892, 60.854, -0.250, 3.338]

# get a joint state message already configured for this arm
js = arm_mover.get_joint_state(arm_name)

rospy.sleep(1.0)

# set desired joint positions
js.position = joint_position
print 'Moving to %s' % (str(joint_position))

# send out the command
예제 #8
0
class Planner:
    def __init__(self):
        self.arm_planner = ArmPlanner('right_arm')
        self.arm_mover = ArmMover()
        self.base_plan_service = rospy.ServiceProxy('/move_base/make_plan',
                                                    GetPlan)
        self.psi = get_planning_scene_interface()
        self.viz_pub = rospy.Publisher('lower_bound', MarkerArray)
        self.pose_pub = rospy.Publisher('/initialpose',
                                        PoseWithCovarianceStamped)
        #self.base = Base()

    def publish(self, markers):
        for i in range(50):
            self.viz_pub.publish(markers)
            rospy.sleep(0.01)

    def plan_base(self, goal):
        #find the current base pose from the planning scene
        robot_state = self.psi.get_robot_state()
        starting_state = PoseStamped()
        starting_state.header.frame_id = robot_state.multi_dof_joint_state.frame_ids[
            0]
        starting_state.pose = robot_state.multi_dof_joint_state.poses[0]

        goal_robot_state = copy.deepcopy(robot_state)
        goal_robot_state.multi_dof_joint_state.poses[0] = goal.pose

        start_markers = vt.robot_marker(robot_state, ns='base_starting_state')
        goal_markers = vt.robot_marker(goal_robot_state,
                                       ns='base_goal_state',
                                       g=1,
                                       b=0)
        self.publish(start_markers)
        self.publish(goal_markers)

        #plan
        good_plan = False
        while not good_plan:
            try:
                start_time = time.time()
                self.base_plan_service(start=starting_state, goal=goal)
                end_time = time.time()
                good_plan = True
            except:
                rospy.loginfo(
                    'Having trouble probably with starting state.  Sleeping and trying again'
                )
                rospy.sleep(0.2)

        #it's apparently better just to do the trajectory
        #than fool with the planning scene
        goal_cov = PoseWithCovarianceStamped()
        goal_cov.header = goal.header
        goal_cov.pose.pose = goal.pose
        self.pose_pub.publish(goal_cov)
        self.psi.reset()

        # self.base.move_to(goal.pose.position.x, goal.pose.position.y,
        #                   2.0*np.arcsin(goal.pose.orientation.z))

        return (end_time - start_time)

    def plan_arm(self, goal, ordered_colls=None, interpolated=False):
        time = 0
        robot_state = self.psi.get_robot_state()
        ending_robot_state = copy.deepcopy(robot_state)

        start_markers = vt.robot_marker(robot_state, ns='arm_starting_state')
        self.publish(start_markers)

        marray = MarkerArray()
        marker = vt.marker_at(goal, ns="arm_goal_pose")
        marray.markers.append(marker)
        self.publish(marray)

        if not interpolated:
            traj, time = self.arm_planner.plan_collision_free(
                goal, ordered_collisions=ordered_colls, runtime=True)
        else:
            goal_bl = self.psi.transform_pose_stamped('base_link', goal)
            marray = MarkerArray()
            marker = vt.marker_at(goal_bl, ns="arm_goal_pose_base_link")
            marray.markers.append(marker)
            self.publish(marray)

            traj, time = self.arm_planner.plan_interpolated_ik(
                goal_bl,
                ordered_collisions=ordered_colls,
                consistent_angle=2.0 * np.pi,
                runtime=True)

        joint_state = tt.joint_trajectory_point_to_joint_state(
            traj.points[-1], traj.joint_names)
        ending_robot_state = tt.set_joint_state_in_robot_state(
            joint_state, ending_robot_state)

        goal_markers = vt.robot_marker(ending_robot_state,
                                       ns='arm_goal_state',
                                       g=1,
                                       b=0)
        self.publish(goal_markers)

        #do the trajectory
        # traj.points = [traj.points[0], traj.points[1], traj.points[-1]]
        # traj.header.stamp = rospy.Time.now()
        # traj.points[-1].time_from_start = 3
        #rospy.loginfo('Trajectory =\n'+str(traj))
        self.arm_mover.execute_joint_trajectory('right_arm', traj)
        #self.psi.set_robot_state(ending_robot_state)

        return time

    def plan_arm_interpolated(self, goal, ordered_colls=None):
        return self.plan_arm(goal,
                             ordered_colls=ordered_colls,
                             interpolated=True)
예제 #9
0
파일: place.py 프로젝트: rll/sushichallenge
 def __init__(self, arm_name):
     self.arm_name = arm_name
     self.mover = ArmMover(arm_name)
     self.planner = ArmPlanner(arm_name)
     self.gripper = Gripper(arm_name)
     self.base = base.Base()
예제 #10
0
파일: place.py 프로젝트: rll/sushichallenge
class SensingPlacer:
    def __init__(self, arm_name):
        self.arm_name = arm_name
        self.mover = ArmMover(arm_name)
        self.planner = ArmPlanner(arm_name)
        self.gripper = Gripper(arm_name)
        self.base = base.Base()
        #self.gripper.close()

    def place(self, x, y, z):
        self.base.move_manipulable_pose(x, y, z, 
            try_hard = True, group=self.arm_name)
        world_pose = Pose()
        world_pose.position.x = x
        world_pose.position.y = y
        world_pose.position.z = z
        world_pose.orientation.x = 0.0
        world_pose.orientation.y = 0.0
        world_pose.orientation.z = 0.0
        world_pose.orientation.w = 1.0
       
        new_pose = transform_pose('base_footprint', 'map', world_pose)
        hand_pose = self.planner.get_hand_frame_pose()
        #print hand_pose
        #print new_pose
        hand_pose.pose.position.x = new_pose.position.x
        hand_pose.pose.position.y = new_pose.position.y
        hand_pose.pose.position.z = new_pose.position.z + 0.1

        self
        if self.arm_name == 'right_arm':
            joint_position = [-1.254, -0.325, -1.064, -1.525, 0.109, -1.185, 0.758]
        else:
            joint_position = [1.191, -0.295, 0.874, -1.610, 0.048, -1.069, -.988]
        joint_state = self.mover.get_joint_state()
        joint_state.position = joint_position
        self.mover.move_to_goal(joint_state)
        self.mover.move_to_goal_directly(hand_pose,collision_aware=False)
        #print "********"
        #print self.mover.get_exceptions()
        #print "********"

        still_in_free_space = True
        hand_pose = self.planner.get_hand_frame_pose()
        while still_in_free_space:
            goal_z = hand_pose.pose.position.z - 0.03
            hand_pose.pose.position.z = goal_z
            self.mover.move_to_goal_directly(hand_pose,collision_aware=False)
            hand_pose = self.planner.get_hand_frame_pose()
            if abs(hand_pose.pose.position.z - goal_z) > 0.01:
                still_in_free_space = False
            #print self.mover.reached_goal()
            #if not self.mover.reached_goal():
            #    still_in_free_space = False

        self.gripper.open()
        if self.arm_name == 'right_arm':
            gripper_name = 'r_gripper_palm_link'
        else:
            gripper_name = 'l_gripper_palm_link'
        gripper_pose = transform_pose_stamped(gripper_name, hand_pose)
        gripper_pose.pose.position.x -= 0.20
        hand_pose = transform_pose_stamped(hand_pose.header.frame_id, gripper_pose)
        self.mover.move_to_goal_directly(hand_pose, collision_aware=False)
        self.mover.move_to_goal(joint_state)
#
# Author: Jon Binney

import roslib; 
roslib.load_manifest('pr2_python')
import rospy

from geometry_msgs.msg import PoseStamped

from pr2_python.arm_mover import ArmMover

arm_name = 'right_arm'

# initialize
rospy.init_node('test_arm_control', anonymous=True)
arm_mover = ArmMover()

# generate desired arm pose
goal_pose = PoseStamped()
goal_pose.pose.position.x = 0.65
goal_pose.pose.orientation.w = 1.0
goal_pose.header.frame_id = '/torso_lift_link'
goal_pose.header.stamp = rospy.Time(0)

handle = arm_mover.move_to_goal_using_cartesian_control(arm_name, goal_pose, timeout=5.0, blocking=True)
if handle.reached_goal():
    print 'Reached the goal!'
else:
    print handle.get_errors()