ias_drawer_executive: ias_drawer_executive: CartesianViaJoint.cpp Source 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 // roslaunch arm_ik.launch
00031 #include <ros/ros.h>
00032 
00033 #include <ias_drawer_executive/RobotArm.h>
00034 
00035 void printPose(const char title[], tf::Stamped<tf::Pose> pose)
00036 {
00037     ROS_INFO("%s %f %f %f %f %f %f %f %s", title, pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()
00038              , pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w(), pose.frame_id_.c_str());
00039 }
00040 
00041 ros::Time lastMessageR;
00042 ros::Time lastMessageL;
00043 
00044 void poseCallbackRMoveArm(const geometry_msgs::PoseStamped::ConstPtr& msg)
00045 {
00046 
00047     if ((ros::Time::now() - lastMessageR) < ros::Duration(0.3))
00048       return;
00049     lastMessageR = ros::Time::now();
00050 
00051     tf::Stamped<tf::Pose> nextPose;
00052     tf::poseStampedMsgToTF(*msg, nextPose);
00053     printPose("target pose: ", nextPose);
00054     RobotArm::getInstance(0)->time_to_target = 0;
00055     RobotArm::getInstance(0)->move_ik(nextPose,0);
00056 }
00057 
00058 void poseCallbackR(const geometry_msgs::PoseStamped::ConstPtr& msg)
00059 {
00060 
00061     //ROS_INFO("seq %i stamp %i.%i", msg->header.seq,msg->header.stamp.sec, msg->header.stamp.nsec );
00062     std::cout << "." << std::flush;
00063     if ((msg->header.stamp - lastMessageR) < ros::Duration(.1))
00064     {
00065       //  ROS_ERROR("too young");
00066         return;
00067     }
00068 
00069     if ((ros::Time::now() - msg->header.stamp) > ros::Duration(0.5))
00070     {
00071         ROS_ERROR("too old %i.%i", (ros::Time::now() - msg->header.stamp).sec, (ros::Time::now() - msg->header.stamp).nsec );
00072         return;
00073     }
00074 
00075     lastMessageR = msg->header.stamp;
00076 
00077     double start_angles[7];
00078     double solutions[7];
00079     RobotArm::getInstance(0)->getJointState(start_angles);
00080 
00081     tf::Stamped<tf::Pose> nextPose;
00082     tf::poseStampedMsgToTF(*msg, nextPose);
00083 
00084     tf::Stamped<tf::Pose> actPose = RobotArm::getInstance(0)->getToolPose("map");
00085     tf::Stamped<tf::Pose> nxtPose = RobotArm::getInstance(0)->getPoseIn("map",nextPose);
00086     float dist = (actPose.getOrigin() - nxtPose.getOrigin()).length();
00087 
00088     float dist_limit = 0.5;
00089 
00090     if (dist > dist_limit) {
00091         btVector3 act = actPose.getOrigin();
00092         btVector3 rel = nxtPose.getOrigin() - actPose.getOrigin();
00093         ROS_INFO("BFR REL LENGTH : %f", rel.length());
00094         float dfactor = dist_limit / rel.length();
00095         rel *= dfactor;
00096         ROS_INFO("AFT REL LENGTH : %f", rel.length());
00097         nxtPose.setOrigin(rel + act);
00098         dist = dist_limit;
00099     }
00100 
00101     nextPose = nxtPose;
00102 
00103     float timeToTarget = dist / 0.1; // 50 cm per sec
00104 
00105     //ROS_INFO(". dist %f timetotarg %f", dist, timeToTarget);
00106 
00107     nextPose = RobotArm::getInstance(0)->tool2wrist(nextPose);
00108     geometry_msgs::PoseStamped inWrist;
00109     tf::poseStampedTFToMsg(nextPose,inWrist);
00110 
00111     bool ik_res = RobotArm::getInstance(0)->run_ik(inWrist, start_angles,solutions,"r_wrist_roll_link");
00112 
00113     printPose("target pose: ", nextPose);
00114 
00115     if (!ik_res)  {
00116         ROS_ERROR("outside bounds");
00117         return;
00118     }
00119 
00120     float solutionsf[7]; for (int i=0;i<7;++i) solutionsf[i] = solutions[i];
00121     RobotArm::getInstance(0)->startTrajectory(RobotArm::getInstance(0)->goalTraj(solutionsf, timeToTarget),false);
00122 
00123     //tf::Stamped<tf::Pose> nextPose;
00124     //tf::poseStampedMsgToTF(*msg, nextPose);
00125     //RobotArm::getInstance(0)->time_to_target = 0;
00126     //RobotArm::getInstance(0)->move_ik(nextPose,0);
00127 }
00128 
00129 
00130 void poseCallbackL(const geometry_msgs::PoseStamped::ConstPtr& msg)
00131 {
00132 
00133     //ROS_INFO("seq %i stamp %i.%i", msg->header.seq,msg->header.stamp.sec, msg->header.stamp.nsec );
00134     std::cout << "." << std::flush;
00135     if ((msg->header.stamp - lastMessageL) < ros::Duration(.1))
00136     {
00137       //  ROS_ERROR("too young");
00138         return;
00139     }
00140 
00141     if ((ros::Time::now() - msg->header.stamp) > ros::Duration(0.5))
00142     {
00143         ROS_ERROR("too old %i.%i", (ros::Time::now() - msg->header.stamp).sec, (ros::Time::now() - msg->header.stamp).nsec );
00144         return;
00145     }
00146 
00147     lastMessageL = msg->header.stamp;
00148 
00149     double start_angles[7];
00150     double solutions[7];
00151     RobotArm::getInstance(1)->getJointState(start_angles);
00152 
00153     tf::Stamped<tf::Pose> nextPose;
00154     tf::poseStampedMsgToTF(*msg, nextPose);
00155 
00156     tf::Stamped<tf::Pose> actPose = RobotArm::getInstance(1)->getToolPose("map");
00157     tf::Stamped<tf::Pose> nxtPose = RobotArm::getInstance(1)->getPoseIn("map",nextPose);
00158     float dist = (actPose.getOrigin() - nxtPose.getOrigin()).length();
00159 
00160     float dist_limit = 0.5;
00161 
00162     if (dist > dist_limit) {
00163         btVector3 act = actPose.getOrigin();
00164         btVector3 rel = nxtPose.getOrigin() - actPose.getOrigin();
00165         ROS_INFO("BFR REL LENGTH : %f", rel.length());
00166         float dfactor = dist_limit / rel.length();
00167         rel *= dfactor;
00168         ROS_INFO("AFT REL LENGTH : %f", rel.length());
00169         nxtPose.setOrigin(rel + act);
00170         dist = dist_limit;
00171     }
00172 
00173     nextPose = nxtPose;
00174 
00175     float timeToTarget = dist / 0.1; // 50 cm per sec
00176 
00177     //ROS_INFO(". dist %f timetotarg %f", dist, timeToTarget);
00178 
00179     nextPose = RobotArm::getInstance(1)->tool2wrist(nextPose);
00180     geometry_msgs::PoseStamped inWrist;
00181     tf::poseStampedTFToMsg(nextPose,inWrist);
00182 
00183     bool ik_res = RobotArm::getInstance(1)->run_ik(inWrist, start_angles,solutions,"l_wrist_roll_link");
00184 
00185     printPose("target pose: ", nextPose);
00186 
00187     if (!ik_res)  {
00188         ROS_ERROR("outside bounds");
00189         return;
00190     }
00191 
00192     float solutionsf[7]; for (int i=0;i<7;++i) solutionsf[i] = solutions[i];
00193     RobotArm::getInstance(1)->startTrajectory(RobotArm::getInstance(1)->goalTraj(solutionsf, timeToTarget),false);
00194 
00195     //tf::Stamped<tf::Pose> nextPose;
00196     //tf::poseStampedMsgToTF(*msg, nextPose);
00197     //RobotArm::getInstance(0)->time_to_target = 0;
00198     //RobotArm::getInstance(0)->move_ik(nextPose,0);
00199 }
00200 
00201 
00202 int main(int argc, char** argv)
00203 {
00204     // Init the ROS node
00205     ros::init(argc, argv, "CartesianViaJoint");
00206 
00207 
00208 
00209     ros::NodeHandle node_handle;
00210 
00211     lastMessageR = ros::Time::now();
00212     lastMessageL = ros::Time::now();
00213 
00214     ros::Subscriber subsRight = node_handle.subscribe("/r_cartesian_via_joint/command", 1, &poseCallbackR);
00215     ros::Subscriber subsLeft = node_handle.subscribe("/l_cartesian_via_joint/command", 1, &poseCallbackL);
00216 
00217     ROS_INFO("READY");
00218 
00219     ros::spin();
00220 }
00221