00001
00002
00003 import numpy as np
00004
00005 import roslib; roslib.load_manifest('assistive_teleop')
00006 import rospy
00007 import actionlib
00008 from std_msgs.msg import String
00009 from geometry_msgs.msg import PoseStamped, Quaternion, Point
00010 from arm_navigation_msgs.msg import (MoveArmAction, MoveArmGoal,
00011 PositionConstraint, OrientationConstraint)
00012 from kinematics_msgs.srv import (GetKinematicSolverInfo, GetPositionFK,
00013 GetPositionFKRequest, GetPositionIK, GetPositionIKRequest)
00014 from pr2_controllers_msgs.msg import (JointTrajectoryAction,
00015 JointTrajectoryControllerState, SingleJointPositionAction,
00016 SingleJointPositionGoal, Pr2GripperCommandAction,
00017 Pr2GripperCommandGoal, JointTrajectoryGoal)
00018 from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
00019 from control_msgs.msg import (FollowJointTrajectoryAction,
00020 FollowJointTrajectoryGoal, JointTolerance)
00021 from arm_navigation_msgs.srv import (SetPlanningSceneDiff,
00022 SetPlanningSceneDiffRequest)
00023 from tf import TransformListener, transformations
00024
00025 import pose_utils as pu
00026
00027 class PR2Arm():
00028
00029 wipe_started = False
00030 standoff = 0.368
00031 torso_min = 0.001
00032 torso_max = 0.299
00033 dist = 0.
00034
00035 move_arm_error_dict = {
00036 -1 : "PLANNING_FAILED: Could not plan a clear path to goal.",
00037 0 : "Succeeded [0]",
00038 1 : "Succeeded [1]",
00039 -2 : "TIMED_OUT",
00040 -3 : "START_STATE_IN_COLLISION: Try freeing the arms manually.",
00041 -4 : "START_STATE_VIOLATES_PATH_CONSTRAINTS",
00042 -5 : "GOAL_IN_COLLISION",
00043 -6 : "GOAL_VIOLATES_PATH_CONSTRAINTS",
00044 -7 : "INVALID_ROBOT_STATE",
00045 -8 : "INCOMPLETE_ROBOT_STATE",
00046 -9 : "INVALID_PLANNER_ID",
00047 -10 : "INVALID_NUM_PLANNING_ATTEMPTS",
00048 -11 : "INVALID_ALLOWED_PLANNING_TIME",
00049 -12 : "INVALID_GROUP_NAME",
00050 -13 : "INVALID_GOAL_JOINT_CONSTRAINTS",
00051 -14 : "INVALID_GOAL_POSITION_CONSTRAINTS",
00052 -15 : "INVALID_GOAL_ORIENTATION_CONSTRAINTS",
00053 -16 : "INVALID_PATH_JOINT_CONSTRAINTS",
00054 -17 : "INVALID_PATH_POSITION_CONSTRAINTS",
00055 -18 : "INVALID_PATH_ORIENTATION_CONSTRAINTS",
00056 -19 : "INVALID_TRAJECTORY",
00057 -20 : "INVALID_INDEX",
00058 -21 : "JOINT_LIMITS_VIOLATED",
00059 -22 : "PATH_CONSTRAINTS_VIOLATED",
00060 -23 : "COLLISION_CONSTRAINTS_VIOLATED",
00061 -24 : "GOAL_CONSTRAINTS_VIOLATED",
00062 -25 : "JOINTS_NOT_MOVING",
00063 -26 : "TRAJECTORY_CONTROLLER_FAILED",
00064 -27 : "FRAME_TRANSFORM_FAILURE",
00065 -28 : "COLLISION_CHECKING_UNAVAILABLE",
00066 -29 : "ROBOT_STATE_STALE",
00067 -30 : "SENSOR_INFO_STALE",
00068 -31 : "NO_IK_SOLUTION: Cannot reach goal configuration.",
00069 -32 : "INVALID_LINK_NAME",
00070 -33 : "IK_LINK_IN_COLLISION: Cannot reach goal configuration\
00071 without contact.",
00072 -34 : "NO_FK_SOLUTION",
00073 -35 : "KINEMATICS_STATE_IN_COLLISION",
00074 -36 : "INVALID_TIMEOUT"
00075 }
00076
00077 def __init__(self, arm, tfListener=None):
00078 self.arm = arm
00079 if not (self.arm == "right" or self.arm=="left"):
00080 rospy.logerr("Failed to initialize PR2Arm: \
00081 Must pass 'right' or 'left'")
00082 return
00083 if tfListener is None:
00084 self.tf = TransformListener()
00085 else:
00086 self.tf = tfListener
00087
00088 self.arm_traj_client = actionlib.SimpleActionClient(
00089 self.arm[0]+
00090 '_arm_controller/joint_trajectory_action',
00091 JointTrajectoryAction)
00092 rospy.loginfo("Waiting for " + self.arm[0] +
00093 "_arm_controller/joint_trajectory_action server")
00094 if self.arm_traj_client.wait_for_server(rospy.Duration(5)):
00095 rospy.loginfo("Found "+self.arm[0]+
00096 "_arm_controller/joint_trajectory_action server")
00097 else:
00098 rospy.logwarn("Cannot find " + self.arm[0] +
00099 " _arm_controller/joint_trajectory_action server")
00100
00101 self.arm_follow_traj_client = actionlib.SimpleActionClient(self.arm[0]+
00102 '_arm_controller/follow_joint_trajectory',
00103 FollowJointTrajectoryAction)
00104 rospy.loginfo("Waiting for "+self.arm[0]+
00105 "_arm_controller/follow_joint_trajectory server")
00106 if self.arm_follow_traj_client.wait_for_server(rospy.Duration(5)):
00107 rospy.loginfo("Found "+self.arm[0]+
00108 "_arm_controller/follow_joint_trajectory server")
00109 else:
00110 rospy.logwarn("Cannot find "+self.arm[0]+
00111 "_arm_controller/follow_joint_trajectory server")
00112
00113 self.torso_client = actionlib.SimpleActionClient(
00114 'torso_controller/position_joint_action',
00115 SingleJointPositionAction)
00116 rospy.loginfo("Waiting for torso_controller/position_joint_action \
00117 server")
00118 if self.torso_client.wait_for_server(rospy.Duration(5)):
00119 rospy.loginfo("Found torso_controller/position_joint_action server")
00120 else:
00121 rospy.logwarn("Cannot find torso_controller/position_joint_action \
00122 server")
00123
00124 self.gripper_client = actionlib.SimpleActionClient(
00125 self.arm[0]+'_gripper_controller/gripper_action',
00126 Pr2GripperCommandAction)
00127 rospy.loginfo("Waiting for "+self.arm[0]+
00128 "_gripper_controller/gripper_action server")
00129 if self.gripper_client.wait_for_server(rospy.Duration(5)):
00130 rospy.loginfo("Found "+self.arm[0]+
00131 "_gripper_controller/gripper_action server")
00132 else:
00133 rospy.logwarn("Cannot find "+self.arm[0]+
00134 "_gripper_controller/gripper_action server")
00135
00136 rospy.Subscriber(self.arm[0]+'_arm_controller/state',
00137 JointTrajectoryControllerState, self.update_joint_state)
00138 rospy.Subscriber('torso_controller/state',
00139 JointTrajectoryControllerState, self.update_torso_state)
00140
00141 self.log_out = rospy.Publisher(rospy.get_name()+'/log_out', String)
00142
00143 rospy.loginfo("Waiting for "+self.arm.capitalize()+" FK/IK Solver services")
00144 try:
00145 rospy.wait_for_service("/pr2_"+self.arm+"_arm_kinematics/get_fk")
00146 rospy.wait_for_service("/pr2_"+self.arm+
00147 "_arm_kinematics/get_fk_solver_info")
00148 rospy.wait_for_service("/pr2_"+self.arm+"_arm_kinematics/get_ik")
00149 rospy.wait_for_service("/pr2_"+self.arm+
00150 "_arm_kinematics/get_ik_solver_info")
00151 self.fk_info_proxy = rospy.ServiceProxy(
00152 "/pr2_"+self.arm+
00153 "_arm_kinematics/get_fk_solver_info",
00154 GetKinematicSolverInfo)
00155 self.fk_info = self.fk_info_proxy()
00156 self.fk_pose_proxy = rospy.ServiceProxy(
00157 "/pr2_"+self.arm+"_arm_kinematics/get_fk",
00158 GetPositionFK, True)
00159 self.ik_info_proxy = rospy.ServiceProxy(
00160 "/pr2_"+self.arm+
00161 "_arm_kinematics/get_ik_solver_info",
00162 GetKinematicSolverInfo)
00163 self.ik_info = self.ik_info_proxy()
00164 self.ik_pose_proxy = rospy.ServiceProxy(
00165 "/pr2_"+self.arm+"_arm_kinematics/get_ik",
00166 GetPositionIK, True)
00167 rospy.loginfo("Found FK/IK Solver services")
00168 except:
00169 rospy.logerr("Could not find FK/IK Solver services")
00170
00171 self.set_planning_scene_diff_name= \
00172 '/environment_server/set_planning_scene_diff'
00173 rospy.wait_for_service('/environment_server/set_planning_scene_diff')
00174 self.set_planning_scene_diff = rospy.ServiceProxy(
00175 '/environment_server/set_planning_scene_diff',
00176 SetPlanningSceneDiff)
00177 self.set_planning_scene_diff(SetPlanningSceneDiffRequest())
00178
00179 self.test_pose = rospy.Publisher("test_pose", PoseStamped)
00180
00181 def update_joint_state(self, jtcs):
00182 self.joint_state_time = jtcs.header.stamp
00183 self.joint_state_act = jtcs.actual
00184 self.joint_state_des = jtcs.desired
00185
00186 def update_torso_state(self, jtcs):
00187 self.torso_state = jtcs
00188
00189 def curr_pose(self):
00190 (trans,rot) = self.tf.lookupTransform("/torso_lift_link",
00191 "/"+self.arm[0]+"_wrist_roll_link",
00192 rospy.Time(0))
00193 cp = PoseStamped()
00194 cp.header.stamp = rospy.Time.now()
00195 cp.header.frame_id = '/torso_lift_link'
00196 cp.pose.position = Point(*trans)
00197 cp.pose.orientation = Quaternion(*rot)
00198 return cp
00199
00200 def wait_for_stop(self, wait_time=1, timeout=60):
00201 rospy.sleep(wait_time)
00202 end_time = rospy.Time.now()+rospy.Duration(timeout)
00203 while not self.is_stopped():
00204 if rospy.Time.now()<end_time:
00205 rospy.sleep(wait_time)
00206 else:
00207 raise
00208
00209 def is_stopped(self):
00210 time = self.joint_state_time
00211 vels = self.joint_state_act.velocities
00212 if (np.allclose(vels, np.zeros(7)) and
00213 (rospy.Time.now()-time)<rospy.Duration(0.1)):
00214 return True
00215 else:
00216 return False
00217
00218 def adjust_elbow(self, f32):
00219 request = self.form_ik_request(self.curr_pose())
00220 angs = list(request.ik_request.ik_seed_state.joint_state.position)
00221 angs[2] += f32.data
00222 request.ik_request.ik_seed_state.joint_state.position = angs
00223 ik_goal = self.ik_pose_proxy(request)
00224 if ik_goal.error_code.val == 1:
00225 self.send_joint_angles(ik_goal.solution.joint_state.position)
00226 else:
00227 rospy.loginfo("Cannot adjust elbow position further")
00228 self.log_out.publish(data="Cannot adjust elbow position further")
00229
00230 def gripper(self, position, effort=30):
00231 pos = np.clip(position,0.0, 0.09)
00232 goal = Pr2GripperCommandGoal()
00233 goal.command.position = pos
00234 goal.command.max_effort = effort
00235 self.gripper_client.send_goal(goal)
00236 finished_within_time = self.gripper_client.wait_for_result(
00237 rospy.Duration(15))
00238 if not (finished_within_time):
00239 self.gripper_client.cancel_goal()
00240 rospy.loginfo("Timed out moving "+self.arm+" gripper")
00241 return False
00242 else:
00243 state = self.gripper_client.get_state()
00244 result = self.gripper_client.get_result()
00245 if (state == 3):
00246 rospy.loginfo("Gripper Action Succeeded")
00247 return True
00248 else:
00249 rospy.loginfo("Gripper Action Failed")
00250 rospy.loginfo("Failure Result: %s" %result)
00251 return False
00252
00253 def hand_frame_move(self, x, y=0, z=0, duration=None):
00254 cp = self.curr_pose()
00255 newpose = pu.pose_relative_trans(cp,x,y,z)
00256 self.dist = pu.calc_dist(cp, newpose)
00257 return newpose
00258
00259 def build_trajectory(self, finish, start=None, ik_space=0.005,
00260 duration=None, tot_points=None):
00261 if start == None:
00262 start = self.curr_pose()
00263
00264
00265
00266
00267
00268
00269 dist = pu.calc_dist(start, finish)
00270 ik_steps = int(round(dist/ik_space)+1.)
00271 print "IK Steps: %s" %ik_steps
00272 if tot_points is None:
00273 tot_points = 1500*dist
00274 if duration is None:
00275 duration = dist*120
00276 ik_fracs = np.linspace(0, 1, ik_steps)
00277 ang_fracs = np.linspace(0,1, tot_points)
00278
00279 x_gap = finish.pose.position.x - start.pose.position.x
00280 y_gap = finish.pose.position.y - start.pose.position.y
00281 z_gap = finish.pose.position.z - start.pose.position.z
00282
00283 qs = [start.pose.orientation.x, start.pose.orientation.y,
00284 start.pose.orientation.z, start.pose.orientation.w]
00285 qf = [finish.pose.orientation.x, finish.pose.orientation.y,
00286 finish.pose.orientation.z, finish.pose.orientation.w]
00287
00288
00289 steps = [PoseStamped() for i in range(len(ik_fracs))]
00290 for i,frac in enumerate(ik_fracs):
00291 steps[i].header.stamp = rospy.Time.now()
00292 steps[i].header.frame_id = start.header.frame_id
00293 steps[i].pose.position.x = start.pose.position.x + x_gap*frac
00294 steps[i].pose.position.y = start.pose.position.y + y_gap*frac
00295 steps[i].pose.position.z = start.pose.position.z + z_gap*frac
00296 new_q = transformations.quaternion_slerp(qs,qf,frac)
00297 steps[i].pose.orientation.x = new_q[0]
00298 steps[i].pose.orientation.y = new_q[1]
00299 steps[i].pose.orientation.z = new_q[2]
00300 steps[i].pose.orientation.w = new_q[3]
00301 rospy.loginfo("Planning straight-line path, please wait")
00302 self.log_out.publish(data="Planning straight-line path, please wait")
00303
00304
00305
00306 req = self.form_ik_request(steps[0])
00307 ik_goal = self.ik_pose_proxy(req)
00308 seed = ik_goal.solution.joint_state.position
00309
00310 ik_points = [[0]*7 for i in range(len(ik_fracs))]
00311 for i, p in enumerate(steps):
00312 request = self.form_ik_request(p)
00313 request.ik_request.ik_seed_state.joint_state.position = seed
00314 ik_goal = self.ik_pose_proxy(request)
00315 ik_points[i] = ik_goal.solution.joint_state.position
00316 seed = ik_goal.solution.joint_state.position
00317 ik_points = np.array(ik_points)
00318
00319 angle_points = np.zeros((7, tot_points))
00320 for i in xrange(7):
00321 angle_points[i,:] = np.interp(ang_fracs, ik_fracs, ik_points[:,i])
00322
00323
00324 traj = JointTrajectory()
00325 traj.header.frame_id = steps[0].header.frame_id
00326 traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
00327
00328 for i in range(len(angle_points[0])):
00329 traj.points.append(JointTrajectoryPoint())
00330 traj.points[i].positions = angle_points[:,i]
00331 traj.points[i].velocities = [0]*7
00332 traj.points[i].time_from_start = rospy.Duration(
00333 ang_fracs[i]*duration)
00334 return traj
00335
00336 def build_follow_trajectory(self, traj):
00337
00338 traj_goal = FollowJointTrajectoryGoal()
00339 traj_goal.trajectory = traj
00340 tolerance = JointTolerance()
00341 tolerance.position = 0.05
00342 tolerance.velocity = 0.1
00343 traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
00344 traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
00345 traj_goal.goal_time_tolerance = rospy.Duration(3)
00346 return traj_goal
00347
00348 def move_torso(self, pos):
00349 rospy.loginfo("Moving Torso to reach arm goal")
00350 goal_out = SingleJointPositionGoal()
00351 goal_out.position = pos
00352 self.torso_client.send_goal(goal_out)
00353 return True
00354
00355 def fast_move(self, ps, time=0.):
00356 ik_goal = self.ik_pose_proxy(self.form_ik_request(ps))
00357 if ik_goal.error_code.val == 1:
00358 point = JointTrajectoryPoint()
00359 point.positions = ik_goal.solution.joint_state.position
00360 self.send_traj_point(point)
00361 else:
00362 rospy.logerr("FAST MOVE IK FAILED!")
00363
00364 def blind_move(self, ps, duration = None):
00365 (reachable, ik_goal, duration) = self.full_ik_check(ps)
00366 if reachable:
00367 self.send_joint_angles(ik_goal.solution.joint_state.position,
00368 duration)
00369
00370 def check_torso(self, request):
00371 """
00372 For unreachable goals, check to see if moving the torso can solve
00373 the problem. If yes, return True, and torso adjustment needed.
00374 If no, return False, and best possible Torso adjustment.
00375 """
00376 goal_z = request.ik_request.pose_stamped.pose.position.z
00377 torso_pos = self.torso_state.actual.positions[0]
00378 spine_range = [self.torso_min - torso_pos, self.torso_max - torso_pos]
00379 if abs(goal_z)<0.005:
00380
00381 rospy.loginfo("Torso Already at best possible position")
00382 return[False, 0]
00383
00384 if goal_z >= spine_range[0] and goal_z <= spine_range[1]:
00385 rospy.loginfo("Goal within spine movement range")
00386 request.ik_request.pose_stamped.pose.position.z = 0;
00387 opt_tor_move = goal_z
00388 elif goal_z > spine_range[1]:
00389 rospy.loginfo("Goal above spine movement range")
00390 request.ik_request.pose_stamped.pose.position.z -= spine_range[1]
00391 opt_tor_move = spine_range[1]
00392 else:
00393 rospy.loginfo("Goal below spine movement range")
00394 request.ik_request.pose_stamped.pose.position.z -= spine_range[0]
00395 opt_tor_move = spine_range[0]
00396
00397 print "Optimal Torso move: ", opt_tor_move
00398 ik_goal = self.ik_pose_proxy(request)
00399 if ik_goal.error_code.val != 1:
00400
00401 rospy.loginfo("Original goal cannot be reached")
00402 self.log_out.publish(data="Original goal cannot be reached")
00403 return [False, opt_tor_move]
00404 opt_ik_goal = ik_goal
00405
00406
00407 rospy.loginfo("Goal can be reached by moving spine")
00408 self.log_out.publish(data="Goal can be reached by moving spine")
00409 trials = np.linspace(0,opt_tor_move,round(abs(opt_tor_move)/0.05))
00410 np.append(trials,opt_tor_move)
00411 rospy.loginfo("Torso far from best position, finding closer sol.")
00412 print "Trials: ", trials
00413 for trial in trials:
00414 request.ik_request.pose_stamped.pose.position.z = goal_z + trial
00415 print "Trying goal @ ", goal_z + trial
00416 ik_goal = self.ik_pose_proxy(request)
00417 if ik_goal.error_code.val == 1:
00418 rospy.loginfo("Tried: %s, Succeeded" %trial)
00419 return [True, trial]
00420 rospy.loginfo("Tried: %s, Failed" %trial)
00421
00422 return [True, opt_tor_move]
00423
00424 def full_ik_check(self, ps):
00425
00426 req = self.form_ik_request(ps)
00427 ik_goal = self.ik_pose_proxy(req)
00428 if ik_goal.error_code.val == 1:
00429 self.dist = pu.calc_dist(self.curr_pose(), ps)
00430 return (True, ik_goal, None)
00431
00432 (torso_succeeded, torso_move) = self.check_torso(req)
00433 self.move_torso(self.torso_state.actual.positions[0]+torso_move)
00434 duration = max(4,85*abs(torso_move))
00435 if torso_succeeded:
00436 ik_goal = self.ik_pose_proxy(req)
00437 self.dist = pu.calc_dist(self.curr_pose(), ps)
00438 if ik_goal.error_code.val ==1:
00439 return (True, ik_goal, duration)
00440 else:
00441 print "Reported Succeeded, then really failed: \r\n", ik_goal
00442
00443
00444 pct = 0.98
00445 curr_pos = self.curr_pose().pose.position
00446 dx = req.ik_request.pose_stamped.pose.position.x - curr_pos.x
00447 dy = req.ik_request.pose_stamped.pose.position.y - curr_pos.y
00448 while pct > 0.01:
00449
00450 req.ik_request.pose_stamped.pose.position.x = curr_pos.x + pct*dx
00451 req.ik_request.pose_stamped.pose.position.y = curr_pos.y + pct*dy
00452 ik_goal = self.ik_pose_proxy(req)
00453 if ik_goal.error_code.val == 1:
00454 rospy.loginfo("Succeeded PARTIALLY (%s) with torso" %pct)
00455 return (True, ik_goal, duration)
00456 else:
00457 pct -= 0.02
00458
00459 rospy.loginfo("IK Failed: Error Code %s" %str(ik_goal.error_code))
00460 rospy.loginfo("Reached as far as possible")
00461 self.log_out.publish(data="Cannot reach farther in this direction.")
00462 return (False, ik_goal, duration)
00463
00464 def form_ik_request(self, ps):
00465
00466 req = GetPositionIKRequest()
00467 req.timeout = rospy.Duration(5)
00468 req.ik_request.pose_stamped = ps
00469 req.ik_request.ik_link_name = \
00470 self.ik_info.kinematic_solver_info.link_names[-1]
00471 req.ik_request.ik_seed_state.joint_state.name = \
00472 self.ik_info.kinematic_solver_info.joint_names
00473 req.ik_request.ik_seed_state.joint_state.position = \
00474 self.joint_state_act.positions
00475 return req
00476
00477 def send_joint_angles(self, angles, duration = None):
00478 point = JointTrajectoryPoint()
00479 point.positions = angles
00480 self.send_traj_point(point, duration)
00481
00482 def send_traj_point(self, point, time=None):
00483 if time is None:
00484 point.time_from_start = rospy.Duration(max(20*self.dist, 4))
00485 else:
00486 point.time_from_start = rospy.Duration(time)
00487 joint_traj = JointTrajectory()
00488 joint_traj.header.stamp = rospy.Time.now()
00489 joint_traj.header.frame_id = '/torso_lift_link'
00490 joint_traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
00491 joint_traj.points.append(point)
00492 joint_traj.points[0].velocities = [0,0,0,0,0,0,0]
00493 arm_goal = JointTrajectoryGoal()
00494 arm_goal.trajectory = joint_traj
00495 self.arm_traj_client.send_goal(arm_goal)
00496
00497 class PR2Arm_Planning(PR2Arm):
00498 def __init__(self, arm, tfl=None):
00499 PR2Arm.__init__(self, arm, tfl)
00500 self.move_arm_client = actionlib.SimpleActionClient(
00501 'move_' + self.arm + '_arm', MoveArmAction)
00502 rospy.loginfo("Waiting for move_" + self.arm + "_arm server")
00503 if self.move_arm_client.wait_for_server(rospy.Duration(50)):
00504 rospy.loginfo("Found move_" + self.arm + "_arm server")
00505 else:
00506 rospy.logwarn("Cannot find move_" + self.arm + "_arm server")
00507
00508 def move_arm_to(self, goal_in):
00509
00510
00511
00512 rospy.loginfo("Composing move_"+self.arm+"_arm goal")
00513 goal_out = MoveArmGoal()
00514 goal_out.motion_plan_request.group_name = self.arm+"_arm"
00515 goal_out.motion_plan_request.num_planning_attempts = 10
00516 goal_out.motion_plan_request.planner_id = ""
00517 goal_out.planner_service_name = "ompl_planning/plan_kinematic_path"
00518 goal_out.motion_plan_request.allowed_planning_time = rospy.Duration(10.0)
00519
00520 pos = PositionConstraint()
00521 pos.header.frame_id = goal_in.header.frame_id
00522 pos.link_name = self.arm[0]+"_wrist_roll_link"
00523 pos.position = goal_in.pose.position
00524
00525 pos.constraint_region_shape.type = 0
00526 pos.constraint_region_shape.dimensions=[0.10]
00527
00528 pos.constraint_region_orientation = Quaternion(0,0,0,1)
00529 pos.weight = 1
00530
00531 goal_out.motion_plan_request.goal_constraints.position_constraints.append(pos)
00532
00533 ort = OrientationConstraint()
00534 ort.header.frame_id=goal_in.header.frame_id
00535 ort.link_name = self.arm[0]+"_wrist_roll_link"
00536 ort.orientation = goal_in.pose.orientation
00537
00538 ort.absolute_roll_tolerance = 3.15
00539 ort.absolute_pitch_tolerance = 3.15
00540 ort.absolute_yaw_tolerance = 3.15
00541 ort.weight = 0.0
00542 goal_out.motion_plan_request.goal_constraints.orientation_constraints.append(ort)
00543 goal_out.accept_partial_plans = True
00544 goal_out.accept_invalid_goals = True
00545 goal_out.disable_collision_monitoring = True
00546 rospy.loginfo("Sending composed move_"+self.arm+"_arm goal")
00547
00548 finished_within_time = False
00549 self.move_arm_client.send_goal(goal_out)
00550 finished_within_time = self.move_arm_client.wait_for_result(
00551 rospy.Duration(60))
00552 if not (finished_within_time):
00553 self.move_arm_client.cancel_goal()
00554 self.log_out.publish(data="Timed out achieving "+self.arm+
00555 " arm goal pose")
00556 rospy.loginfo("Timed out achieving right arm goal pose")
00557 return False
00558 else:
00559 state = self.move_arm_client.get_state()
00560 result = self.move_arm_client.get_result()
00561 if (state == 3):
00562 rospy.loginfo("Action Finished: %s" %state)
00563 self.log_out.publish(data="Move "+self.arm.capitalize()+
00564 " Arm with Motion Planning: %s"
00565 %self.move_arm_error_dict[
00566 result.error_code.val])
00567 return True
00568 else:
00569 if result.error_code.val == 1:
00570 rospy.loginfo("Move_"+self.arm+"_arm_action failed: \
00571 Unable to plan a path to goal")
00572 self.log_out.publish(data="Move "+self.arm.capitalize()+
00573 " Arm with Motion Planning Failed: \
00574 Unable to plan a path to the goal")
00575 elif result.error_code.val == -31:
00576 (torso_succeeded, pos) = self.check_torso(
00577 self.form_ik_request(goal_in))
00578 if torso_succeeded:
00579 rospy.loginfo("IK Failed in Current Position. \
00580 Adjusting Height and Retrying")
00581 self.log_out.publish(data="IK Failed in Current \
00582 Position. Adjusting \
00583 Height and Retrying")
00584 self.move_arm_to(pos)
00585 else:
00586 rospy.loginfo("Move_"+self.arm+"_arm action failed: %s"
00587 %state)
00588 rospy.loginfo("Failure Result: %s" %result)
00589 self.log_out.publish(data="Move "+self.arm.capitalize()+
00590 " Arm with Motion Planning \
00591 and Torso Check Failed: %s"
00592 %self.move_arm_error_dict[
00593 result.error_code.val])
00594 else:
00595 rospy.loginfo("Move_"+self.arm+"_arm action failed: %s" %state)
00596 rospy.loginfo("Failure Result: %s" %result)
00597 self.log_out.publish(data="Move "+self.arm.capitalize()+
00598 " Arm with Motion Planning \
00599 Failed: %s"
00600 %self.move_arm_error_dict[
00601 result.error_code.val])
00602 return False
00603
00604 if __name__ == '__main__':
00605 rospy.init_node('pr2_arms')
00606 left_arm = PR2Arm('left')
00607 right_arm = PR2Arm('right')
00608 rospy.spin()