pr2_plugs_common: pr2_plugs_common: joint_space

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 Willow Garage, Inc. 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: Wim Meeussen
00036 *********************************************************************/
00037 
00038 
00039 #include <string.h>
00040 #include <pr2_controllers_msgs/JointTrajectoryGoal.h>
00041 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00042 #include <actionlib/client/simple_action_client.h>
00043 
00044 namespace pr2_plugs_common{
00045 
00046 static bool gotoJointPosition(double q1, double q2,double q3,double q4,double q5,double q6,double q7, bool right_arm)
00047 {
00048   ROS_INFO("Waiting for joint trajectory action");
00049   std::string arm = "r";
00050   if (!right_arm) arm = "l";
00051 
00052   actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> trajectory_action(arm+"_arm_controller/joint_trajectory_action", true);
00053   trajectory_action.waitForServer();
00054   ROS_INFO("Moving %s arm to detection pose", arm.c_str());
00055 
00056   pr2_controllers_msgs::JointTrajectoryGoal trajectory_goal ;
00057   trajectory_goal.trajectory.joint_names.resize(7);
00058   trajectory_goal.trajectory.joint_names[0] = arm+"_upper_arm_roll_joint";
00059   trajectory_goal.trajectory.joint_names[1] = arm+"_shoulder_pan_joint";
00060   trajectory_goal.trajectory.joint_names[2] = arm+"_shoulder_lift_joint";
00061   trajectory_goal.trajectory.joint_names[3] = arm+"_forearm_roll_joint";
00062   trajectory_goal.trajectory.joint_names[4] = arm+"_elbow_flex_joint";
00063   trajectory_goal.trajectory.joint_names[5] = arm+"_wrist_flex_joint";
00064   trajectory_goal.trajectory.joint_names[6] = arm+"_wrist_roll_joint";
00065 
00066   
00067   // moves arm to given joint space position
00068   trajectory_goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.2);
00069   trajectory_goal.trajectory.points.resize(1);
00070   trajectory_goal.trajectory.points[0].positions.resize(7);
00071   trajectory_goal.trajectory.points[0].positions[0] = q1;
00072   trajectory_goal.trajectory.points[0].positions[1] = q2;
00073   trajectory_goal.trajectory.points[0].positions[2] = q3;
00074   trajectory_goal.trajectory.points[0].positions[3] = q4;
00075   trajectory_goal.trajectory.points[0].positions[4] = q5;
00076   trajectory_goal.trajectory.points[0].positions[5] = q6;
00077   trajectory_goal.trajectory.points[0].positions[6] = q7;
00078   trajectory_goal.trajectory.points[0].time_from_start = ros::Duration(4.0);
00079   if (trajectory_action.sendGoalAndWait(trajectory_goal,ros::Duration(20.0), ros::Duration(5.0)) != actionlib::SimpleClientGoalState::SUCCEEDED){
00080     ROS_ERROR("Failed to reach joint space detection position");
00081     return false;
00082   }
00083   return true;
00084 }
00085 
00086 }
joint_space_move.h
