00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
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
00062 std::cout << "." << std::flush;
00063 if ((msg->header.stamp - lastMessageR) < ros::Duration(.1))
00064 {
00065
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;
00104
00105
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
00124
00125
00126
00127 }
00128
00129
00130 void poseCallbackL(const geometry_msgs::PoseStamped::ConstPtr& msg)
00131 {
00132
00133
00134 std::cout << "." << std::flush;
00135 if ((msg->header.stamp - lastMessageL) < ros::Duration(.1))
00136 {
00137
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;
00176
00177
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
00196
00197
00198
00199 }
00200
00201
00202 int main(int argc, char** argv)
00203 {
00204
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