katana_openrave_test: katana_openrave_test: get_constraint_aware_ik

00001 #include <ros/ros.h>
00002 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00003 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00004 #include <sensor_msgs/JointState.h>
00005 #include <ompl/util/RandomNumbers.h>
00006 #include <tf/transform_broadcaster.h>
00007 #include <tf/transform_datatypes.h>
00008 #include <arm_navigation_msgs/DisplayTrajectory.h>
00009 
00010 int main(int argc, char **argv)
00011 {
00012   ros::init(argc, argv, "get_constraint_aware_ik_test");
00013   ros::NodeHandle rh;
00014   ros::NodeHandle pn("~");
00015 
00016   ROS_DEBUG("Starting 'get_constraint_aware_ik_test'...");
00017 
00018   std::string ik_service;
00019   pn.param<std::string> ("ik_service", ik_service, "katana_constraint_aware_kinematics/get_constraint_aware_ik");
00020 
00021   ros::service::waitForService(ik_service);
00022   ros::service::waitForService("katana_constraint_aware_kinematics/get_ik_solver_info");
00023 
00024 //  ros::Publisher pub =
00025   //   rh.advertise<arm_navigation_msgs::DisplayTrajectory> ("planned_robot_pose", 1000);
00026   ros::ServiceClient ik_client_ =
00027       rh.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK> (ik_service);
00028   ros::ServiceClient query_client =
00029       rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo> ("katana_constraint_aware_kinematics/get_ik_solver_info");
00030   ROS_ERROR("Connect to the necessary services...");
00031 
00032   // define the query service messages
00033   ompl::RNG rng = ompl::RNG();
00034   kinematics_msgs::GetKinematicSolverInfo::Request request;
00035   kinematics_msgs::GetKinematicSolverInfo::Response response;
00036 
00037   sensor_msgs::JointState initialState;
00038   arm_navigation_msgs::DisplayTrajectory planned_robot_pose;
00039 
00040   if (query_client.call(request, response))
00041   {
00042     for (unsigned int i = 0; i < response.kinematic_solver_info.joint_names.size(); i++)
00043     {
00044    //   ROS_INFO("Joint: %d %s", i,
00045      //     response.kinematic_solver_info.joint_names[i].c_str());
00046     }
00047 
00048     // publish initial joint state
00049     initialState.header.stamp = ros::Time::now();
00050     initialState.name = response.kinematic_solver_info.joint_names;
00051     initialState.position.resize(response.kinematic_solver_info.joint_names.size());
00052 
00053     for (unsigned int i = 0; i < response.kinematic_solver_info.joint_names.size(); i++)
00054     {
00055      // double rand = rng.uniformReal(response.kinematic_solver_info.limits[i].min_position,
00056       //                              response.kinematic_solver_info.limits[i].max_position);
00057      // initialState.position[i] = rand;
00058       // ROS_INFO("Initial Joint Position: %s %f",response.kinematic_solver_info.joint_names[i].c_str(), rand);
00059 
00060     }
00061 
00062     // publish initial position
00063 /*
00064     planned_robot_pose.trajectory.joint_trajectory.header = initialState.header;
00065     planned_robot_pose.trajectory.joint_trajectory.joint_names = initialState.name;
00066     planned_robot_pose.trajectory.joint_trajectory.points.resize(1);
00067     planned_robot_pose.trajectory.joint_trajectory.points[0].positions.resize(initialState.position.size());
00068 
00069     for (size_t i = 0; i < initialState.position.size(); i++)
00070     {
00071       arm_navigation_msgs::JointTrajectoryPoint* point = &planned_robot_pose.trajectory.joint_trajectory.points[0];
00072       point->positions[i] = initialState.position[i];
00073     }
00074 
00075     pub.publish(planned_robot_pose);
00076     ROS_DEBUG("Published initial joint state...");
00077     ros::Duration(1.0).sleep();
00078 */
00079   }
00080   else
00081   {
00082     ROS_ERROR("Could not call query service");
00083     ros::shutdown();
00084     exit(1);
00085   }
00086 
00087   // define the service messages
00088   kinematics_msgs::GetConstraintAwarePositionIK::Request gcapik_req;
00089   kinematics_msgs::GetConstraintAwarePositionIK::Response gcapik_res;
00090 
00091   sensor_msgs::JointState jointState = initialState;
00092 
00093   tf::TransformBroadcaster br;
00094   tf::Transform transform;
00095 
00096   do
00097   {
00098     gcapik_req.timeout = ros::Duration(5.0);
00099     gcapik_req.ik_request.ik_link_name = "katana_gripper_tool_frame";
00100 
00101     // generate a random pose in katana's operation space:
00102     // [-0.12, 0.68] in the z direction and [-0.48, 0.48] in both x and y
00103 
00104     gcapik_req.ik_request.pose_stamped.header.frame_id = "katana_base_link";
00105 
00106 
00107      // position should have a solution
00108 /*
00109     gcapik_req.ik_request.pose_stamped.pose.position.x = 0.16192;
00110      gcapik_req.ik_request.pose_stamped.pose.position.y = 0.161038;
00111      gcapik_req.ik_request.pose_stamped.pose.position.z = 0.517586;
00112 
00113      gcapik_req.ik_request.pose_stamped.pose.orientation.x = 0.708434;
00114      gcapik_req.ik_request.pose_stamped.pose.orientation.y = 0.024962;
00115      gcapik_req.ik_request.pose_stamped.pose.orientation.z = 0.704894;
00116      gcapik_req.ik_request.pose_stamped.pose.orientation.w = 0.024962;
00117 
00118 
00119 */
00120     gcapik_req.ik_request.pose_stamped.pose.position.x = rng.uniformReal(-0.15, 0.15);
00121     gcapik_req.ik_request.pose_stamped.pose.position.y = rng.uniformReal(-0.15, 0.15);
00122     gcapik_req.ik_request.pose_stamped.pose.position.z = rng.uniformReal(-0.10, 0.40);
00123 
00124     double value[4];
00125 
00126     rng.quaternion(value);
00127 
00128     gcapik_req.ik_request.pose_stamped.pose.orientation.x = value[0];
00129     gcapik_req.ik_request.pose_stamped.pose.orientation.y = value[1];
00130     gcapik_req.ik_request.pose_stamped.pose.orientation.z = value[2];
00131     gcapik_req.ik_request.pose_stamped.pose.orientation.w = value[3];
00132 
00133     // publishing the goal pose as TF for visualising it in rviz
00134 
00135     transform.setOrigin(tf::Vector3(gcapik_req.ik_request.pose_stamped.pose.position.x,
00136                                     gcapik_req.ik_request.pose_stamped.pose.position.y,
00137                                     gcapik_req.ik_request.pose_stamped.pose.position.z));
00138 
00139     transform.setRotation(tf::Quaternion(gcapik_req.ik_request.pose_stamped.pose.orientation.x,
00140                                          gcapik_req.ik_request.pose_stamped.pose.orientation.y,
00141                                          gcapik_req.ik_request.pose_stamped.pose.orientation.z,
00142                                          gcapik_req.ik_request.pose_stamped.pose.orientation.w));
00143 
00144     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "katana_base_link", "katana_goal_pose"));
00145 
00146     ROS_DEBUG("cartesian goal pose orientation: %f, %f, %f",  gcapik_req.ik_request.pose_stamped.pose.position.x,
00147                                                               gcapik_req.ik_request.pose_stamped.pose.position.y,
00148                                                               gcapik_req.ik_request.pose_stamped.pose.position.z);
00149 
00150     ROS_DEBUG("cartesian goal pose orientation: %f, %f, %f, %f", gcapik_req.ik_request.pose_stamped.pose.orientation.x,
00151                                                                 gcapik_req.ik_request.pose_stamped.pose.orientation.y,
00152                                                                 gcapik_req.ik_request.pose_stamped.pose.orientation.z,
00153                                                                 gcapik_req.ik_request.pose_stamped.pose.orientation.w);
00154 
00155     gcapik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
00156 
00157     gcapik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
00158 
00159     // let the IK plan from the initial state...
00160  //   gcapik_req.ik_request.ik_seed_state.joint_state = initialState;
00161 
00162     // or from the middle position of all joints...
00163 
00164     for (unsigned int i = 0; i < response.kinematic_solver_info.joint_names.size(); i++)
00165     {
00166       gcapik_req.ik_request.ik_seed_state.joint_state.position[i]
00167           = (response.kinematic_solver_info.limits[i].min_position
00168               + response.kinematic_solver_info.limits[i].max_position) / 2.0;
00169     }
00170 
00171 
00172    ros::Rate loop_rate1(100);
00173    ros::Time end_time = ros::Time::now() + ros::Duration(0.3);
00174 
00175     while (ros::ok() && ros::Time::now() < end_time)
00176     {
00177       jointState.header.stamp = ros::Time::now();
00178     //  planned_robot_pose.trajectory.joint_trajectory.header = jointState.header;
00179   //    pub.publish(planned_robot_pose);
00180       br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "katana_base_link", "katana_goal_pose"));
00181 
00182       loop_rate1.sleep();
00183     }
00184 
00185   //  ROS_WARN("Calling the constraint_aware IK client...");
00186 
00187     bool success = ik_client_.call(gcapik_req, gcapik_res);
00188 
00189     //ROS_WARN("Calling the client succeded...");
00190 
00191 
00192     if (!success)
00193     {
00194       ROS_ERROR("Could not call service");
00195       break;
00196     }
00197 
00198  } while (gcapik_res.error_code.val != gcapik_res.error_code.SUCCESS);
00199 
00200 
00201   ROS_INFO("Joint Set Up...");
00202 
00203   for (unsigned int i = 0; i < gcapik_res.solution.joint_state.name.size(); i++)
00204   {
00205     ROS_INFO("Joint: %s - %f",gcapik_res.solution.joint_state.name[i].c_str(),gcapik_res.solution.joint_state.position[i]);
00206 
00207   }
00208 
00209   ROS_INFO("...leads to:");
00210 
00211 
00212   ROS_INFO("pose position: %f, %f, %f",  gcapik_req.ik_request.pose_stamped.pose.position.x,
00213                                                                gcapik_req.ik_request.pose_stamped.pose.position.y,
00214                                                                gcapik_req.ik_request.pose_stamped.pose.position.z);
00215 
00216   ROS_INFO("pose orientation: %f, %f, %f, %f", gcapik_req.ik_request.pose_stamped.pose.orientation.x,
00217                                                                  gcapik_req.ik_request.pose_stamped.pose.orientation.y,
00218                                                                  gcapik_req.ik_request.pose_stamped.pose.orientation.z,
00219                                                                  gcapik_req.ik_request.pose_stamped.pose.orientation.w);
00220 
00221   btScalar roll, pitch, yaw;
00222    btQuaternion q = btQuaternion(gcapik_req.ik_request.pose_stamped.pose.orientation.x,
00223                                  gcapik_req.ik_request.pose_stamped.pose.orientation.y,
00224                                  gcapik_req.ik_request.pose_stamped.pose.orientation.z,
00225                                  gcapik_req.ik_request.pose_stamped.pose.orientation.w);
00226 
00227    btMatrix3x3(q).getRPY(roll, pitch, yaw);
00228 
00229    ROS_INFO("pose orientation RPY: %f, %f, %f", roll, pitch, yaw);
00230 
00231   ROS_INFO("...");
00232   // publish goal state
00233 
00234   jointState = gcapik_res.solution.joint_state;
00235 /*
00236   planned_robot_pose.trajectory.joint_trajectory.header = gcapik_res.solution.joint_state.header;
00237   planned_robot_pose.trajectory.joint_trajectory.joint_names = gcapik_res.solution.joint_state.name;
00238 
00239   planned_robot_pose.trajectory.joint_trajectory.points[0].positions.clear();
00240   planned_robot_pose.trajectory.joint_trajectory.points[0].positions.resize(gcapik_res.solution.joint_state.position.size());
00241 
00242 
00243   for (size_t i = 0; i <gcapik_res.solution.joint_state.position.size(); i++)
00244   {
00245     arm_navigation_msgs::JointTrajectoryPoint* point = &planned_robot_pose.trajectory.joint_trajectory.points[0];
00246     point->positions[i] = gcapik_res.solution.joint_state.position[i];
00247   }
00248 */
00249   ros::Rate loop_rate(100);
00250 
00251   while (ros::ok())
00252   {
00253     ROS_DEBUG("Publish goal state");
00254 
00255     jointState.header.stamp = ros::Time::now();
00256   //  planned_robot_pose.trajectory.joint_trajectory.header = jointState.header;
00257    // pub.publish(planned_robot_pose);
00258     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "katana_base_link", "katana_goal_pose"));
00259 
00260     loop_rate.sleep();
00261   }
00262 }
00263
get_constraint_aware_ik_test.cpp
