JointSpaceArmController.cpp
Go to the documentation of this file.
00001 
00002 /*
00003  * JointSpaceArmController.cpp
00004  *
00005  *  Created on: Feb 9, 2012
00006  *      Author: hess
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) { //everything ok
00029 
00030                 if (cross_product[2] > 0) {
00031                         //increment v2 until v2 larger v1
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                         //increment v2 until v2 larger v1
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 //function needed to adapt to JointSplineTrajectory Controller --> turning always towards the smallest angle
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         //  r_arm_joints_.push_back("r_shoulder_pan_joint");
00061         //  r_arm_joints_.push_back("r_shoulder_lift_joint");
00062         //  r_arm_joints_.push_back("r_upper_arm_roll_joint");
00063         //  r_arm_joints_.push_back("r_elbow_flex_joint");
00064         //  r_arm_joints_.push_back("r_forearm_roll_joint");
00065         //  r_arm_joints_.push_back("r_wrist_flex_joint");
00066         //  r_arm_joints_.push_back("r_wrist_roll_joint");
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 //      ros::service::waitForService("trajectory_filter/filter_trajectory");
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         //std_srvs::Empty collider_reset;
00131         //reset_collider_client_.call(collider_reset);
00132         std::vector<tf::Pose> tmp_touch_points;
00133         ROS_INFO_STREAM("JointSpaceArmController: Move arm to start");
00134         bool move_success = true;
00135         //bool move_success = moveArmToJointGoalUsingPlanner(arm, start_r_arm_, false,
00136         //tmp_touch_points); //move arm to start pose
00137         //ROS_WARN("POSE 1");
00138         //      tmp_touch_points.push_back(touch_points_allowed[0]);
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); // move arm planned to first pose in the graph
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 //      ROS_INFO("Collider reset");
00203 //      std_srvs::Empty collider_reset;
00204         //reset_collider_client_.call(collider_reset);
00205         std::vector<tf::Pose> tmp_touch_points;
00206 
00207         ROS_INFO("Moving arm to start");
00208         //bool move_success = moveArmToJointGoalUsingPlanner(arm, start_r_arm_, true,tmp_touch_points);
00209         //remove arm from surface
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); // move arm planned to first pose in the graph
00217 
00218                         //we have reached the first position of the coverage path
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                 //remove arm to the start position
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 //              ROS_INFO("Moving arm to start");
00242 //              move_success = move_success && moveArmToJointGoalUsingPlanner(arm, start_r_arm_, true,
00243 //                              tmp_touch_points);
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         //parameter values for the spline smoother
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                         //currently only set positions
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 //                      tmp.velocities = std::vector<double> (7,0); //traj_vel[i];
00297 //                      tmp.time_from_start = ros::Duration((i + 1) * time_diff); //ros::Duration(traj_time[i]);
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 //              req.allowed_time = ros::Duration(5.0);
00315 //
00316 //              if (filter_trajectory_client_.call(req, res)) {
00317 //                      if (res.error_code.val == res.error_code.SUCCESS) {
00318 //                              ROS_INFO("Requested trajectory was filtered");
00319 //                      } else
00320 //                              ROS_INFO("Requested trajectory was not filtered. Error code: %d", res.error_code.val);
00321 //              }
00322         }
00323 
00324         //construct the client
00325         ROS_INFO("Waiting for action server to start.");
00326 
00327         // wait for the action server to start
00328         traj_client_->waitForServer(); //will wait for infinite time
00329 
00330         //our goal variable
00331         pr2_controllers_msgs::JointTrajectoryGoal goal;
00332         // First, the joint names, which apply to all waypoints
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 //      goal.trajectory = req.trajectory;
00337 //      goal.trajectory = res.trajectory;
00338 //      // We will have two waypoints in this goal trajectory
00339 //      goal.trajectory.points.resize(traj.size());
00340 //
00341 //      //fill the goal
00342 //      double time_diff = 2.0;
00343 //      trajectory_msgs::JointTrajectoryPoint tmp;
00344 //
00345 //      for (unsigned int i = 0; i < traj.size(); ++i) {
00346 //
00347 //              //currently only set positions
00348 //              tmp.positions = traj[i];
00349 //              if (compute_vel) {
00350 //                      tmp.velocities = traj_vel[i];
00351 //                      tmp.time_from_start = ros::Duration(traj_time[i]);
00352 //              } else {
00353 //                      tmp.velocities.resize(7, 0);
00354 //                      tmp.time_from_start = ros::Duration((i + 1) * time_diff);
00355 //              }
00356 //              goal.trajectory.points[i] = tmp;
00357 //      }
00358 
00359         //send the goal and wait for the result
00360         traj_client_->sendGoal(goal);
00361 
00362         //wait for the action to return
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         //attach sponge
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 //      arm_goal.planning_scene_diff.attached_collision_objects.push_back(att_object);
00436 
00437         for (unsigned int i = 0; i < touch_points_allowed.size(); ++i) {
00438 
00439                 //allowed contacts
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 //              object1.dimensions[1] = .05;
00447 //              object1.dimensions[2] = .05;
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 //              all_contact.link_names.push_back("attached_sponge");
00476 //              all_contact.link_names.push_back("collision_map");
00477 //              arm_goal.planning_scene_diff.allowed_contacts.push_back(all_contact);
00478 //              all_contact.link_names.clear();
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 //              op.object2 = "collision_map";
00514 //              op.object1 = "attached_sponge";
00515 //              arm_goal.operations.collision_operations.push_back(op);
00516 //
00517 //              op.object1 = "collision_map";
00518 //              op.object2 = "attached_sponge";
00519 //              arm_goal.operations.collision_operations.push_back(op);
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         // set desired values for goal
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         // connect to the planning server
00539         ROS_INFO("moveArmToJointGoalUsingPlanner: Waiting for move_arm server");
00540         move_arm.waitForServer();
00541         ROS_INFO("moveArmToJointGoalUsingPlanner: Connected to move_arm server");
00542 
00543         // send goal to server
00544         move_arm.sendGoal(arm_goal);
00545 
00546         // and wait for some time for completion of task
00547         bool finished_within_time = move_arm.waitForResult();
00548 
00549         // check whether task was fulfilled in time
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 //              op.object2 = "collision_map";
00634 //              op.object1 = "attached_sponge";
00635 //              arm_goal.operations.collision_operations.push_back(op);
00636 //
00637 //              op.object1 = "collision_map";
00638 //              op.object2 = "attached_sponge";
00639 //              arm_goal.operations.collision_operations.push_back(op);
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          desired_pose.absolute_position_tolerance.x = 0.02;
00657          desired_pose.absolute_position_tolerance.y = 0.02;
00658          desired_pose.absolute_position_tolerance.z = 0.02;
00659          desired_pose.absolute_roll_tolerance = 0.04;
00660          desired_pose.absolute_pitch_tolerance = 0.04;
00661          desired_pose.absolute_yaw_tolerance = 0.04;
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 /* JOINTSPACEARMCONTROLLER_CPP_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


coverage_3d_arm_navigation
Author(s): Juergen Hess
autogenerated on Wed Dec 26 2012 15:25:56