ias_drawer_executive: ias_drawer_executive: RobotArm.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 #include <ias_drawer_executive/RobotArm.h>
00031 #include <ias_drawer_executive/Pressure.h>
00032 #include <ias_drawer_executive/Geometry.h>
00033 
00034 //#include <ias_drawer_executive/Poses.h>
00035 #include <ias_drawer_executive/RobotDriver.h>
00036 #include <boost/thread.hpp>
00037 #include <visualization_msgs/Marker.h>
00038 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00039 #include <kinematics_msgs/GetPositionFK.h>
00040 
00041 
00042 actionlib::SimpleActionClient<find_base_pose::FindBasePoseAction> *RobotArm::ac_fbp_ = NULL;
00043 
00044 
00045 void RobotArm::jointStateCallback(const  pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg)
00046 {
00047     mutex_.lock();
00048     for (int i = 0; i < 7; ++i)
00049     {
00050         joint_names[i] = msg->joint_names[i].c_str();
00051         //ROS_INFO("JOINT NAME msg %s", msg->joint_names[i].c_str());
00052         //ROS_INFO("JOINT NAME %s", joint_names[i].c_str());
00053 
00054         jointState[i] = msg->actual.positions[i];
00055         jointStateDes[i] = msg->desired.positions[i];
00056         jointStateErr[i] = msg->error.positions[i];
00057     }
00058     haveJointState = true;
00059     mutex_.unlock();
00060 }
00061 
00062 void RobotArm::init()
00063 {
00064     if (!listener_)
00065         listener_ = new tf::TransformListener();
00066 
00067     if (!ac_fbp_)
00068     {
00069         ac_fbp_ = new actionlib::SimpleActionClient<find_base_pose::FindBasePoseAction>("find_base_pose_action",true);
00070         ROS_INFO("waiting for fbp server");
00071         ac_fbp_->waitForServer(); //will wait for infinite time
00072         ROS_INFO("done");
00073     }
00074 
00075 }
00076 
00078 RobotArm::RobotArm(int side)
00079 {
00080 
00081     init();
00082 
00083     retries = 0;
00084 
00085     raise_elbow = false;
00086     preset_angle = 1.9;
00087 
00088     tucked = false;
00089     side_ = side;
00090     // tell the action client that we want to spin a thread by default
00091     traj_client_ = new TrajClient((side == 0) ? "r_arm_controller/joint_trajectory_action" : "l_arm_controller/joint_trajectory_action", true);
00092     // wait for action server to come up
00093     traj_client_->waitForServer(ros::Duration(1));
00094     //{
00095     //  ROS_INFO("Waiting for the joint_trajectory_action server");
00096     //}
00097 
00098     ac_ = new actionlib::SimpleActionClient<pr2_common_action_msgs::ArmMoveIKAction>((side == 0) ? "r_arm_ik" : "l_arm_ik", true);
00099 
00100 
00101     //while (ros::ok() && !
00102     ac_->waitForServer(ros::Duration(1));
00103     //)
00104     //{
00105     //ROS_INFO("Waiting for the move ik server %s", (side == 0) ? "r_arm_ik" : "l_arm_ik");
00106     //}
00107 
00108     for (int i = 0; i < 7; ++i)
00109         jointState[i] = 0.0f;
00110 
00111     haveJointState = false;
00112     jointStateSubscriber_ = n_.subscribe((side == 0) ? "/r_arm_controller/state" : "/l_arm_controller/state", 1, &RobotArm::jointStateCallback,this);
00113 
00114     //ik_client = n_.serviceClient<kinematics_msgs::GetPositionIK>((side == 0) ? "/pr2_right_arm_kinematics/get_ik" : "/pr2_left_arm_kinematics/get_ik" , true);
00115     ik_client = n_.serviceClient<kinematics_msgs::GetPositionIK>((side == 0) ? "/pr2_right_arm_kinematics/get_ik" : "/pr2_left_arm_kinematics/get_ik" , true);
00116 
00117     time_to_target = 1;
00118 
00119     evil_switch =false;
00120 
00121     ros::service::waitForService((side_==0) ? "pr2_right_arm_kinematics/get_fk_solver_info" : "pr2_left_arm_kinematics/get_fk_solver_info",1);
00122     ros::service::waitForService((side_==0) ? "pr2_right_arm_kinematics/get_fk" : "pr2_left_arm_kinematics/get_fk",1);
00123 
00124     ROS_INFO("WAITING FOR FBP AS");
00125     ac_fbp_->waitForServer(ros::Duration(1)); //will wait for infinite time
00126 
00127     query_client = n_.serviceClient<kinematics_msgs::GetKinematicSolverInfo>((side_==0) ? "pr2_right_arm_kinematics/get_fk_solver_info" : "pr2_left_arm_kinematics/get_fk_solver_info");
00128     fk_client = n_.serviceClient<kinematics_msgs::GetPositionFK>((side_==0)? "pr2_right_arm_kinematics/get_fk" : "pr2_left_arm_kinematics/get_fk");
00129 
00130     excludeBaseProjectionFromWorkspace = false;
00131 
00132     wrist_frame = (side == 0) ? "r_wrist_roll_link" : "l_wrist_roll_link";
00133     tool_frame = (side == 0) ? "r_gripper_tool_frame" : "l_gripper_tool_frame";
00134 
00135     tool2wrist_ = Geometry::getRelativeTransform(tool_frame.c_str(),wrist_frame.c_str());
00136     wrist2tool_ = Geometry::getRelativeTransform(wrist_frame.c_str(),tool_frame.c_str());
00137 
00138 }
00139 
00141 RobotArm::~RobotArm()
00142 {
00143     ac_->cancelAllGoals();
00144     traj_client_->cancelAllGoals();
00145     delete traj_client_;
00146 }
00147 
00148 RobotArm* RobotArm::getInstance(int side)
00149 {
00150     if (!instance[side])
00151         instance[side] = new RobotArm(side);
00152     return instance[side];
00153 }
00154 
00155 void RobotArm::getJointState(double state[])
00156 {
00157     haveJointState = false; //make sure to get a new state
00158     ros::Rate rate(20);
00159     while (!haveJointState)
00160     {
00161         rate.sleep();
00162         ros::spinOnce();
00163     }
00164     mutex_.lock();
00165     for (int i = 0; i < 7; ++i)
00166         state[i]=jointState[i];
00167     mutex_.unlock();
00168 }
00169 
00170 void RobotArm::getJointStateDes(double state[])
00171 {
00172     ros::Rate rate(20);
00173     while (!haveJointState)
00174     {
00175         rate.sleep();
00176         ros::spinOnce();
00177     }
00178     mutex_.lock();
00179     for (int i = 0; i < 7; ++i)
00180         state[i]=jointStateDes[i];
00181     mutex_.unlock();
00182 }
00183 
00184 void RobotArm::getJointStateErr(double state[])
00185 {
00186     ros::Rate rate(20);
00187     while (!haveJointState)
00188     {
00189         rate.sleep();
00190         ros::spinOnce();
00191     }
00192     mutex_.lock();
00193     for (int i = 0; i < 7; ++i)
00194         state[i]=jointStateErr[i];
00195     mutex_.unlock();
00196 }
00197 
00198 
00199 tf::TransformListener *RobotArm::listener_=0;
00200 
00201 //run inverse kinematics on a PoseStamped (7-dof pose
00202 //(position + quaternion orientation) + header specifying the
00203 //frame of the pose)
00204 //tries to stay close to double start_angles[7]
00205 //returns the solution angles in double solution[7]
00206 bool RobotArm::run_ik(geometry_msgs::PoseStamped pose, double start_angles[7],
00207                       double solution[7], std::string link_name)
00208 {
00209 
00210     if (0)
00211         if (excludeBaseProjectionFromWorkspace)
00212         {
00213 
00214             //bool poseInsideFootprint = false;
00215             double padding = 0.05;
00216             double x = pose.pose.position.x;
00217             double y = pose.pose.position.y;
00218             // does target pose lie inside projection of base footprint?
00219             if ((x < .325 + padding) && (x > -.325 - padding) && (y < .325 + padding) && (y > -.325 - padding))
00220             {
00221                 //poseInsideFootprint = true;
00222                 ROS_ERROR("REJECTED BECAUSE OF COLLISION WITH BASE PROJECTION");
00223                 return false;
00224             }
00225         }
00226 
00227 
00228     kinematics_msgs::GetPositionIK::Request  ik_request;
00229     kinematics_msgs::GetPositionIK::Response ik_response;
00230 
00231     ik_request.timeout = ros::Duration(5.0);
00232     if (side_ == 0)
00233     {
00234         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_shoulder_pan_joint");
00235         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_shoulder_lift_joint");
00236         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_upper_arm_roll_joint");
00237         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_elbow_flex_joint");
00238         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_forearm_roll_joint");
00239         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_wrist_flex_joint");
00240         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_wrist_roll_joint");
00241 
00242         ik_request.ik_request.robot_state.joint_state.name.push_back("r_shoulder_pan_joint");
00243         ik_request.ik_request.robot_state.joint_state.name.push_back("r_shoulder_lift_joint");
00244         ik_request.ik_request.robot_state.joint_state.name.push_back("r_upper_arm_roll_joint");
00245         ik_request.ik_request.robot_state.joint_state.name.push_back("r_elbow_flex_joint");
00246         ik_request.ik_request.robot_state.joint_state.name.push_back("r_forearm_roll_joint");
00247         ik_request.ik_request.robot_state.joint_state.name.push_back("r_wrist_flex_joint");
00248         ik_request.ik_request.robot_state.joint_state.name.push_back("r_wrist_roll_joint");
00249     }
00250     else
00251     {
00252         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("l_shoulder_pan_joint");
00253         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("l_shoulder_lift_joint");
00254         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("l_upper_arm_roll_joint");
00255         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("l_elbow_flex_joint");
00256         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("l_forearm_roll_joint");
00257         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("l_wrist_flex_joint");
00258         ik_request.ik_request.ik_seed_state.joint_state.name.push_back("l_wrist_roll_joint");
00259 
00260         ik_request.ik_request.robot_state.joint_state.name.push_back("l_shoulder_pan_joint");
00261         ik_request.ik_request.robot_state.joint_state.name.push_back("l_shoulder_lift_joint");
00262         ik_request.ik_request.robot_state.joint_state.name.push_back("l_upper_arm_roll_joint");
00263         ik_request.ik_request.robot_state.joint_state.name.push_back("l_elbow_flex_joint");
00264         ik_request.ik_request.robot_state.joint_state.name.push_back("l_forearm_roll_joint");
00265         ik_request.ik_request.robot_state.joint_state.name.push_back("l_wrist_flex_joint");
00266         ik_request.ik_request.robot_state.joint_state.name.push_back("l_wrist_roll_joint");
00267     }
00268 
00269     ik_request.ik_request.ik_link_name = wrist_frame;
00270 
00271     ik_request.ik_request.pose_stamped = pose;
00272     ik_request.ik_request.ik_seed_state.joint_state.position.resize(7);
00273     ik_request.ik_request.robot_state.joint_state.position.resize(7);
00274 
00275     for (int i=0; i<7; i++)
00276     {
00277         ik_request.ik_request.ik_seed_state.joint_state.position[i] = start_angles[i];
00278         ik_request.ik_request.robot_state.joint_state.position[i] = start_angles[i];
00279     }
00280 
00281     //ROS_INFO("request pose: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);
00282 
00283     ros::service::waitForService((side_==0) ? "pr2_right_arm_kinematics/get_ik" : "pr2_left_arm_kinematics/get_ik",1);
00284 
00285     bool ik_service_call = ik_client.call(ik_request,ik_response);
00286     if (!ik_service_call)
00287     {
00288         ROS_ERROR("IK service call failed! is ik service running? %s",side_ ? "/pr2_left_arm_kinematics/get_ik" : "/pr2_right_arm_kinematics/get_ik");
00289         return 0;
00290     }
00291     if (ik_response.error_code.val == ik_response.error_code.SUCCESS)
00292     {
00293         for (int i=0; i<7; i++)
00294         {
00295             solution[i] = ik_response.solution.joint_state.position[i];
00296         }
00297         //ROS_INFO("solution angles: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f",solution[0],solution[1], solution[2],solution[3],solution[4],solution[5],solution[6]);
00298         //ROS_INFO("IK service call succeeded");
00299         return true;
00300     }
00301     //ROS_INFO("IK service call error code: %d", ik_response.error_code.val);
00302     return false;
00303 }
00304 
00305 bool RobotArm::run_ik(tf::Stamped<tf::Pose> pose, double start_angles[7],double solution[7], std::string link_name)
00306 {
00307     if (link_name == tool_frame) {
00308         //ROS_INFO("Converting to wrist");
00309         pose = tool2wrist(pose);
00310     }
00311     pose = Geometry::getPoseIn("base_link", pose);
00312     printPose(pose);
00313     geometry_msgs::PoseStamped ps;
00314     tf::poseStampedTFToMsg(pose, ps);
00315     return run_ik(ps,start_angles,solution,wrist_frame);
00316 }
00317 
00318 
00320 void RobotArm::startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal, bool wait)
00321 {
00322 
00323     // When to start the trajectory: 1s from now
00324     goal.trajectory.header.stamp = ros::Time::now(); //; + ros::Duration(0.1);
00325 
00326     traj_client_->sendGoal(goal);
00327 
00328     if (wait)
00329     {
00330 
00331         try
00332         {
00333             traj_client_->waitForResult();
00334         }
00335         catch ( boost::thread_interrupted ti )
00336         {
00337             traj_client_->cancelAllGoals();
00338             ROS_ERROR("RobotArm startTrajectory side %i Interrupted: Thread killed. Cancelling all arm ac goals", this->side_);
00339             throw ti;
00340         }
00341     }
00342 
00343     //if (traj_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00344 //      ROS_INFO("traj action succ");
00345     //else
00346 //      ROS_INFO("traj action failed");
00347 }
00348 
00349 pr2_controllers_msgs::JointTrajectoryGoal RobotArm::goalTraj(double a0, double a1, double a2, double a3, double a4, double a5, double a6, double dur)
00350 {
00351     double a[7];
00352     a[0] = a0;
00353     a[1] = a1;
00354     a[2] = a2;
00355     a[3] = a3;
00356     a[4] = a4;
00357     a[5] = a5;
00358     a[6] = a6;
00359     return goalTraj(a, dur);
00360 }
00361 
00362 pr2_controllers_msgs::JointTrajectoryGoal RobotArm::goalTraj(double *poseA, double dur)
00363 {
00364 
00365     //ROS_INFO("JOINT TRAJECTORY CONTROL %s ARM", side_ == 0 ? "right" : "left");
00366 
00367     //our goal variable
00368     pr2_controllers_msgs::JointTrajectoryGoal goal;
00369 
00370     // First, the joint names, which apply to all waypoints
00371     if (side_==0)
00372     {
00373         goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
00374         goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
00375         goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
00376         goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
00377         goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
00378         goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
00379         goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
00380     }
00381     else
00382     {
00383         goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
00384         goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
00385         goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
00386         goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
00387         goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
00388         goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
00389         goal.trajectory.joint_names.push_back("l_wrist_roll_joint");
00390     }
00391 
00392     // We will have two waypoints in this goal trajectory
00393     goal.trajectory.points.resize(1);
00394 
00395     // First trajectory point
00396     int ind = 0;
00397     goal.trajectory.points[ind].positions.resize(7);
00398     goal.trajectory.points[ind].velocities.resize(7);
00399     for (size_t j = 0; j < 7; ++j)
00400     {
00401         goal.trajectory.points[ind].positions[j] = poseA[j];
00402         goal.trajectory.points[ind].velocities[j] = 0.0;
00403     }
00404     // To be reached 1 second after starting along the trajectory
00405     goal.trajectory.points[ind].time_from_start = ros::Duration(dur);
00406 
00407     //we are done; return the goal
00408     return goal;
00409 }
00410 
00411 pr2_controllers_msgs::JointTrajectoryGoal RobotArm::goalTraj(double *poseA, double *vel)
00412 {
00413 
00414     //ROS_INFO("JOINT TRAJECTORY CONTROL %s ARM", side_ == 0 ? "right" : "left");
00415 
00416     //our goal variable
00417     pr2_controllers_msgs::JointTrajectoryGoal goal;
00418 
00419     // First, the joint names, which apply to all waypoints
00420     if (side_==0)
00421     {
00422         goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
00423         goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
00424         goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
00425         goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
00426         goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
00427         goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
00428         goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
00429     }
00430     else
00431     {
00432         goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
00433         goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
00434         goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
00435         goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
00436         goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
00437         goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
00438         goal.trajectory.joint_names.push_back("l_wrist_roll_joint");
00439     }
00440 
00441     // We will have two waypoints in this goal trajectory
00442     goal.trajectory.points.resize(1);
00443 
00444     // First trajectory point
00445 
00446     int ind = 0;
00447     goal.trajectory.points[ind].positions.resize(7);
00448     goal.trajectory.points[ind].velocities.resize(7);
00449     for (size_t j = 0; j < 7; ++j)
00450     {
00451         goal.trajectory.points[ind].positions[j] = poseA[j];
00452         goal.trajectory.points[ind].velocities[j] = vel[j];
00453     }
00454     // To be reached 1 second after starting along the trajectory
00455     goal.trajectory.points[ind].time_from_start = ros::Duration(0.25);
00456 
00457     //we are done; return the goal
00458     return goal;
00459 }
00460 
00461 pr2_controllers_msgs::JointTrajectoryGoal RobotArm::multiPointTrajectory(const std::vector<std::vector<double> > &poses, const double &duration)
00462 {
00463 
00464     ROS_INFO("JOINT TRAJECTORY CONTROL %s ARM", side_ == 0 ? "right" : "left");
00465 
00466     pr2_controllers_msgs::JointTrajectoryGoal goal;
00467 
00468     // First, the joint names, which apply to all waypoints
00469     if (side_==0)
00470     {
00471         goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
00472         goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
00473         goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
00474         goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
00475         goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
00476         goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
00477         goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
00478     }
00479     else
00480     {
00481         goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
00482         goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
00483         goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
00484         goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
00485         goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
00486         goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
00487         goal.trajectory.joint_names.push_back("l_wrist_roll_joint");
00488     }
00489 
00490     // We will have two waypoints in this goal trajectory
00491     goal.trajectory.points.resize(poses.size());
00492 
00493     for (size_t ind = 0; ind < poses.size(); ind++)
00494     {
00495 
00496         goal.trajectory.points[ind].positions.resize(7);
00497         goal.trajectory.points[ind].velocities.resize(7);
00498 
00499         ros::Duration timetonext = ros::Duration(duration / ((double) poses.size()));
00500 
00501         for (size_t j = 0; j < 7; ++j)
00502         {
00503             goal.trajectory.points[ind].positions[j] = poses[ind][j];
00504             if (ind == poses.size() -1)
00505                 goal.trajectory.points[ind].velocities[j] = 0.0;
00506             else
00507                 goal.trajectory.points[ind].velocities[j] = (poses[ind + 1][j] - poses[ind][j]) / timetonext.toSec();
00508         }
00509         // To be reached 1 second after starting along the trajectory
00510         goal.trajectory.points[ind].time_from_start = ros::Duration((ind * duration) / ((double) poses.size()));
00511     }
00512 
00513     //we are done; return the goal
00514     return goal;
00515 }
00516 
00517 pr2_controllers_msgs::JointTrajectoryGoal RobotArm::multiPointTrajectory(const std::vector<std::vector<double> > &poses, const std::vector<double> &duration)
00518 {
00519 
00520     ROS_INFO("JOINT TRAJECTORY CONTROL %s ARM", side_ == 0 ? "right" : "left");
00521 
00522     pr2_controllers_msgs::JointTrajectoryGoal goal;
00523 
00524     // First, the joint names, which apply to all waypoints
00525     if (side_==0)
00526     {
00527         goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
00528         goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
00529         goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
00530         goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
00531         goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
00532         goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
00533         goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
00534     }
00535     else
00536     {
00537         goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
00538         goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
00539         goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
00540         goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
00541         goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
00542         goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
00543         goal.trajectory.joint_names.push_back("l_wrist_roll_joint");
00544     }
00545 
00546     // We will have two waypoints in this goal trajectory
00547     goal.trajectory.points.resize(poses.size());
00548 
00549     for (size_t ind = 0; ind < poses.size(); ind++)
00550     {
00551 
00552         ros::Duration timetonext = ros::Duration(duration[ind + 1] - duration[ind]);
00553 
00554         goal.trajectory.points[ind].positions.resize(7);
00555         goal.trajectory.points[ind].velocities.resize(7);
00556         for (size_t j = 0; j < 7; ++j)
00557         {
00558             goal.trajectory.points[ind].positions[j] = poses[ind][j];
00559             if (ind == poses.size() -1)
00560                 goal.trajectory.points[ind].velocities[j] = 0;
00561             else
00562                 goal.trajectory.points[ind].velocities[j] = (poses[ind + 1][j] - poses[ind][j]) / timetonext.toSec();
00563         }
00564         // To be reached 1 second after starting along the trajectory
00565         goal.trajectory.points[ind].time_from_start = ros::Duration(duration[ind]);
00566     }
00567 
00568     //we are done; return the goal
00569     return goal;
00570 }
00571 
00572 pr2_controllers_msgs::JointTrajectoryGoal RobotArm::multiPointTrajectory(const std::vector<tf::Stamped<tf::Pose> > &poses,  const std::vector<double> &duration)
00573 {
00574     std::vector<std::vector<double> > jointstates;
00575     pose2Joint(poses, jointstates);
00576     return multiPointTrajectory(jointstates,duration);
00577 }
00578 
00579 
00580 pr2_controllers_msgs::JointTrajectoryGoal RobotArm::twoPointTrajectory(double *poseA, double *poseB)
00581 {
00582 
00583     ROS_INFO("JOINT TRAJECTORY CONTROL %s ARM", side_ == 0 ? "right" : "left");
00584 
00585     //tucked = (poseB == Poses::tuckPose);
00586     //our goal variable
00587     pr2_controllers_msgs::JointTrajectoryGoal goal;
00588 
00589     // First, the joint names, which apply to all waypoints
00590     if (side_==0)
00591     {
00592         goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
00593         goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
00594         goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
00595         goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
00596         goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
00597         goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
00598         goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
00599     }
00600     else
00601     {
00602         goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
00603         goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
00604         goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
00605         goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
00606         goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
00607         goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
00608         goal.trajectory.joint_names.push_back("l_wrist_roll_joint");
00609     }
00610 
00611     // We will have two waypoints in this goal trajectory
00612     goal.trajectory.points.resize(2);
00613 
00614     // First trajectory point
00615 
00616     int ind = 0;
00617     goal.trajectory.points[ind].positions.resize(7);
00618     goal.trajectory.points[ind].velocities.resize(7);
00619     for (size_t j = 0; j < 7; ++j)
00620     {
00621         goal.trajectory.points[ind].positions[j] = poseA[j];
00622         goal.trajectory.points[ind].velocities[j] = 0.0;
00623     }
00624     // To be reached 1 second after starting along the trajectory
00625     goal.trajectory.points[ind].time_from_start = ros::Duration(2.0);
00626 
00627     // Second trajectory point
00628 
00629     ind += 1;
00630     goal.trajectory.points[ind].positions.resize(7);
00631     goal.trajectory.points[ind].velocities.resize(7);
00632     for (size_t j = 0; j < 7; ++j)
00633     {
00634         goal.trajectory.points[ind].positions[j] = poseB[j];
00635         goal.trajectory.points[ind].velocities[j] = 0.0;
00636     }
00637     // To be reached 2 seconds after starting along the trajectory
00638     goal.trajectory.points[ind].time_from_start = ros::Duration(4.0);
00639 
00640     //we are done; return the goal
00641     return goal;
00642 }
00643 
00644 // Returns the current state of the action
00645 actionlib::SimpleClientGoalState RobotArm::getState()
00646 {
00647     return traj_client_->getState();
00648 }
00649 
00650 void RobotArm::printPose(const tf::Stamped<tf::Pose> &toolTargetPose)
00651 {
00652     ROS_INFO("%f %f %f %f %f %f %f %s\n",
00653              toolTargetPose.getOrigin().x(),  toolTargetPose.getOrigin().y(),  toolTargetPose.getOrigin().z(),toolTargetPose.getRotation().x(),
00654              toolTargetPose.getRotation().y(),toolTargetPose.getRotation().z(),toolTargetPose.getRotation().w(),toolTargetPose.frame_id_.c_str());
00655 
00656 }
00657 
00658 
00659 tf::Stamped<tf::Pose> RobotArm::getToolPose(const char frame[])
00660 {
00661     //tf::TransformListener listener;
00662     ros::Rate rate(100.0);
00663 
00664     tf::StampedTransform transform;
00665     bool transformOk = false;
00666     while (ros::ok() && (!transformOk))
00667     {
00668         transformOk = true;
00669         try
00670         {
00671             listener_->lookupTransform(frame, (side_==0) ? "r_gripper_tool_frame" : "l_gripper_tool_frame" ,ros::Time(0), transform);
00672         }
00673         catch (tf::TransformException ex)
00674         {
00675             ROS_ERROR("getToolPose s f %s",ex.what());
00676             transformOk = false;
00677         }
00678         rate.sleep();
00679     }
00680     tf::Stamped<tf::Pose> ret;
00681     ret.frame_id_ = transform.frame_id_;
00682     ret.stamp_ = transform.stamp_;
00683     ret.setOrigin(transform.getOrigin());
00684     ret.setRotation(transform.getRotation());
00685 
00686     return ret;
00687 }
00688 
00689 
00690 
00691 void RobotArm::getToolPose(tf::Stamped<tf::Pose> &marker, const char frame[])
00692 {
00693     //tf::TransformListener listener;
00694     ros::Rate rate(100.0);
00695 
00696     tf::StampedTransform transform;
00697     bool transformOk = false;
00698     while (ros::ok() && (!transformOk))
00699     {
00700         transformOk = true;
00701         try
00702         {
00703             listener_->lookupTransform(frame, (side_==0) ? "r_gripper_tool_frame" : "l_gripper_tool_frame" ,ros::Time(0), transform);
00704         }
00705         catch (tf::TransformException ex)
00706         {
00707             ROS_ERROR("getToolPose s f %s",ex.what());
00708             transformOk = false;
00709         }
00710         rate.sleep();
00711     }
00712     marker.frame_id_ = transform.frame_id_;
00713     marker.setOrigin(transform.getOrigin());
00714     marker.setRotation(transform.getRotation());
00715     marker.stamp_ = transform.stamp_;
00716 }
00717 
00718 
00719 void RobotArm::getWristPose(tf::Stamped<tf::Pose> &marker, const char frame[])
00720 {
00721     //tf::TransformListener listener;
00722     ros::Rate rate(100.0);
00723 
00724     tf::StampedTransform transform;
00725     bool transformOk = false;
00726     while (ros::ok() && (!transformOk))
00727     {
00728         transformOk = true;
00729         try
00730         {
00731             listener_->lookupTransform(frame, (side_==0) ? "r_wrist_roll_link" : "l_wrist_roll_link" ,ros::Time(0), transform);
00732         }
00733         catch (tf::TransformException ex)
00734         {
00735             ROS_ERROR("getWristPose s f %s",ex.what());
00736             transformOk = false;
00737         }
00738         rate.sleep();
00739     }
00740     marker.frame_id_ = transform.frame_id_;
00741     marker.setOrigin(transform.getOrigin());
00742     marker.setRotation(transform.getRotation());
00743     marker.stamp_ = transform.stamp_;
00744 }
00745 
00746 
00747 tf::Stamped<tf::Pose> RobotArm::rotate_toolframe_ik_p(double r_x, double r_y, double r_z)
00748 {
00749     //ROS_INFO("ROTATING TOOLFRAME %f %f %f", r_x, r_y, r_z);
00750     tf::Stamped<tf::Pose> toolTargetPose;
00751     tf::Stamped<tf::Pose> trans;
00752     getToolPose(trans);
00753     toolTargetPose = trans;
00754     toolTargetPose.setOrigin(trans.getOrigin());
00755     toolTargetPose.setRotation(trans.getRotation());
00756 
00757     tf::Stamped<tf::Pose> toolTargetPoseRotPlus = toolTargetPose;
00758     toolTargetPose *= wrist2tool_;
00759 
00760     // printPose(toolTargetPose);
00761 
00762     // init with some yaw pitch roll
00763     tf::Stamped<tf::Pose> toolRotPlus;
00764     toolRotPlus.setOrigin(btVector3(0,0,0));
00765     toolRotPlus.setRotation(btQuaternion(r_x,r_y,r_z));
00766 
00767     toolTargetPoseRotPlus*=toolRotPlus;
00768     toolTargetPoseRotPlus*=wrist2tool_;
00769 
00770     return toolTargetPoseRotPlus;
00771 }
00772 
00773 bool RobotArm::rotate_toolframe_ik(double r_x, double r_y, double r_z)
00774 {
00775     tf::Stamped<tf::Pose> toolTargetPoseRotPlus = rotate_toolframe_ik_p(r_x, r_y, r_z);
00776 
00777     return move_ik(toolTargetPoseRotPlus.getOrigin().x(),toolTargetPoseRotPlus.getOrigin().y(),toolTargetPoseRotPlus.getOrigin().z(),
00778                    toolTargetPoseRotPlus.getRotation().x(),toolTargetPoseRotPlus.getRotation().y(),toolTargetPoseRotPlus.getRotation().z(),toolTargetPoseRotPlus.getRotation().w(),0.1);
00779 
00780     return true;
00781 }
00782 
00783 
00784 bool RobotArm::move_toolframe_ik_pose(tf::Stamped<tf::Pose> toolTargetPose)
00785 {
00786     tf::Stamped<tf::Pose> toolTargetPoseWristBase;
00787 
00788     toolTargetPoseWristBase = Geometry::getPoseIn("base_link", toolTargetPose);
00789     toolTargetPoseWristBase = tool2wrist(toolTargetPoseWristBase);
00790 
00791     //printPose(toolTargetPoseWristBase);
00792 
00793     return move_ik(toolTargetPoseWristBase.getOrigin().x(),toolTargetPoseWristBase.getOrigin().y(),toolTargetPoseWristBase.getOrigin().z(),
00794                    toolTargetPoseWristBase.getRotation().x(),toolTargetPoseWristBase.getRotation().y(),toolTargetPoseWristBase.getRotation().z(),toolTargetPoseWristBase.getRotation().w(),time_to_target);
00795 
00796 
00797     return true;
00798 }
00799 
00800 //moves the toolframe to the given position
00801 bool RobotArm::move_toolframe_ik(double x, double y, double z, double ox, double oy, double oz, double ow)
00802 {
00803     tf::Stamped<tf::Pose> toolTargetPose;
00804 
00805     toolTargetPose.frame_id_ = "base_link";
00806     toolTargetPose.stamp_ = ros::Time();
00807     toolTargetPose.setOrigin(btVector3( x,y,z));
00808     toolTargetPose.setRotation(btQuaternion(ox,oy,oz,ow));
00809 
00810     return move_toolframe_ik_pose(toolTargetPose);
00811 
00812     /*tf::Stamped<tf::Pose> wrist2tool;
00813 
00814     if (side_==0)
00815         wrist2tool.frame_id_ = "r_wrist_roll_link";
00816     else
00817         wrist2tool.frame_id_ = "l_wrist_roll_link";
00818     wrist2tool.stamp_ = ros::Time();
00819     wrist2tool.setOrigin(btVector3(-.18,0,0));
00820     wrist2tool.setRotation(btQuaternion(0,0,0,1));
00821 
00822     toolTargetPose *= wrist2tool;
00823 
00824     printPose(toolTargetPose);
00825 
00826     return move_ik(toolTargetPose.getOrigin().x(),toolTargetPose.getOrigin().y(),toolTargetPose.getOrigin().z(),
00827                    toolTargetPose.getRotation().x(),toolTargetPose.getRotation().y(),toolTargetPose.getRotation().z(),toolTargetPose.getRotation().w(),time_to_target);
00828 
00829 
00830     return true;*/
00831 }
00832 
00833 
00834 void RobotArm::stabilize_grip()
00835 {
00836     RobotArm *arm = this;
00837 
00838     double oldtime = time_to_target;
00839     time_to_target = 1.2;
00840 
00841     Pressure *p = Pressure::getInstance(side_);
00842 
00843     ros::Rate rate(25.0);
00844     double pGain = 0.02; //actually seomthing p-gainy and not an increment
00845     double limit = 0.25;
00846     long cnt_ok = 0;
00847     long cnt_bad = 0;
00848     double lastval = 0;
00849     double lastdiff = 100;
00850     double lastchange = 0; // what did we do
00851     double lasteffect = 0; // how did it change the pressure center difference?
00852     while (ros::ok() && (cnt_ok < 5) && (cnt_bad < 100))
00853     {
00854         rate.sleep();
00855         ros::spinOnce();
00856         double r[2],l[2];
00857         p->getCenter(r,l);
00858 
00859         double d[2];
00860         d[0] = r[0] - l[0];
00861         d[1] = r[1] - l[1];
00862 
00863         ROS_INFO("SIDE %i CENTERS %f %f , %f %f DIFFS %f %f", side_, r[0], r[1], l[0], l[1], d[0], d[1]);
00864 
00865         lasteffect = d[1] - lastdiff;
00866         if (lastchange != 0)
00867         {
00868             double optpGain = fabs(pGain * (d[1] / lasteffect)); // for now only positive gains, although there are some strange cases where the effect seems reversed
00869             if (optpGain > 0.1)
00870                 optpGain = 0.1;
00871             if (optpGain < -0.1)
00872                 optpGain = -0.1;
00873             ROS_INFO("change %f effect %f change in pGains %f, new pGain %f", lastchange, lasteffect, lastchange / pGain,optpGain);
00874             pGain = optpGain;
00875         }
00876 
00877         double k = d[1];
00878         double current = 0;
00879         if (k > 1)
00880             k = 1;
00881         if (k < -1)
00882             k = -1;
00883 
00884         lastchange = 0;
00885         if (fabs(d[1]) > limit)
00886         {
00887             if (d[1] > 0)
00888                 ROS_INFO("pos DIFF %f %f limit %f k %f", d[0], d[1], limit, k);
00889             else
00890                 ROS_INFO("neg DIFF %f %f limit %f k %f", d[0], d[1], limit, k);
00891 
00892             arm->rotate_toolframe_ik(pGain * k,0,0);
00893             lastchange = pGain * k;
00894             ros::spinOnce();
00895             current = +1;
00896         }
00897 
00898         if (fabs(d[1]) > fabs(lastdiff))
00899         {
00900 
00901             ROS_INFO("SOMETHINGS WRONG, GET OUT OF STABIL %f %f", fabs(d[1]), fabs(lastdiff));
00902             cnt_bad = 1000;
00903         }
00904         lastdiff = d[1];
00905 
00906         if (fabs(current - lastval) > 1.5)
00907         {
00908             ROS_INFO("LET ME OUT limit %f", limit);
00909             limit *= 2;
00910             pGain *= 0.5;
00911         }
00912 
00913         if (fabs(d[1]) < limit)
00914             cnt_ok++;
00915         else
00916             cnt_bad++;
00917     }
00918 
00919     time_to_target = oldtime;
00920 }
00921 
00922 
00923 bool RobotArm::move_ik(tf::Stamped<tf::Pose> targetPose, double time)
00924 {
00925 
00926     ROS_INFO("MOVE TIME %f", time);
00927     tf::Stamped<tf::Pose> inbase = Geometry::getPoseIn("base_link", targetPose);
00928     return move_ik(inbase.getOrigin().x(), inbase.getOrigin().y(), inbase.getOrigin().z(), inbase.getRotation().x(), inbase.getRotation().y(), inbase.getRotation().z(), inbase.getRotation().w(), time);
00929 }
00930 
00931 // rosrun tf tf_echo /base_link /r_wrist_roll_link -> position
00932 bool RobotArm::move_ik(double x, double y, double z, double ox, double oy, double oz, double ow, double time)
00933 {
00934 
00935     ROS_INFO("%s arm move ik action started %f %f %f %f %f %f %f (time %f)",((side_==0) ? "right" : "left"), x, y, z, ox, oy, oz, ow, time);
00936     //actionlib::SimpleActionClient<pr2_common_action_msgs::ArmMoveIKAction> ac("r_arm_ik", true);
00937 
00938     //ac.waitForServer(); //will wait for infinite time
00939 
00940     //ROS_INFO("server found.");
00941 
00942     pr2_common_action_msgs::ArmMoveIKGoal goal;
00943     goal.pose.header.frame_id = "base_link";
00944     goal.pose.header.stamp = ros::Time(0);
00945 
00946     goal.pose.pose.orientation.x = ox;
00947     goal.pose.pose.orientation.y = oy;
00948     goal.pose.pose.orientation.z = oz;
00949     goal.pose.pose.orientation.w = ow;
00950     goal.pose.pose.position.x = x;
00951     goal.pose.pose.position.y = y;
00952     goal.pose.pose.position.z = z;
00953 
00954     goal.ik_timeout = ros::Duration(5.0);
00955 
00956     if (side_==0)
00957     {
00958         goal.ik_seed.name.push_back("r_shoulder_pan_joint");
00959         goal.ik_seed.name.push_back("r_shoulder_lift_joint");
00960         goal.ik_seed.name.push_back("r_upper_arm_roll_joint");
00961         goal.ik_seed.name.push_back("r_elbow_flex_joint");
00962         goal.ik_seed.name.push_back("r_forearm_roll_joint");
00963         goal.ik_seed.name.push_back("r_wrist_flex_joint");
00964         goal.ik_seed.name.push_back("r_wrist_roll_joint");
00965     }
00966     else
00967     {
00968         goal.ik_seed.name.push_back("l_shoulder_pan_joint");
00969         goal.ik_seed.name.push_back("l_shoulder_lift_joint");
00970         goal.ik_seed.name.push_back("l_upper_arm_roll_joint");
00971         goal.ik_seed.name.push_back("l_elbow_flex_joint");
00972         goal.ik_seed.name.push_back("l_forearm_roll_joint");
00973         goal.ik_seed.name.push_back("l_wrist_flex_joint");
00974         goal.ik_seed.name.push_back("l_wrist_roll_joint");
00975     }
00976 
00977     ros::Rate rate(20);
00978     bool good = true;
00979     while (!good)
00980     {
00981         mutex_.lock();
00982         // start from current joint states
00983         good = true;
00984         for (int i=0; i<7; ++i)
00985         {
00986             //sumofj += jointState[i];
00987             if (jointState[i] == 0.0f)
00988                 good = false;
00989         }
00990         //ROS_INFO("Joint States %f %f %f %f %f %f %f", jointState[0],jointState[1],jointState[2],jointState[3],jointState[4],jointState[5],jointState[6]);
00991         mutex_.unlock();
00992         ros::spinOnce();
00993         rate.sleep();
00994     }
00995 
00996     mutex_.lock(); // for accessing current joint state
00997     for (int i=0; i<7; ++i)
00998     {
00999         if ((i != 2) || (!raise_elbow))
01000             goal.ik_seed.position.push_back(jointState[i]);
01001         else
01002             goal.ik_seed.position.push_back((side_==0) ? -preset_angle : preset_angle);
01003         //goal.ik_seed.position.push_back((side_==0) ? -3.1 : 3.1);
01004 
01005         //ROS_INFO("JOINT %i : %f", i, goal.ik_seed.position[i]);
01006     }
01007 
01008 
01009     mutex_.unlock();
01010     // somewhat close to what we use for looking at the drawer
01011     goal.move_duration = ros::Duration(time);
01012 
01013     ac_->sendGoal(goal);
01014 
01015     if (!evil_switch)
01016     {
01017 
01018         bool finished_before_timeout = false;
01019         try
01020         {
01021             finished_before_timeout = ac_->waitForResult(ros::Duration(time_to_target + 3.0f));
01022         }
01023         catch ( boost::thread_interrupted ti)
01024         {
01025             ac_->cancelAllGoals();
01026             ROS_ERROR("MOVE_IK side %i Interrupted: Thread killed. Cancelling all arm ac goals", this->side_);
01027             throw ti;
01028         }
01029         if (finished_before_timeout)
01030         {
01031             actionlib::SimpleClientGoalState state = ac_->getState();
01032             ROS_INFO("%s Arm Action finished: %s",((side_==0) ? "right" : "left"), state.toString().c_str());
01033             if ((state == actionlib::SimpleClientGoalState::PREEMPTED) || (state == actionlib::SimpleClientGoalState::SUCCEEDED))
01034                 return true;
01035             else
01036             {
01037                 ROS_ERROR("Action finished: %s", state.toString().c_str());
01038                 ROS_ERROR("In move_ik, with arm %i", this->side_);
01039                 ROS_ERROR("failed goal (in base): %f %f %f %f %f %f %f (time %f)", x, y, z, ox, oy, oz, ow, time);
01040                 //if (--retries > 0)
01041                 //    return move_ik(x, y, z, ox, oy, oz, ow, time);
01042                 //else
01043                 return false;
01044             }
01045         }
01046         else
01047             ROS_INFO("Action did not finish before the time out.");
01048         return false;
01049     }
01050     else return true;
01051 }
01052 
01053 
01054 RobotArm *RobotArm::instance[] = {0,0};
01055 
01056 tf::Stamped<tf::Pose> RobotArm::tool2wrist(tf::Stamped<tf::Pose> toolPose)
01057 {
01058     tf::Stamped<tf::Pose> ret;
01059     ret = toolPose;
01060     ret *= tool2wrist_;
01061     return ret;
01062 }
01063 
01064 tf::Stamped<tf::Pose> RobotArm::wrist2tool(tf::Stamped<tf::Pose> toolPose)
01065 {
01066     tf::Stamped<tf::Pose> ret;
01067     ret = toolPose;
01068     ret *= wrist2tool_;
01069     return ret;
01070 }
01071 
01072 
01073 //bool RobotArm::checkHypothesis()
01074 
01075 #include <stdio.h>
01076 #include <stdlib.h>
01077 
01078 bool RobotArm::findBaseMovement(tf::Stamped<tf::Pose> &result, std::vector<int> arm, std::vector<tf::Stamped<tf::Pose> > goal, bool drive, bool reach)
01079 {
01080 
01081     init();
01082 
01083     //system("rosrun dynamic_reconfigure dynparam set  /camera_synchronizer_node projector_mode 1");
01084 
01085     ros::NodeHandle node_handle;
01086     //ros::Publisher vis_pub = node_handle.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
01087     ros::Publisher vis_pub = node_handle.advertise<visualization_msgs::Marker>( "robot_arm_marker", 1000 );
01088 
01089     ROS_INFO("looking for base movement to satisfy ik constraints : ");
01090 
01091     if (arm.size() != goal.size())
01092     {
01093         ROS_ERROR("NUMBER OF GOAL TRAJECTORY POINTS DIFFERENT FROM ARM SELECTION");
01094         return false;
01095     }
01096 
01097     find_base_pose::FindBasePoseGoal goal_msg;
01098 
01099     bool some_outside_workspace = false; // TODO: move this test also to find_base_pose package
01100 
01101     for (size_t k=0; k < arm.size(); ++k)
01102     {
01103 
01104         geometry_msgs::PoseStamped ps;
01105         tf::poseStampedTFToMsg(goal[k],ps);
01106 
01107         std_msgs::Int32 int32;
01108         int32.data = arm[k];
01109         goal_msg.target_poses.push_back(ps);
01110         goal_msg.arm.push_back(int32);
01111 
01112         if (!some_outside_workspace)
01113         {
01114             double start_angles[7];
01115             RobotArm::getInstance(arm[k])->getJointState(start_angles);
01116             double sol_angles[7];
01117             tf::Stamped<tf::Pose> inWrist = RobotArm::getInstance(arm[k])->tool2wrist(goal[k]);
01118             geometry_msgs::PoseStamped pstamped;
01119             tf::poseStampedTFToMsg(inWrist, pstamped);
01120             bool found = RobotArm::getInstance(arm[k])->run_ik(ps, start_angles, sol_angles, (arm[k] == 0) ? "r_wrist_roll_link" : "l_wrist_roll_link");
01121             if (!found)
01122                 some_outside_workspace = true;
01123         }
01124     }
01125 
01126     if (!some_outside_workspace)
01127     {
01128         result.setOrigin(btVector3(0,0,0));
01129         result.setRotation(btQuaternion(0,0,0,1));
01130         ROS_INFO("find_base_pose: should exit now because all points are within workspace");
01131         //return  true; // TODO: why does this not work ?
01132     }
01133     //ROS_ERROR("we shouldnt be here");
01134 
01135 
01136     ROS_INFO("Waiting for action server to start.");
01137     // wait for the action server to start
01138     //ac_fbp_->waitForServer(); //will wait for infinite time
01139 
01140     ac_fbp_->sendGoal(goal_msg);
01141 
01142     bool finished_before_timeout = ac_fbp_->waitForResult(ros::Duration(15.0));
01143 
01144     if (!finished_before_timeout)
01145     {
01146         ROS_INFO("findBaseMovement Action did not finish before the time out.");
01147         return false;
01148     }
01149 
01150     actionlib::SimpleClientGoalState state = ac_fbp_->getState();
01151     ROS_INFO("Action finished: %s",state.toString().c_str());
01152     find_base_pose::FindBasePoseResultConstPtr res = ac_fbp_->getResult();
01153 
01154     if (res->base_poses.size() < 1)
01155     {
01156         ROS_INFO(" findBaseMovement Action finished before the time out but without success.");
01157         return false;
01158     }
01159 
01160     for (size_t k = 0; k < res->base_poses.size(); ++k)
01161     {
01162         tf::Stamped<tf::Pose> respose;
01163         tf::poseStampedMsgToTF(res->base_poses[k], respose);
01164         ROS_INFO("Result: %f %f  %f", res->base_poses[k].pose.position.x, res->base_poses[k].pose.position.y,respose.getRotation().getAngle());
01165     }
01166 
01167     //if (minDist < 10000)
01168     tf::Stamped<tf::Pose> respose;
01169     tf::poseStampedMsgToTF(res->base_poses[0], respose);
01170 
01171     double ps[4];
01172     ps[0] = respose.getOrigin().x();
01173     ps[1] = respose.getOrigin().y();
01174     ps[2] = respose.getRotation().z();
01175     ps[3] = respose.getRotation().w();
01176 
01177     ROS_ERROR("FOUND BASE MOVEMENT %f %f %f %f", ps[0], ps[1], ps[2], ps[3]);
01178 
01179     tf::Stamped<tf::Pose> inMap = Geometry::getPoseIn("map", respose);
01180 
01181     ROS_INFO("In Map: ");
01182     printPose(inMap);
01183 
01184     RobotDriver *driver = RobotDriver::getInstance();
01185     // todo: chck for localization, since move base is not exact! pose -> mappose -> pose
01186     //move_ik(x + best_x,y + best_y,z,ox,oy,oz,ow);
01187     if (drive && ((fabs(ps[0]) > 0.001) || (fabs(ps[1]) > 0.001) || (fabs(ps[2]) > 0.001))) // && ((fabs(best_x) > 0.01) || (fabs(best_y) > 0.01)))
01188     {
01189         //driver->driveInOdom(ps,true);
01190         boost::thread t2(&RobotDriver::driveInOdom,driver,ps,true);
01191         //boost::thread t2(&RobotDriver::driveInOdom,driver,ps,false);
01192         ros::Rate rate(25.0);
01193         while (!t2.timed_join(boost::posix_time::seconds(0.01)))
01194         {
01195             //ROS_ERROR("not yet joinable");
01196             tf::Stamped<tf::Pose> firstGoalBase = Geometry::getPoseIn("base_link",goal[0]);
01197             if (reach)
01198                 RobotArm::getInstance(arm[0])->move_toolframe_ik(firstGoalBase.getOrigin().x(), firstGoalBase.getOrigin().y(), firstGoalBase.getOrigin().z(),
01199                         firstGoalBase.getRotation().x(),firstGoalBase.getRotation().y(),firstGoalBase.getRotation().z(),firstGoalBase.getRotation().w());
01200             //RobotArm::getInstance(arm[0])->move_toolframe_ik(x + best_x,y + best_y,z,ox,oy,oz,ow);
01201             rate.sleep();
01202         }
01203         t2.join();
01204 
01205     }
01206 
01207     //result = btVector3(best_x, best_y, 0);
01208     result = respose;
01209     return true;
01210 
01211 
01212 }
01213 
01214 /*
01215 tf::Stamped<tf::Pose> RobotArm::make_pose(double x, double y, double z, double ox, double oy, double oz, double ow, const char target_frame[])
01216 {
01217     tf::Stamped<tf::Pose> toolTargetPose;
01218 
01219     toolTargetPose.frame_id_ = target_frame;
01220     toolTargetPose.stamp_ = ros::Time(0);
01221     toolTargetPose.setOrigin(btVector3( x,y,z));
01222     toolTargetPose.setRotation(btQuaternion(ox,oy,oz,ow));
01223 
01224     return toolTargetPose;
01225 }
01226 
01227 tf::Stamped<tf::Pose> RobotArm::make_pose(const btTransform &trans, const char target_frame[])
01228 {
01229     tf::Stamped<tf::Pose> toolTargetPose;
01230     toolTargetPose.frame_id_ = target_frame;
01231     toolTargetPose.stamp_ = ros::Time(0);
01232     toolTargetPose.setOrigin(trans.getOrigin());
01233     toolTargetPose.setRotation(trans.getRotation());
01234 
01235     return toolTargetPose;
01236 }
01237 */
01238 
01239 
01240 //  move the toolframe including base motions when necessary
01241 btVector3 RobotArm::universal_move_toolframe_ik(double x, double y, double z, double ox, double oy, double oz, double ow, const char target_frame[])
01242 {
01243     tf::Stamped<tf::Pose> toolTargetPose;
01244 
01245     toolTargetPose.frame_id_ = target_frame;
01246     toolTargetPose.stamp_ = ros::Time(0);
01247     toolTargetPose.setOrigin(btVector3( x,y,z));
01248     toolTargetPose.setRotation(btQuaternion(ox,oy,oz,ow));
01249 
01250     return universal_move_toolframe_ik_pose(toolTargetPose);
01251 }
01252 
01253 void RobotArm::bring_into_reach(tf::Stamped<tf::Pose> toolTargetPose)
01254 {
01255     std::vector<int> arm;
01256     std::vector<tf::Stamped<tf::Pose> > goal;
01257     tf::Stamped<tf::Pose> result;
01258     arm.push_back(side_);
01259     //toolTargetPose *= wrist2tool;
01260     tf::Stamped<tf::Pose> toolTargetPoseInMap = Geometry::getPoseIn("map",toolTargetPose);
01261     goal.push_back(toolTargetPoseInMap);
01262     //goal.push_back(toolTargetPose);
01263     ROS_INFO("BRING INTO  REACH ");
01264     RobotArm::findBaseMovement(result, arm, goal, true, false);
01265 }
01266 
01267 bool RobotArm::reachable(tf::Stamped<tf::Pose> target)
01268 {
01269 
01270     tf::Stamped<tf::Pose> toolTargetPoseInBase = Geometry::getPoseIn("base_link",target);
01271     //ROS_INFO("tool in base_link");
01272     //printPose(toolTargetPoseInBase);
01273 
01274     tf::Stamped<tf::Pose> wristTargetBase = tool2wrist(toolTargetPoseInBase);
01275 
01276     //ROS_INFO("wrist in base_link");
01277     //printPose(wristTargetBase);
01278 
01279     geometry_msgs::PoseStamped stamped_pose;
01280     tf::poseStampedTFToMsg(wristTargetBase,stamped_pose);
01281 
01282     double new_state[7];
01283 
01284     double start_angles[7];
01285     getJointState(start_angles);
01286 
01287     if (raise_elbow)
01288         start_angles[2] = ((side_==0) ? -preset_angle : preset_angle);
01289 
01290     return run_ik(stamped_pose,start_angles,new_state,wrist_frame);
01291 
01292 }
01293 
01294 
01295 btVector3 RobotArm::universal_move_toolframe_ik_pose(tf::Stamped<tf::Pose> toolTargetPose)
01296 {
01297     tf::Stamped<tf::Pose> toolTargetPoseInBase = Geometry::getPoseIn("base_link",toolTargetPose);
01298     ROS_INFO("tool in base_link");
01299     printPose(toolTargetPoseInBase);
01300 
01301     tf::Stamped<tf::Pose> toolPose;
01302     getToolPose(toolPose, "base_link");
01303     //ROS_INFO("tool pose in base link");
01304     //printPose(toolPose);
01305 
01306     tf::Stamped<tf::Pose> wristTargetBase = tool2wrist(toolTargetPoseInBase);
01307 
01308     ROS_INFO("wrist in base_link");
01309     printPose(wristTargetBase);
01310 
01311     geometry_msgs::PoseStamped stamped_pose;
01312     tf::poseStampedTFToMsg(wristTargetBase,stamped_pose);
01313 
01314     double new_state[7];
01315 
01316     double start_angles[7];
01317     getJointState(start_angles);
01318 
01319     if (raise_elbow)
01320         start_angles[2] = ((side_==0) ? -preset_angle : preset_angle);
01321 
01322     bool foundInitially = run_ik(stamped_pose,start_angles,new_state,wrist_frame);
01323 
01324     //if (poseInsideFootprint && excludeBaseProjectionFromWorkspace)
01325     // foundInitially = false;
01326     bool poseInsideFootprint = false;
01327     if (0)
01328         if (excludeBaseProjectionFromWorkspace)
01329         {
01330             ROS_WARN("excludeBaseProjectionFromWorkspace == true");
01331 
01332             double padding = 0.05;
01333             double x = stamped_pose.pose.position.x;
01334             double y = stamped_pose.pose.position.y;
01335             // does target pose lie inside projection of base footprint?
01336             if ((x < .325 + padding) && (x > -.325 - padding) && (y < .325 + padding) && (y > -.325 - padding))
01337                 poseInsideFootprint = true;
01338         }
01339 
01340 
01341 //if (!move_ik(x,y,z,ox,oy,oz,ow,time_to_target))
01342     if (foundInitially && !poseInsideFootprint && move_ik(wristTargetBase.getOrigin().x(),wristTargetBase.getOrigin().y(),wristTargetBase.getOrigin().z(),
01343             wristTargetBase.getRotation().x(),wristTargetBase.getRotation().y(),wristTargetBase.getRotation().z(),wristTargetBase.getRotation().w(),time_to_target))
01344     {
01345         if (!foundInitially)
01346             ROS_ERROR("RUN IK SAYS NOT FOUND INITIALLY BUT WE REACHED IT WITH MOVE_IK");
01347 
01348         return btVector3(0,0,0);
01349     }
01350     else
01351     {
01352         if (foundInitially)
01353         {
01354             ROS_ERROR("RobotArm::universal_move_toolframe_ik run_ik says reachable move_ik says not!");
01355         }
01356         else
01357         {
01358             ROS_ERROR("NOT FOUND INITIALLY");
01359         }
01360         ROS_INFO("looking for base movement to reach tf %f %f %f %f %f %f %f in %s", toolTargetPose.getOrigin().x(),toolTargetPose.getOrigin().y(),toolTargetPose.getOrigin().z(),
01361                  toolTargetPose.getRotation().x(),toolTargetPose.getRotation().y(),toolTargetPose.getRotation().z(),toolTargetPose.getRotation().w(), toolTargetPose.frame_id_.c_str());
01362 
01363         std::vector<int> arm;
01364         std::vector<tf::Stamped<tf::Pose> > goal;
01365         tf::Stamped<tf::Pose> result;
01366         arm.push_back(side_);
01367         //toolTargetPose *= wrist2tool;
01368         tf::Stamped<tf::Pose> toolTargetPoseInMap = Geometry::getPoseIn("map",toolTargetPose);
01369         goal.push_back(toolTargetPoseInMap);
01370         //goal.push_back(toolTargetPose);
01371         RobotArm::findBaseMovement(result, arm, goal, true, true);
01372         return universal_move_toolframe_ik_pose(toolTargetPoseInMap);
01373     }
01374 
01375 
01376     return btVector3(0,0,0);
01377 }
01378 
01379 
01380 btVector3 RobotArm::universal_move_toolframe_ik_pose_tolerance(tf::Stamped<tf::Pose> toolTargetPose, double tolerance, double timeout)
01381 {
01382 
01383     evil_switch = true;
01384 
01385     ros::Time start = ros::Time::now();
01386 
01387     universal_move_toolframe_ik_pose(toolTargetPose);
01388 
01389     bool reached = false;
01390     while (!reached)
01391     {
01392         reached = (getToolPose(toolTargetPose.frame_id_.c_str()).getOrigin() - toolTargetPose.getOrigin()).length() < tolerance;
01393         if ((ros::Time::now() - start) > ros::Duration(timeout))
01394             reached = true;
01395         ros::Duration(0.001).sleep();
01396     }
01397 
01398     ROS_INFO("universal_move_toolframe_ik_pose_tolerance DIST : %f ", (getToolPose(toolTargetPose.frame_id_.c_str()).getOrigin() - toolTargetPose.getOrigin()).length());
01399 
01400     evil_switch = false;
01401 
01402     return btVector3(0,0,0);
01403 }
01404 
01405 
01406 
01407 tf::Stamped<tf::Pose> RobotArm::runFK(double jointAngles[], tf::Stamped<tf::Pose> *elbow)
01408 {
01409 
01410     tf::Stamped<tf::Pose> res;
01411     // define the service messages
01412     kinematics_msgs::GetKinematicSolverInfo::Request request;
01413     kinematics_msgs::GetKinematicSolverInfo::Response response;
01414 
01415     ros::service::waitForService((side_==0) ? "pr2_right_arm_kinematics/get_fk_solver_info" : "pr2_left_arm_kinematics/get_fk_solver_info");
01416 
01417     if (query_client.call(request,response))
01418     {
01419 
01420         //for (unsigned int i=0; i < response.kinematic_solver_info.joint_names.size(); i++)
01421         // {
01422         //   ROS_INFO("Joint: %d %s", i,response.kinematic_solver_info.joint_names[i].c_str());
01423         // }
01424     }
01425     else
01426     {
01427         ROS_ERROR("Could not call query service");
01428         //ros::shutdown();
01429         //exit(1);
01430         return res;
01431     }
01432     // define the service messages
01433     kinematics_msgs::GetPositionFK::Request  fk_request;
01434     kinematics_msgs::GetPositionFK::Response fk_response;
01435     //fk_request.header.frame_id = "torso_lift_link";
01436     //fk_request.header.frame_id = "torso_lift_joint";
01437     fk_request.header.frame_id = "base_link";
01438     fk_request.fk_link_names.resize(2);
01439     //for (int i = 0; i < 7; ++i) {
01440     //ROS_INFO("LINK %i : %s", i, joint_names[i].c_str());
01441     //fk_request.fk_link_names[i] = joint_names[i];
01442     //}
01443     fk_request.fk_link_names[0] = (side_==0) ? "r_wrist_roll_link" : "l_wrist_roll_link";
01444     fk_request.fk_link_names[1] = (side_==0) ? "r_elbow_flex_link" : "l_elbow_flex_link" ;
01445     //fk_request.fk_link_names[0] = "r_wrist_roll_link";
01446     //fk_request.fk_link_names[1] = "r_elbow_flex_link";
01447     //fk_request.robot_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
01448     fk_request.robot_state.joint_state.position.resize(7);
01449     fk_request.robot_state.joint_state.name=response.kinematic_solver_info.joint_names;
01450     for (unsigned int i=0;
01451             i< response.kinematic_solver_info.joint_names.size(); i++)
01452     {
01453         //ROS_INFO("LINK %i : %f", i, jointAngles[i]);
01454         fk_request.robot_state.joint_state.position[i] = jointAngles[i];
01455     }
01456 
01457     ros::service::waitForService((side_==0) ? "pr2_right_arm_kinematics/get_fk" : "pr2_left_arm_kinematics/get_fk");
01458     if (fk_client.call(fk_request, fk_response))
01459     {
01460         //if (0)
01461         if (fk_response.error_code.val == fk_response.error_code.SUCCESS)
01462         {
01463             if (0)
01464                 for (unsigned int i=0; i < fk_response.pose_stamped.size(); i ++)
01465                 {
01466                     ROS_INFO_STREAM("Link    : " << fk_response.fk_link_names[i].c_str());
01467                     ROS_INFO_STREAM("Position: " <<
01468                                     fk_response.pose_stamped[i].pose.position.x << "," <<
01469                                     fk_response.pose_stamped[i].pose.position.y << "," <<
01470                                     fk_response.pose_stamped[i].pose.position.z);
01471                     ROS_INFO("Orientation: %f %f %f %f",
01472                              fk_response.pose_stamped[i].pose.orientation.x,
01473                              fk_response.pose_stamped[i].pose.orientation.y,
01474                              fk_response.pose_stamped[i].pose.orientation.z,
01475                              fk_response.pose_stamped[i].pose.orientation.w);
01476                 }
01477 
01478 
01479             res.frame_id_ = fk_request.header.frame_id;
01480             int o = 0;
01481             res.setOrigin(btVector3(fk_response.pose_stamped[o].pose.position.x, fk_response.pose_stamped[o].pose.position.y, fk_response.pose_stamped[o].pose.position.z));
01482             res.setRotation(btQuaternion(fk_response.pose_stamped[o].pose.orientation.x,fk_response.pose_stamped[o].pose.orientation.y,fk_response.pose_stamped[o].pose.orientation.z,fk_response.pose_stamped[o].pose.orientation.w));
01483 
01484             if (elbow)
01485             {
01486                 res.frame_id_ = fk_request.header.frame_id;
01487                 int o = 1;
01488                 elbow->setOrigin(btVector3(fk_response.pose_stamped[o].pose.position.x, fk_response.pose_stamped[o].pose.position.y, fk_response.pose_stamped[o].pose.position.z));
01489                 elbow->setRotation(btQuaternion(fk_response.pose_stamped[o].pose.orientation.x,fk_response.pose_stamped[o].pose.orientation.y,fk_response.pose_stamped[o].pose.orientation.z,fk_response.pose_stamped[o].pose.orientation.w));
01490             }
01491         }
01492         else
01493         {
01494             ROS_ERROR("Forward kinematics failed");
01495         }
01496     }
01497     else
01498     {
01499         ROS_ERROR("Forward kinematics service call failed");
01500     }
01501 
01502 
01503     //fk_request.fk_link_names[1] = "r_elbow_flex_link";
01504 
01505     return res;
01506 }
01507 
01508 
01509 btVector3 RobotArm::cartError()
01510 {
01511     double state[7];
01512     getJointState(state);
01513     double stateDes[7];
01514     getJointStateDes(stateDes);
01515     tf::Stamped<tf::Pose> posAct = runFK(state);
01516     tf::Stamped<tf::Pose> posDes = runFK(stateDes);
01517     btVector3 diff = posDes.getOrigin() - posAct.getOrigin();
01518     //ROS_INFO("DIFF %f %f %f DISTANCE %f", diff.x(), diff.y(), diff.y(), diff.length());
01519     return diff;
01520 }
01521 
01522 
01523 void RobotArm::moveElbowOutOfWay(tf::Stamped<tf::Pose> toolTargetPose)
01524 {
01525 
01526     RobotArm *arm = this;
01527     double state[7];
01528     arm->getJointState(state);
01529 
01530     tf::Stamped<tf::Pose> actPose;
01531     actPose = tool2wrist(Geometry::getPoseIn("base_link", toolTargetPose));
01532 
01533 
01534     //arm->getWristPose(actPose,"base_link");
01535 
01536     geometry_msgs::PoseStamped stamped_pose;
01537     stamped_pose.header.frame_id = "base_link";
01538     stamped_pose.header.stamp = ros::Time::now();
01539     stamped_pose.pose.position.x=actPose.getOrigin().x();
01540     stamped_pose.pose.position.y=actPose.getOrigin().y();
01541     stamped_pose.pose.position.z=actPose.getOrigin().z();
01542     stamped_pose.pose.orientation.x=actPose.getRotation().x();
01543     stamped_pose.pose.orientation.y=actPose.getRotation().y();
01544     stamped_pose.pose.orientation.z=actPose.getRotation().z();
01545     stamped_pose.pose.orientation.w=actPose.getRotation().w();
01546 
01547     ros::Rate rate(50.0);
01548 
01549     //for (double add = 0; ros::ok(); add+= 0.01)
01550     while (ros::ok())
01551     {
01552         rate.sleep();
01553         ros::spinOnce();
01554 
01555         double jErr[7];
01556         double jDes[7];
01557 
01558         arm->getJointStateErr(jErr);
01559         arm->getJointStateDes(jDes);
01560 
01561         double increment = .1;
01562 
01563         ROS_INFO("jERR[1]=%f", jErr[1]);
01564 
01565         if (fabs(jErr[1]) > 0.0001)
01566         {
01567             // {
01568 
01569             double stateP[7];
01570             double statePK[7];
01571             double stateM[7];
01572             //double solStateP[7];
01573             //double solStatePK[7];
01574             //double solStateM[7];
01575 
01576             arm->getJointState(stateP);
01577             arm->getJointState(statePK);
01578             arm->getJointState(stateM);
01579 
01580             double stA[7];
01581             double stB[7];
01582             double stC[7];
01583             arm->getJointState(stA);
01584             arm->getJointState(stB);
01585             arm->getJointState(stC);
01586 
01587             stB[2] += increment;
01588 
01589             double stAs[7];
01590             double stBs[7];
01591             double stCs[7];
01592 
01593             arm->run_ik(stamped_pose,stA,stAs,wrist_frame);
01594             arm->run_ik(stamped_pose,stB,stBs,wrist_frame);
01595 
01596             double newinc = (jErr[1] / (stBs[1] - stAs[1])) * increment;
01597 
01598             stC[2] += newinc;
01599 
01600             arm->run_ik(stamped_pose,stC,stCs,wrist_frame);
01601 
01602             ROS_INFO("curr (sta[1]) %f  stCs %f", stA[1], stCs[1]);
01603 
01604             double pose[7];
01605             double sum = 0;
01606             for (int k = 0; k < 7; ++k)
01607             {
01608                 pose[k] = stCs[k];
01609                 sum += stCs[k] * stCs[k];
01610             }
01611 
01612             if (sum > 0.01)
01613                 arm->startTrajectory(arm->goalTraj(pose));
01614 
01615             if (stBs[2]==0)
01616             {
01617                 increment = -increment;
01618                 ROS_INFO("HIT LIMIT?");
01619             }
01620         }
01621         else
01622         {
01623             break;
01624         }
01625     }
01626 }
01627 
01628 void RobotArm::moveBothArms(tf::Stamped<tf::Pose> leftArm, tf::Stamped<tf::Pose> rightArm, double tolerance, bool wait)
01629 {
01630     std::vector<int> arm;
01631     std::vector<tf::Stamped<tf::Pose> > goal;
01632     tf::Stamped<tf::Pose> result;
01633 
01634     arm.push_back(1);
01635     goal.push_back(leftArm);
01636     arm.push_back(0);
01637     goal.push_back(rightArm);
01638 
01639     RobotArm::findBaseMovement(result, arm, goal,true, false);
01640 
01641     if (!wait)
01642     {
01643         RobotArm::getInstance(0)->evil_switch = true;
01644         RobotArm::getInstance(1)->evil_switch = true;
01645     }
01646 
01647     if (tolerance == 0)
01648     {
01649         boost::thread t1(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(0),rightArm);
01650         boost::thread t2(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(1),leftArm);
01651         t2.join();
01652         t1.join();
01653     }
01654     else
01655     {
01656         boost::thread t1(&RobotArm::universal_move_toolframe_ik_pose_tolerance,RobotArm::getInstance(0),rightArm,tolerance,5);
01657         boost::thread t2(&RobotArm::universal_move_toolframe_ik_pose_tolerance,RobotArm::getInstance(1),leftArm,tolerance,5);
01658         t2.join();
01659         t1.join();
01660     }
01661 
01662     if (!wait)
01663     {
01664         RobotArm::getInstance(0)->evil_switch = false;
01665         RobotArm::getInstance(1)->evil_switch = false;
01666     }
01667 
01668 }
01669 
01670 
01671 bool RobotArm::pose2Joint(const std::vector<tf::Stamped<tf::Pose> > &poses, std::vector<std::vector<double> > &joints)
01672 {
01673 
01674     joints.resize(poses.size());
01675 
01676     double current_state[7];
01677     getJointState(current_state);
01678 
01679     for (size_t k = 0; k < poses.size(); ++k)
01680     {
01681 
01682         printPose(poses[k]);
01683         double solution[7];
01684         bool found = run_ik(poses[k], current_state,solution, tool_frame);
01685         if (!found) {
01686             ROS_ERROR("Not all poses of trajectory found %zu of %i", k, poses.size());
01687             return false;
01688         }
01689         joints[k] = std::vector<double>();
01690         joints[k].resize(7);
01691         for (size_t j = 0; j < 7; j++) {
01692             joints[k][j] = solution[j];
01693             //printf("%f\n", joints[k][j] );
01694         }
01695     }
01696     return true;
01697 }
01698 
01699 bool RobotArm::executeViaJointControl(const std::vector<tf::Stamped<tf::Pose> > &poses, int start, int end)
01700 {
01701     std::vector<std::vector<double> > joint_angles;
01702     if (start < 0)
01703         start = 0;
01704     if (end < 0)
01705         end = poses.size() - 1;
01706     joint_angles.resize(end - start);
01707     int direction = (end > start) ? +1 : -1;
01708     joint_angles.resize(std::max(start,end));
01709 
01710     double current_state[7];
01711     getJointState(current_state);
01712 
01713     for (int k = start; k != end + direction; k += direction)
01714     {
01715         double solution[7];
01716         bool found = run_ik(poses[k], current_state,solution, tool_frame);
01717         if (!found)
01718             return false;
01719         joint_angles[k].resize(7);
01720         for (size_t j = 0; j < 7; j++)
01721             joint_angles[k][j] = solution[j];
01722     }
01723 
01724     return true;
01725 }