00001
00002
00003
00004
00005
00006
00007
00008
00009 #ifndef JOINTSPACEARMCONTROLLER_CPP_
00010 #define JOINTSPACEARMCONTROLLER_CPP_
00011
00012 #include<coverage_3d_arm_navigation/JointSpaceArmController.h>
00013 #include<trajectory_msgs/JointTrajectoryPoint.h>
00014 #include <arm_navigation_msgs/FilterJointTrajectory.h>
00015 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00016 #include <arm_navigation_msgs/SimplePoseConstraint.h>
00017 #include <arm_navigation_msgs/MoveArmAction.h>
00018 #include <arm_navigation_msgs/utils.h>
00019 #include <coverage_3d_arm_navigation/iterative_smoother.h>
00020 #include <std_srvs/Empty.h>
00021 double denormalizeAngle(double angle1, double angle2) {
00022
00023 Eigen::Vector3f v1 = Eigen::Vector3f(cos(angle1), sin(angle1), 0);
00024 Eigen::Vector3f v2 = Eigen::Vector3f(cos(angle2), sin(angle2), 0);
00025 Eigen::Vector3f cross_product = v1.cross(v2);
00026
00027 cross_product.normalize();
00028 if (cross_product.norm() > 0.000000001) {
00029
00030 if (cross_product[2] > 0) {
00031
00032 while (angle2 < angle1) {
00033 angle2 = angle2 + 2 * M_PI;
00034 }
00035 while ((angle2 - angle1) > 2 * M_PI) {
00036 angle2 = angle2 - 2 * M_PI;
00037 }
00038 } else {
00039
00040 while (angle2 > angle1) {
00041 angle2 = angle2 - 2 * M_PI;
00042 }
00043 while ((angle2 - angle1) < -2 * M_PI) {
00044 angle2 = angle2 + 2 * M_PI;
00045 }
00046
00047 }
00048 }
00049 return angle2;
00050 }
00051
00052
00053 void denormalizeJointTrajectory(const std::vector<std::vector<double> >& traj,
00054 std::vector<std::vector<double> >& res_traj) {
00055
00056 res_traj.clear();
00057 for (unsigned int i = 0; i < traj.size(); ++i) {
00058 res_traj.push_back(traj[i]);
00059 }
00060
00061
00062
00063
00064
00065
00066
00067 for (unsigned int i = 1; i < res_traj.size(); ++i) {
00068
00069 res_traj[i][4] = denormalizeAngle(res_traj[i - 1][4], res_traj[i][4]);
00070 res_traj[i][6] = denormalizeAngle(res_traj[i - 1][6], res_traj[i][6]);
00071 }
00072
00073 }
00074
00075 JointSpaceArmController::JointSpaceArmController(ros::NodeHandle& nh) :
00076 nh_(nh) {
00077
00078 start_r_arm_.resize(7);
00079 start_r_arm_[0] = -1.68;
00080 start_r_arm_[1] = 0.052;
00081 start_r_arm_[2] = -1.48;
00082 start_r_arm_[3] = -1.65;
00083 start_r_arm_[4] = -1.60;
00084 start_r_arm_[5] = -1.57;
00085 start_r_arm_[6] = -3.02;
00086
00087 r_arm_joints_.push_back("r_shoulder_pan_joint");
00088 r_arm_joints_.push_back("r_shoulder_lift_joint");
00089 r_arm_joints_.push_back("r_upper_arm_roll_joint");
00090 r_arm_joints_.push_back("r_elbow_flex_joint");
00091 r_arm_joints_.push_back("r_forearm_roll_joint");
00092 r_arm_joints_.push_back("r_wrist_flex_joint");
00093 r_arm_joints_.push_back("r_wrist_roll_joint");
00094
00095 l_arm_joints_.push_back("l_shoulder_pan_joint");
00096 l_arm_joints_.push_back("l_shoulder_lift_joint");
00097 l_arm_joints_.push_back("l_upper_arm_roll_joint");
00098 l_arm_joints_.push_back("l_elbow_flex_joint");
00099 l_arm_joints_.push_back("l_forearm_roll_joint");
00100 l_arm_joints_.push_back("l_wrist_flex_joint");
00101 l_arm_joints_.push_back("l_wrist_roll_joint");
00102
00103 traj_client_ = new actionlib::SimpleActionClient<
00104 pr2_controllers_msgs::JointTrajectoryAction>(
00105 "/r_arm_controller/joint_trajectory_action", true);
00106
00107 filter_trajectory_client_ = nh_.serviceClient<
00108 arm_navigation_msgs::FilterJointTrajectory>(
00109 "/trajectory_filter_smoother/filter_trajectory");
00110 reset_collider_client_ = nh_.serviceClient<std_srvs::Empty>(
00111 "/collider_node/reset");
00112 }
00113
00114 JointSpaceArmController::~JointSpaceArmController() {
00115 delete traj_client_;
00116 }
00117
00118 void JointSpaceArmController::followEntireTrajectory(
00119 const std::vector<std::vector<double> >& path, const std::string arm,
00120 const std::vector<unsigned int>&action,
00121 const std::vector<tf::Pose>& pose_path,
00122 std::vector<tf::Pose>& touch_points_allowed) {
00123 ROS_DEBUG_STREAM(
00124 "JointSpaceArmController::followEntireTrajectory: " << path.size() << " " << action.size());
00125 ROS_ASSERT_MSG(path.size()==action.size(),
00126 "Path size does not match action size.");
00127 ROS_ASSERT_MSG(arm=="right_arm",
00128 "Currently only the right arm is supported.");
00129
00130
00131
00132 std::vector<tf::Pose> tmp_touch_points;
00133 ROS_INFO_STREAM("JointSpaceArmController: Move arm to start");
00134 bool move_success = true;
00135
00136
00137
00138
00139 ROS_INFO_STREAM(
00140 "JointSpaceArmController: Move arm to first trajectory point");
00141 move_success = moveArmToJointGoalUsingPlanner("right_arm", path[0], false,
00142 tmp_touch_points);
00143 ROS_WARN("POSE 2");
00144 if (move_success) {
00145
00146 tmp_touch_points.clear();
00147
00148 std::vector<std::vector<double> > tmp_path;
00149 std::vector<tf::Pose> tmp_pose_path;
00150 for (unsigned int i = 0; i < path.size() - 1; ++i) {
00151 ROS_DEBUG_STREAM(
00152 "JointSpaceArmController::followEntireTrajectory: Action " << i << " " << action[i]);
00153 }
00154
00155 for (unsigned int i = 0; i < path.size() - 1; ++i) {
00156 ROS_DEBUG_STREAM(
00157 "JointSpaceArmController::followEntireTrajectory: Action " << i << " " << action[i]);
00158 if (action[i] == 0) {
00159 tmp_path.push_back(path[i + 1]);
00160 tmp_pose_path.push_back(pose_path[i + 1]);
00161 } else if (action[i] == 1) {
00162 if (tmp_path.size() > 0) {
00163 followTrajectory(tmp_path, tmp_pose_path, "right_arm");
00164 tmp_path.clear();
00165 tmp_pose_path.clear();
00166 }
00167 tmp_touch_points.push_back(touch_points_allowed[i]);
00168 tmp_touch_points.push_back(touch_points_allowed[i + 1]);
00169 move_success = moveArmToJointGoalUsingPlanner("right_arm",
00170 path[i + 1], false, tmp_touch_points);
00171 if (!move_success) {
00172 return;
00173 }
00174 tmp_path.push_back(path[i + 1]);
00175 tmp_pose_path.push_back(pose_path[i + 1]);
00176 tmp_touch_points.clear();
00177 }
00178 }
00179 if (tmp_path.size() > 0) {
00180 followTrajectory(tmp_path, tmp_pose_path, "right_arm");
00181 }
00182 tmp_touch_points.push_back(
00183 touch_points_allowed[touch_points_allowed.size() - 1]);
00184 moveArmToJointGoalUsingPlanner(arm, start_r_arm_, false,
00185 tmp_touch_points);
00186 }
00187 }
00188
00189 bool JointSpaceArmController::followEntireTrajectory(
00190 const std::vector<std::vector<double> >& path, const std::string arm,
00191 const std::vector<unsigned int>&action,
00192 const std::vector<tf::Pose>& pose_path,
00193 std::vector<tf::Pose>& touch_points_allowed,
00194 std::vector<tf::Pose>& poses_wide_above_surface) {
00195 ROS_DEBUG_STREAM(
00196 "JointSpaceArmController::followEntireTrajectory: " << path.size() << " " << action.size());
00197 ROS_ASSERT_MSG(path.size()==action.size(),
00198 "Path size does not match action size.");
00199 ROS_ASSERT_MSG(arm=="right_arm",
00200 "Currently only the right arm is supported.");
00201
00202
00203
00204
00205 std::vector<tf::Pose> tmp_touch_points;
00206
00207 ROS_INFO("Moving arm to start");
00208
00209
00210 ROS_INFO("Moving arm to start");
00211 bool move_success = moveArmToPoseGoalUsingPlanner("right_arm",
00212 poses_wide_above_surface[1], true);
00213 ROS_INFO("Moving arm to first trajectory point");
00214 move_success = move_success
00215 && moveArmToJointGoalUsingPlanner("right_arm", path[1], false,
00216 tmp_touch_points);
00217
00218
00219 if (move_success) {
00220
00221 std::vector<std::vector<double> > tmp_path;
00222 std::vector<tf::Pose> tmp_pose_path;
00223
00224 for (unsigned int i = 1; i < path.size() - 1; ++i) {
00225 tmp_path.push_back(path[i]);
00226 tmp_pose_path.push_back(pose_path[i]);
00227 }
00228
00229 followTrajectory(tmp_path, tmp_pose_path, "right_arm");
00230
00231 ROS_INFO("Moving arm up");
00232 tmp_touch_points.clear();
00233
00234 tmp_touch_points.push_back(
00235 touch_points_allowed[touch_points_allowed.size() - 2]);
00236 move_success = move_success
00237 && moveArmToPoseGoalUsingPlanner("right_arm",
00238 poses_wide_above_surface[poses_wide_above_surface.size()
00239 - 2], false);
00240
00241
00242
00243
00244 }
00245 return move_success;
00246 }
00247
00248 void JointSpaceArmController::followTrajectory(
00249 const std::vector<std::vector<double> >& path2,
00250 const std::vector<tf::Pose>& pose_traj, const std::string arm) {
00251
00252 ROS_INFO("followTrajectory: Following coverage trajectory.");
00253 std::string action_name;
00254 std::vector<std::string> joint_names;
00255 if (arm == "right_arm") {
00256 joint_names = r_arm_joints_;
00257 action_name = "/r_arm_controller/joint_trajectory_action";
00258 } else if (arm == "left_arm") {
00259 joint_names = l_arm_joints_;
00260 action_name = "/l_arm_controller/joint_trajectory_action";
00261 }
00262
00263 std::vector<std::vector<double> > traj;
00264 denormalizeJointTrajectory(path2, traj);
00265
00266 bool compute_vel = true;
00267 std::vector<std::vector<double> > traj_vel;
00268 std::vector<double> traj_time;
00269
00270
00271 double scale_vel = 1.0 / 2.0;
00272 double scale_time = (1.0 / scale_vel) + (0.5 * (1.0 / scale_vel));
00273
00274 arm_navigation_msgs::FilterJointTrajectory::Request req;
00275 arm_navigation_msgs::FilterJointTrajectory::Request req2;
00276 arm_navigation_msgs::FilterJointTrajectory::Response res;
00277
00278 arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request r1;
00279 arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request r2;
00280 double time_diff = 2.0;
00281 if (compute_vel) {
00282 traj_planner_.planTrajectoryVelocity(traj, pose_traj, traj_vel,
00283 traj_time, scale_time, scale_vel);
00284
00285 req.trajectory.joint_names = joint_names;
00286 r1.trajectory.joint_names = joint_names;
00287
00288 for (unsigned int i = 0; i < traj.size(); i++) {
00289 trajectory_msgs::JointTrajectoryPoint tmp;
00290
00291 tmp.positions = traj[i];
00292 tmp.velocities = traj_vel[i];
00293 tmp.time_from_start = ros::Duration(traj_time[i]);
00294
00295 tmp.accelerations = std::vector<double>(7, 0);
00296
00297
00298 req.trajectory.points.push_back(tmp);
00299 r1.trajectory.points.push_back(tmp);
00300 }
00301 r1.limits.resize(joint_names.size());
00302 for (int i = 0; i < joint_names.size(); ++i) {
00303 r1.limits[i].has_velocity_limits = true;
00304 r1.limits[i].max_velocity = 1.0;
00305 r1.limits[i].has_acceleration_limits = true;
00306 r1.limits[i].max_acceleration = 0.3;
00307 std::cout << joint_names[i] << std::endl;
00308 }
00309
00310 IterativeParabolicSmoother<
00311 arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request> smoother;
00312 smoother.smooth(r1, r2);
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322 }
00323
00324
00325 ROS_INFO("Waiting for action server to start.");
00326
00327
00328 traj_client_->waitForServer();
00329
00330
00331 pr2_controllers_msgs::JointTrajectoryGoal goal;
00332
00333 goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(2.0);
00334 goal.trajectory.joint_names = joint_names;
00335 goal.trajectory = r2.trajectory;
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360 traj_client_->sendGoal(goal);
00361
00362
00363 bool exec_okay = traj_client_->waitForResult();
00364
00365 if (exec_okay) {
00366 actionlib::SimpleClientGoalState state = traj_client_->getState();
00367 ROS_INFO("Action finished: %s", state.toString().c_str());
00368 } else
00369 ROS_INFO("Action did not finish before the time out.");
00370 }
00371
00372 bool JointSpaceArmController::moveArmToJointGoalUsingPlanner(
00373 const std::string arm, const std::vector<double> & state,
00374 const bool collision_check,
00375 std::vector<tf::Pose>& touch_points_allowed) {
00376
00377 double planning_tolerance = 0.02;
00378 int n_joints = state.size();
00379 std::vector<std::string> joint_names;
00380 if (arm == "right_arm") {
00381 joint_names = r_arm_joints_;
00382 } else if (arm == "left_arm") {
00383 joint_names = l_arm_joints_;
00384 }
00385 std::string action_name("/move_");
00386 action_name.append(arm);
00387
00388 ROS_INFO("moveArmToJointGoalUsingPlanner: connecting to move arm action");
00389 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(
00390 action_name, true);
00391 move_arm.waitForServer();
00392 ROS_INFO("moveArmToJointGoalUsingPlanner: Connected to move_arm server");
00393 arm_navigation_msgs::MoveArmGoal arm_goal;
00394
00395 arm_goal.motion_plan_request.group_name = arm;
00396 arm_goal.motion_plan_request.num_planning_attempts = 3;
00397 arm_goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00398 arm_goal.motion_plan_request.planner_id = std::string("");
00399 arm_goal.planner_service_name = "/ompl_planning/plan_kinematic_path";
00400 arm_goal.accept_invalid_goals = false;
00401 arm_goal.accept_partial_plans = false;
00402 arm_goal.motion_plan_request.goal_constraints.joint_constraints.resize(
00403 n_joints);
00404
00405
00406 arm_navigation_msgs::AttachedCollisionObject att_object;
00407 att_object.link_name = "r_gripper_r_finger_tip_link";
00408
00409 att_object.object.id = "attached_sponge";
00410 att_object.object.operation.operation =
00411 arm_navigation_msgs::CollisionObjectOperation::ADD;
00412
00413 att_object.object.header.frame_id = "r_gripper_r_finger_tip_link";
00414 att_object.object.header.stamp = ros::Time::now();
00415 arm_navigation_msgs::Shape object;
00416 object.type = arm_navigation_msgs::Shape::BOX;
00417 object.dimensions.resize(3);
00418 object.dimensions[0] = .05;
00419 object.dimensions[1] = .05;
00420 object.dimensions[2] = .05;
00421
00422 geometry_msgs::Pose pose;
00423 pose.position.x = 0.0;
00424 pose.position.y = 0.0;
00425 pose.position.z = 0.0;
00426 pose.orientation.x = 0;
00427 pose.orientation.y = 0;
00428 pose.orientation.z = 0;
00429 pose.orientation.w = 1;
00430
00431 att_object.object.shapes.push_back(object);
00432 att_object.object.poses.push_back(pose);
00433 att_object.touch_links.push_back("r_end_effector");
00434
00435
00436
00437 for (unsigned int i = 0; i < touch_points_allowed.size(); ++i) {
00438
00439
00440 arm_navigation_msgs::AllowedContactSpecification all_contact;
00441 all_contact.name = "start_contact";
00442 arm_navigation_msgs::Shape object1;
00443 object1.type = arm_navigation_msgs::Shape::SPHERE;
00444 object1.dimensions.resize(1);
00445 object1.dimensions[0] = .03;
00446
00447
00448 all_contact.shape = object1;
00449 geometry_msgs::PoseStamped pose_stamped;
00450 tf::poseTFToMsg(touch_points_allowed[i], pose_stamped.pose);
00451 pose_stamped.header.frame_id = "/torso_lift_link";
00452
00453 all_contact.pose_stamped = pose_stamped;
00454 all_contact.penetration_depth = 0.03;
00455 all_contact.link_names.push_back("r_gripper_r_finger_tip_link");
00456 all_contact.link_names.push_back("collision_map");
00457 arm_goal.planning_scene_diff.allowed_contacts.push_back(all_contact);
00458 all_contact.link_names.clear();
00459
00460 all_contact.link_names.push_back("r_gripper_l_finger_tip_link");
00461 all_contact.link_names.push_back("collision_map");
00462 arm_goal.planning_scene_diff.allowed_contacts.push_back(all_contact);
00463 all_contact.link_names.clear();
00464
00465 all_contact.link_names.push_back("r_gripper_l_finger_link");
00466 all_contact.link_names.push_back("collision_map");
00467 arm_goal.planning_scene_diff.allowed_contacts.push_back(all_contact);
00468 all_contact.link_names.clear();
00469
00470 all_contact.link_names.push_back("r_gripper_r_finger_link");
00471 all_contact.link_names.push_back("collision_map");
00472 arm_goal.planning_scene_diff.allowed_contacts.push_back(all_contact);
00473 all_contact.link_names.clear();
00474
00475
00476
00477
00478
00479 }
00480
00481 if (!collision_check) {
00482 arm_navigation_msgs::CollisionOperation op;
00483 op.operation = op.DISABLE;
00484
00485 op.object1 = "collision_map";
00486 op.object2 = "r_gripper_l_finger_tip_link";
00487 arm_goal.operations.collision_operations.push_back(op);
00488
00489 op.object1 = "collision_map";
00490 op.object2 = "r_gripper_r_finger_tip_link";
00491 arm_goal.operations.collision_operations.push_back(op);
00492
00493 op.object2 = "collision_map";
00494 op.object1 = "r_gripper_l_finger_tip_link";
00495 arm_goal.operations.collision_operations.push_back(op);
00496
00497 op.object2 = "collision_map";
00498 op.object1 = "r_gripper_r_finger_tip_link";
00499 arm_goal.operations.collision_operations.push_back(op);
00500
00501 op.object1 = "collision_map";
00502 op.object2 = "l_gripper_l_finger_tip_link";
00503 arm_goal.operations.collision_operations.push_back(op);
00504
00505 op.object1 = "collision_map";
00506 op.object2 = "l_gripper_r_finger_tip_link";
00507 arm_goal.operations.collision_operations.push_back(op);
00508
00509 op.object2 = "collision_map";
00510 op.object1 = "l_gripper_l_finger_tip_link";
00511 arm_goal.operations.collision_operations.push_back(op);
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521 op.object1 = op.COLLISION_SET_ALL;
00522 op.object2 = op.COLLISION_SET_ALL;
00523 arm_goal.operations.collision_operations.push_back(op);
00524 }
00525
00526
00527 for (int i = 0; i < n_joints; i++) {
00528 arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name =
00529 joint_names[i];
00530 arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].position =
00531 state[i];
00532 arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below =
00533 planning_tolerance;
00534 arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above =
00535 planning_tolerance;
00536 }
00537
00538
00539 ROS_INFO("moveArmToJointGoalUsingPlanner: Waiting for move_arm server");
00540 move_arm.waitForServer();
00541 ROS_INFO("moveArmToJointGoalUsingPlanner: Connected to move_arm server");
00542
00543
00544 move_arm.sendGoal(arm_goal);
00545
00546
00547 bool finished_within_time = move_arm.waitForResult();
00548
00549
00550 if (!finished_within_time) {
00551 move_arm.cancelGoal();
00552 actionlib::SimpleClientGoalState state = move_arm.getState();
00553 ROS_INFO_STREAM(
00554 "moveArmToJointGoalUsingPlanner: " << state.toString());
00555 return false;
00556 } else {
00557 actionlib::SimpleClientGoalState state = move_arm.getState();
00558
00559 if (state == actionlib::SimpleClientGoalState::SUCCEEDED) {
00560 ROS_INFO(
00561 "moveArmToJointGoalUsingPlanner: %s reached goal.", arm.c_str());
00562 return true;
00563 } else {
00564 ROS_INFO("moveArmToJointGoalUsingPlanner: Something went wrong:");
00565 ROS_INFO("%s status: %s", arm.c_str(), state.toString().c_str());
00566 return false;
00567 }
00568 }
00569
00570 return true;
00571
00572 }
00573
00574 bool JointSpaceArmController::moveArmToPoseGoalUsingPlanner(
00575 const std::string arm, const tf::Pose& pose,
00576 const bool collision_check) {
00577
00578 double planning_tolerance = 0.02;
00579 std::vector<std::string> joint_names;
00580 if (arm == "right_arm") {
00581 joint_names = r_arm_joints_;
00582 } else if (arm == "left_arm") {
00583 joint_names = l_arm_joints_;
00584 }
00585 std::string action_name("/move_");
00586 action_name.append(arm);
00587
00588 ROS_INFO("moveArmToPoseGoalUsingPlanner: connecting to move arm action");
00589 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(
00590 action_name, true);
00591 move_arm.waitForServer();
00592 ROS_INFO("moveArmToPoseGoalUsingPlanner: connected to move_arm server");
00593 arm_navigation_msgs::MoveArmGoal arm_goal;
00594
00595 arm_goal.motion_plan_request.group_name = arm;
00596 arm_goal.motion_plan_request.num_planning_attempts = 3;
00597 arm_goal.motion_plan_request.allowed_planning_time = ros::Duration(10.0);
00598 arm_goal.motion_plan_request.planner_id = std::string("");
00599 arm_goal.planner_service_name = "/ompl_planning/plan_kinematic_path";
00600
00601 if (!collision_check) {
00602 arm_navigation_msgs::CollisionOperation op;
00603 op.operation = op.DISABLE;
00604
00605 op.object1 = "collision_map";
00606 op.object2 = "r_gripper_l_finger_tip_link";
00607 arm_goal.operations.collision_operations.push_back(op);
00608
00609 op.object1 = "collision_map";
00610 op.object2 = "r_gripper_r_finger_tip_link";
00611 arm_goal.operations.collision_operations.push_back(op);
00612
00613 op.object2 = "collision_map";
00614 op.object1 = "r_gripper_l_finger_tip_link";
00615 arm_goal.operations.collision_operations.push_back(op);
00616
00617 op.object2 = "collision_map";
00618 op.object1 = "r_gripper_r_finger_tip_link";
00619 arm_goal.operations.collision_operations.push_back(op);
00620
00621 op.object1 = "collision_map";
00622 op.object2 = "l_gripper_l_finger_tip_link";
00623 arm_goal.operations.collision_operations.push_back(op);
00624
00625 op.object1 = "collision_map";
00626 op.object2 = "l_gripper_r_finger_tip_link";
00627 arm_goal.operations.collision_operations.push_back(op);
00628
00629 op.object2 = "collision_map";
00630 op.object1 = "l_gripper_l_finger_tip_link";
00631 arm_goal.operations.collision_operations.push_back(op);
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641 op.object1 = op.COLLISION_SET_ALL;
00642 op.object2 = op.COLLISION_SET_ALL;
00643 arm_goal.operations.collision_operations.push_back(op);
00644 }
00645
00646 arm_navigation_msgs::SimplePoseConstraint desired_pose;
00647 desired_pose.header.frame_id = "torso_lift_link";
00648 desired_pose.link_name = "r_wrist_roll_link";
00649
00650 tf::Pose tmp_pose = pose;
00651 tf::Quaternion q = tmp_pose.getRotation();
00652 q.normalize();
00653 tmp_pose.setRotation(q);
00654 tf::poseTFToMsg(tmp_pose, desired_pose.pose);
00655
00656
00657
00658
00659
00660
00661
00662
00663 desired_pose.absolute_position_tolerance.x = 0.03;
00664 desired_pose.absolute_position_tolerance.y = 0.03;
00665 desired_pose.absolute_position_tolerance.z = 0.03;
00666 desired_pose.absolute_roll_tolerance = 0.2;
00667 desired_pose.absolute_pitch_tolerance = 0.2;
00668 desired_pose.absolute_yaw_tolerance = 0.2;
00669
00670 arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose, arm_goal);
00671
00672 bool finished_within_time = false;
00673 move_arm.sendGoal(arm_goal);
00674 finished_within_time = move_arm.waitForResult(ros::Duration(20.0));
00675 if (!finished_within_time) {
00676 move_arm.cancelGoal();
00677 ROS_INFO(
00678 "moveArmToPoseGoalUsingPlanner: Timed out achieving pose goal");
00679 } else {
00680 actionlib::SimpleClientGoalState state = move_arm.getState();
00681 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00682 if (success) {
00683 ROS_INFO(
00684 "moveArmToPoseGoalUsingPlanner: Pose Goal Action finished: %s", state.toString().c_str());
00685 return true;
00686 } else {
00687 ROS_INFO(
00688 "moveArmToPoseGoalUsingPlanner: Pose Goal Action failed: %s", state.toString().c_str());
00689 return false;
00690 }
00691 }
00692
00693 return true;
00694 }
00695
00696 #endif