cotesys_ros_grasping: cotesys_ros_grasping: move_arm_to

00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *  \author E. Gil Jones
00036  *********************************************************************/
00037 
00038 #include <stdio.h>
00039 #include <stdlib.h>
00040 #include <time.h>
00041 #include <boost/thread.hpp>
00042 
00043 #include <ros/ros.h>
00044 #include <actionlib/client/simple_action_client.h>
00045 #include <actionlib/server/simple_action_server.h>
00046 
00047 #include <move_arm_msgs/MoveArmAction.h>
00048 #include <cotesys_ros_grasping/MoveArmToPositionAction.h>
00049 
00050 namespace cotesys_ros_grasping
00051 {
00052 
00053 struct GraspDef {
00054   std::string name;
00055   double ik_to_gripper_x_diff_;
00056   double ik_to_gripper_y_diff_;
00057   double ik_to_gripper_z_diff_;
00058 
00059   double end_effector_rot_x_;
00060   double end_effector_rot_y_;
00061   double end_effector_rot_z_;
00062   double end_effector_rot_w_;
00063 };
00064 
00065 class MoveArmToPositionServer
00066 {
00067 public:
00068   MoveArmToPositionServer();
00069 
00070   bool execute(const cotesys_ros_grasping::MoveArmToPositionGoalConstPtr& goal);
00071 
00072 private:
00073 
00074   std::string left_arm_name_, right_arm_name_;
00075   std::string right_ik_link_, left_ik_link_;
00076   
00077   std::map<std::string, GraspDef> grasp_def_map_;
00078   
00079   ros::NodeHandle priv_nh_, root_nh_;
00080   boost::shared_ptr<actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> > left_move_arm_client_, right_move_arm_client_;
00081   boost::shared_ptr<actionlib::SimpleActionServer<cotesys_ros_grasping::MoveArmToPositionAction> > action_server_;
00082   
00083 };
00084 
00085 MoveArmToPositionServer::MoveArmToPositionServer()
00086   : priv_nh_("~")
00087 {
00088   priv_nh_.param<std::string>("left_arm_name", left_arm_name_, "left_arm");
00089   priv_nh_.param<std::string>("right_arm_name", right_arm_name_, "right_arm");
00090   priv_nh_.param<std::string>("right_ik_link", right_ik_link_, "r_wrist_roll_link");
00091   priv_nh_.param<std::string>("left_ik_link", left_ik_link_, "l_wrist_roll_link");
00092 
00093   if(!priv_nh_.hasParam("grasps")) {
00094     ROS_WARN_STREAM("No grasps loaded");
00095   } else {
00096     XmlRpc::XmlRpcValue grasps_xml;
00097     priv_nh_.getParam("grasps", grasps_xml);
00098     if(grasps_xml.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00099       ROS_WARN("grasps is not an array");
00100     } else if(grasps_xml.size() == 0) {
00101       ROS_WARN("No grasps specified in grasps yaml");
00102     } else {
00103       bool hasDefault = false;
00104       for(int i = 0; i < grasps_xml.size(); i++) {
00105         if(!grasps_xml[i].hasMember("name")) {
00106           ROS_WARN("Each grasp must have a name");
00107           continue;
00108         }
00109         GraspDef grasp;
00110         grasp.name = std::string(grasps_xml[i]["name"]);
00111         if(grasp.name == "default") {
00112           hasDefault = true;
00113         }
00114         ROS_INFO_STREAM("Adding grasp named " << grasp.name);
00115         if(grasps_xml[i].hasMember("ik_to_gripper_x_diff")) {
00116           grasp.ik_to_gripper_x_diff_ = grasps_xml[i]["ik_to_gripper_x_diff"];
00117         }
00118         if(grasps_xml[i].hasMember("ik_to_gripper_y_diff")) {
00119           grasp.ik_to_gripper_y_diff_ = grasps_xml[i]["ik_to_gripper_y_diff"];
00120         }
00121         if(grasps_xml[i].hasMember("ik_to_gripper_z_diff")) {
00122           grasp.ik_to_gripper_z_diff_ = grasps_xml[i]["ik_to_gripper_z_diff"];
00123         }
00124         if(grasps_xml[i].hasMember("end_effector_x_rot")) {
00125           grasp.end_effector_rot_x_ = grasps_xml[i]["end_effector_x_rot"];
00126         } else {
00127           ROS_WARN("no x rot");
00128         }
00129         if(grasps_xml[i].hasMember("end_effector_y_rot")) {
00130           grasp.end_effector_rot_y_ = grasps_xml[i]["end_effector_y_rot"];
00131         } else {
00132           ROS_WARN("no y rot");
00133         }
00134         if(grasps_xml[i].hasMember("end_effector_z_rot")) {
00135           grasp.end_effector_rot_z_ = grasps_xml[i]["end_effector_z_rot"];
00136         } else {
00137           ROS_WARN("no z rot");
00138         }
00139         if(grasps_xml[i].hasMember("end_effector_w_rot")) {
00140           grasp.end_effector_rot_w_ = grasps_xml[i]["end_effector_w_rot"];
00141         } else {
00142           ROS_WARN("no w rot");
00143         }
00144         ROS_INFO_STREAM("Quaternion is " << grasp.end_effector_rot_x_ << " " 
00145                         << grasp.end_effector_rot_y_ << " " 
00146                         << grasp.end_effector_rot_z_ << " " 
00147                         << grasp.end_effector_rot_w_);
00148 
00149         grasp_def_map_[grasp.name] = grasp;
00150       }
00151       if(!hasDefault) {
00152         ROS_WARN("No default grasp pose");
00153       }
00154     }
00155   }
00156 
00157   //TODO - test if the end effector parameters specify a valid quaternion
00158 
00159   left_move_arm_client_.reset(new actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction>("left_move_arm_action", true));
00160   right_move_arm_client_.reset(new actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction>("right_move_arm_action", true));
00161 
00162   //TODO - wait for the servers
00163 
00164   action_server_.reset(new actionlib::SimpleActionServer<cotesys_ros_grasping::MoveArmToPositionAction>(root_nh_, "move_arm_to_position",
00165                                                                                                         boost::bind(&MoveArmToPositionServer::execute, this, _1)));
00166 }
00167 
00168 bool MoveArmToPositionServer::execute(const cotesys_ros_grasping::MoveArmToPositionGoalConstPtr& req)
00169 {
00170   if(req->arm_name != left_arm_name_ && req->arm_name != right_arm_name_) {
00171     ROS_ERROR_STREAM("Can't do anything for arm named " << req->arm_name);
00172     return false;
00173   }
00174 
00175   std::string grasp_name = req->grasp_name;
00176   if(grasp_def_map_.find(req->grasp_name) == grasp_def_map_.end()) {
00177     if(grasp_def_map_.find("default") != grasp_def_map_.end()) {
00178       ROS_DEBUG("Using default");
00179       grasp_name = "default";
00180     } else {
00181       ROS_INFO_STREAM("Don't have grasp named " << req->grasp_name << " and no default defined");
00182       return false;
00183     }
00184   }
00185 
00186   ROS_INFO_STREAM("Going to execute grasp named " << grasp_name);
00187   GraspDef& grasp = grasp_def_map_[grasp_name];
00188 
00189   std::string ik_link_name;
00190   boost::shared_ptr<actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> >  move_arm_client;
00191   if(req->arm_name == left_arm_name_) {
00192     ik_link_name = left_ik_link_;
00193     move_arm_client = left_move_arm_client_;
00194   } else {
00195     ik_link_name = right_ik_link_;
00196     move_arm_client = right_move_arm_client_;
00197   }
00198 
00199   move_arm_msgs::MoveArmGoal goal;
00200 
00201   goal.motion_plan_request.group_name=req->arm_name;
00202   goal.motion_plan_request.num_planning_attempts = 1;
00203   goal.motion_plan_request.planner_id = "";
00204   goal.planner_service_name = "ompl_planning/plan_kinematic_path";
00205   goal.motion_plan_request.allowed_planning_time = ros::Duration(2.0);
00206   
00207   goal.motion_plan_request.goal_constraints.position_constraints.resize(1);
00208   goal.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time();
00209   goal.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "base_link";
00210     
00211   goal.motion_plan_request.goal_constraints.position_constraints[0].link_name = ik_link_name;
00212   goal.motion_plan_request.goal_constraints.position_constraints[0].position.x = req->point.x-grasp.ik_to_gripper_x_diff_;
00213   goal.motion_plan_request.goal_constraints.position_constraints[0].position.y = req->point.y-grasp.ik_to_gripper_y_diff_;
00214   goal.motion_plan_request.goal_constraints.position_constraints[0].position.z = req->point.z-grasp.ik_to_gripper_z_diff_;
00215     
00216   goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX;
00217   goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.20);
00218   goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.20);
00219   goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.20);
00220 
00221   goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
00222   goal.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
00223 
00224   goal.motion_plan_request.goal_constraints.orientation_constraints.resize(1);
00225   goal.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time();
00226   goal.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "base_link";
00227   goal.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = ik_link_name;
00228   goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = grasp.end_effector_rot_x_;
00229   goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = grasp.end_effector_rot_y_;
00230   goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = grasp.end_effector_rot_z_;
00231   goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = grasp.end_effector_rot_w_;
00232     
00233   goal.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.15;
00234   goal.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.15;
00235   goal.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.15;
00236 
00237   goal.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
00238 
00239   //turning off collisions with the stupid camera frame
00240   motion_planning_msgs::CollisionOperation coll;
00241   coll.object1 = "r_forearm_cam";
00242   coll.object2 = coll.COLLISION_SET_ALL;
00243   coll.operation = coll.DISABLE;
00244   goal.motion_plan_request.ordered_collision_operations.collision_operations.push_back(coll);
00245   coll.object1 = "l_forearm_cam";
00246   goal.motion_plan_request.ordered_collision_operations.collision_operations.push_back(coll);
00247   if(req->arm_name == left_arm_name_) {
00248     coll.object1 = "l_forearm_link";
00249     coll.object2 = coll.COLLISION_SET_ATTACHED_OBJECTS;
00250     coll.operation = coll.DISABLE;
00251     goal.motion_plan_request.ordered_collision_operations.collision_operations.push_back(coll);
00252   } else {
00253     coll.object1 = "r_forearm_link";
00254     coll.object2 = coll.COLLISION_SET_ATTACHED_OBJECTS;
00255     coll.operation = coll.DISABLE;
00256     goal.motion_plan_request.ordered_collision_operations.collision_operations.push_back(coll);
00257   }
00258 
00259   cotesys_ros_grasping::MoveArmToPositionResult res;
00260 
00261   move_arm_client->sendGoal(goal);
00262   bool call_ok = move_arm_client->waitForResult(ros::Duration(30.0));
00263   if(!call_ok) {
00264     res.error_code = move_arm_client->getResult()->error_code;
00265     action_server_->setAborted(res);
00266   } else {
00267     actionlib::SimpleClientGoalState state = move_arm_client->getState();
00268     if(state != actionlib::SimpleClientGoalState::SUCCEEDED) {
00269       res.error_code = move_arm_client->getResult()->error_code;
00270       action_server_->setAborted(res);
00271     } else {
00272       res.error_code = move_arm_client->getResult()->error_code;
00273       action_server_->setSucceeded(res);
00274     } 
00275   }
00276   return true;
00277 }
00278 
00279 }
00280 
00281 int main(int argc, char** argv)
00282 {
00283   ros::init(argc, argv, "move_arm_to_position");
00284   
00285   ros::AsyncSpinner spinner(1); // Use 1 thread
00286   spinner.start();
00287 
00288   cotesys_ros_grasping::MoveArmToPositionServer move_arm_pos;
00289 
00290   ROS_INFO("Move_arm_to_position action started");
00291   ros::waitForShutdown();
00292     
00293   return 0;
00294 }
move_arm_to_position.cpp
