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)
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!')
# 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'
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)
# 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)
# 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()
# 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
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)
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()
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()