RobotArm.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #ifndef __ROBOTARM_H__
00031 #define __ROBOTARM_H__
00032 
00033 // roslaunch arm_ik.launch
00034 #include <ros/ros.h>
00035 
00036 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00037 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00038 #include <actionlib/client/simple_action_client.h>
00039 #include <actionlib/client/terminal_state.h>
00040 #include <pr2_common_action_msgs/ArmMoveIKAction.h>
00041 #include <tf/transform_listener.h>
00042 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
00043 #include <move_base_msgs/MoveBaseAction.h>
00044 #include <geometry_msgs/Twist.h>
00045 #include <pr2_msgs/PressureState.h>
00046 #include <kinematics_msgs/GetPositionIK.h>
00047 #include <kinematics_msgs/GetPositionFK.h>
00048 
00049 
00050 #include <find_base_pose/FindBasePoseAction.h>
00051 
00052 
00053 #include <boost/thread/mutex.hpp>
00054 
00055 
00056 //l_arm_tucked = [0.06024, 1.248526, 1.789070, -1.683386, -1.7343417, -0.0962141, -0.0864407]
00057 //r_arm_tucked = [-0.023593, 1.1072800, -1.5566882, -2.124408, -1.4175, -1.8417, 0.21436]
00058 
00059 
00060 typedef actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > TrajClient;
00061 
00062 class RobotArm
00063 {
00064 private:
00065     // Action client for the joint trajectory action
00066     // used to trigger the arm movement action
00067 
00068     int side_;
00069 
00070     TrajClient* traj_client_;
00071     static tf::TransformListener *listener_;
00072     actionlib::SimpleActionClient<pr2_common_action_msgs::ArmMoveIKAction> *ac_;
00073     static actionlib::SimpleActionClient<find_base_pose::FindBasePoseAction> *ac_fbp_;
00074     ros::Subscriber jointStateSubscriber_;
00075     ros::NodeHandle n_;
00076     boost::mutex mutex_;
00077 
00078     ros::ServiceClient query_client;
00079     ros::ServiceClient fk_client;
00080     ros::ServiceClient ik_client;
00081 
00082     volatile bool haveJointState;
00083     double jointState[7];
00084     double jointStateDes[7];
00085     double jointStateErr[7];
00086     std::string joint_names[7];
00087 
00088     void jointStateCallback(const  pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg);
00089 
00090     static RobotArm *instance[];
00091 
00093     RobotArm(int side);
00094 
00096     ~RobotArm();
00097 
00098 public:
00099     //will be private again one day
00100     bool tucked;
00101 
00102     int retries;
00103 
00104     bool raise_elbow;
00105     double preset_angle;
00106 
00107     actionlib::SimpleActionClient<pr2_common_action_msgs::ArmMoveIKAction> *getActionClient()
00108     {
00109         return ac_;
00110     }
00111 
00112     static RobotArm* getInstance(int side = 0);
00113 
00114     bool isTucked()
00115     {
00116         return tucked;
00117     }
00118 
00119     void getJointState(double state[]);
00120     void getJointStateDes(double state[]);
00121     void getJointStateErr(double state[]);
00122 
00123     static void init();
00124 
00125     //tf::Stamped<tf::Pose> getTransformIn(const char target_frame[], tf::Stamped<tf::Pose>src);
00126 
00127   // static helpers
00128     //scales the transform, e.g. scale .10 = apply returned transform 10 times on the right side to get the same result as applying in once
00129     //static tf::Stamped<tf::Pose> scaleStampedPose(const tf::Stamped<tf::Pose> &in, double scale);
00130     //static tf::Stamped<tf::Pose> scaleStampedTransform(const tf::Stamped<tf::Pose> &in, double scale);
00131 
00132     //static tf::Stamped<tf::Pose> getPoseIn(const char target_frame[], tf::Stamped<tf::Pose> src);
00133 
00134     //run inverse kinematics on a PoseStamped (7-dof pose
00135     //(position + quaternion orientation) + header specifying the
00136     //frame of the pose)
00137     //tries to stay close to double start_angles[7]
00138     //returns the solution angles in double solution[7]
00139     bool run_ik(geometry_msgs::PoseStamped pose, double start_angles[7],double solution[7], std::string link_name);
00140 
00141     bool run_ik(tf::Stamped<tf::Pose> pose, double start_angles[7],double solution[7], std::string link_name);
00142 
00143     tf::Stamped<tf::Pose> runFK(double jointAngles[], tf::Stamped<tf::Pose> *elbow = 0);
00144 
00145 
00147     btVector3 cartError();
00148 
00150     void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal,bool wait = true);
00151 
00152     // Generates a simple trajectory with two waypoints, used as an example
00153     pr2_controllers_msgs::JointTrajectoryGoal twoPointTrajectory(double *poseA, double *poseB);
00154 
00155     pr2_controllers_msgs::JointTrajectoryGoal multiPointTrajectory(const std::vector<std::vector<double> > &poses, const double &duration = 1.0);
00156 
00157     pr2_controllers_msgs::JointTrajectoryGoal multiPointTrajectory(const std::vector<std::vector<double> > &poses, const std::vector<double> &duration);
00158 
00159     pr2_controllers_msgs::JointTrajectoryGoal multiPointTrajectory(const std::vector<tf::Stamped<tf::Pose> > &poses,  const std::vector<double> &duration);
00160 
00161     pr2_controllers_msgs::JointTrajectoryGoal goalTraj(double a0, double a1, double a2, double a3, double a4, double a5, double a6, double dur=1.0);
00162     // 0 velocity at goal point
00163     pr2_controllers_msgs::JointTrajectoryGoal goalTraj(double *poseA, double dur=1.0);
00164     // some velocity at goal point
00165     pr2_controllers_msgs::JointTrajectoryGoal goalTraj(double *poseA, double *vel);
00166 
00168     actionlib::SimpleClientGoalState getState();
00169 
00170     static void printPose(const tf::Stamped<tf::Pose> &toolTargetPose);
00171 
00172     //void getToolPose(tf::Stamped<tf::Pose> &marker);
00173 
00174     //static tf::Stamped<tf::Pose> getPose(const char target_frame[],const char lookup_frame[]);
00175 
00176     tf::Stamped<tf::Pose> getToolPose(const char frame[] = "base_link");
00177 
00178     //static tf::Stamped<tf::Pose> getRelativeTransform(const char source_frameid[], const char target_frameid[]);
00179 
00180     //tf::Stamped<tf::Pose> getTransform(const char baseframe[], const char toolframe[]);
00181 
00182     void getToolPose(tf::Stamped<tf::Pose> &marker, const char frame[] = "base_link");
00183 
00184     void getWristPose(tf::Stamped<tf::Pose> &marker, const char frame[] = "base_link");
00185 
00186     //tf::Stamped<tf::Pose>  rotateAroundBaseAxis(tf::Stamped<tf::Pose> toolPose, double r_x,double r_y,double r_z);
00187     //tf::Stamped<tf::Pose>  rotateAroundToolframeAxis(tf::Stamped<tf::Pose> toolPose, double r_x,double r_y,double r_z);
00188     //static tf::Stamped<tf::Pose>  rotateAroundPose(tf::Stamped<tf::Pose> toolPose, tf::Stamped<tf::Pose> pivot, double r_x, double r_y, double r_z);
00189     //static tf::Stamped<tf::Pose>  rotateAroundPose(tf::Stamped<tf::Pose> toolPose, tf::Stamped<tf::Pose> pivot, btQuaternion qa);
00190 
00191     // rotate gripper around gripper tool frame
00192     bool rotate_toolframe_ik(double r_x, double r_y, double r_z);
00193     tf::Stamped<tf::Pose> rotate_toolframe_ik_p(double r_x, double r_y, double r_z);
00194 
00195     // rotate gripper arounr some frame down from wrist
00196     //bool rotate_toolframe_ik(double r_x, double r_y, double r_z, const char frame_id[]);
00197     //tf::Stamped<tf::Pose> rotate_toolframe_ik(tf::Stamped<tf::Pose> current,double r_x, double r_y, double r_z);
00198 
00199     //moves the toolframe to the given position
00200     bool move_toolframe_ik_pose(tf::Stamped<tf::Pose> toolTargetPose);
00201     bool move_toolframe_ik(double x, double y, double z, double ox, double oy, double oz, double ow);
00202 
00203     void stabilize_grip();
00204 
00205     // rosrun tf tf_echo /base_link /r_wrist_roll_link -> position
00206     bool move_ik(double x, double y, double z, double ox, double oy, double oz, double ow, double time = 1.0);
00207     bool move_ik(tf::Stamped<tf::Pose> targetPose, double time = 0.0);
00208 
00209     tf::Stamped<tf::Pose> tool2wrist(tf::Stamped<tf::Pose> toolPose);
00210     tf::Stamped<tf::Pose> wrist2tool(tf::Stamped<tf::Pose> toolPose);
00211 
00212     //static tf::Stamped<tf::Pose> approach(tf::Stamped<tf::Pose> toolPose, double dist = 0.1);
00213 
00214     static bool findBaseMovement(tf::Stamped<tf::Pose> &result, std::vector<int> arm, std::vector<tf::Stamped<tf::Pose> > goal, bool drive, bool reach);
00215 
00217     btVector3 universal_move_toolframe_ik_pose(tf::Stamped<tf::Pose> toolTargetPose);
00218 
00220     btVector3 universal_move_toolframe_ik_pose_tolerance(tf::Stamped<tf::Pose> toolTargetPose, double tolerance, double timeout = 5.0);
00221 
00222     btVector3 universal_move_toolframe_ik(double x, double y, double z, double ox, double oy, double oz, double ow, const char target_frame[]="base_link");
00223 
00224     //static tf::Stamped<tf::Pose> make_pose(double x, double y, double z, double ox, double oy, double oz, double ow, const char target_frame[]);
00225 
00226     //static tf::Stamped<tf::Pose> make_pose(const btTransform &trans, const char target_frame[]);
00227 
00228     void bring_into_reach(tf::Stamped<tf::Pose> toolTargetPose);
00229 
00230     bool reachable(tf::Stamped<tf::Pose> target);
00231 
00232     void moveElbowOutOfWay(tf::Stamped<tf::Pose> toolTargetPose);
00233 
00234     bool executeViaJointControl(const std::vector<tf::Stamped<tf::Pose> > &poses, int start = -1, int end = -1);
00235 
00236     bool pose2Joint(const std::vector<tf::Stamped<tf::Pose> > &poses, std::vector<std::vector<double> > &joints);
00237 
00238     static void moveBothArms(tf::Stamped<tf::Pose> leftArm, tf::Stamped<tf::Pose> rightArm, double tolerance = 0, bool wait = true);
00239 
00240     // given relative pose is relative to the root_frame, return its pose in the frame the root frame is defined in
00241     //static tf::Stamped<tf::Pose> getRel(const tf::Stamped<tf::Pose> &root_frame,  const tf::Stamped<tf::Pose> &relative_pose);
00242 
00243     //static tf::Stamped<tf::Pose> getRelInBase(const tf::Stamped<tf::Pose> &root_frame,  const btVector3 &dist);
00244 
00245     double time_to_target;
00246 
00247     bool evil_switch;
00248 
00249     bool excludeBaseProjectionFromWorkspace;
00250 
00251     tf::Stamped<tf::Pose> wrist2tool_;
00252     tf::Stamped<tf::Pose> tool2wrist_;
00253 
00254     std::string wrist_frame;
00255     std::string tool_frame;
00256 };
00257 
00258 
00259 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


ias_drawer_executive
Author(s): Thomas Ruehr
autogenerated on Thu May 23 2013 09:44:57