$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00037 #include <ompl_ros_interface/state_transformers/ompl_ros_rpy_ik_state_transformer.h> 00038 00039 namespace ompl_ros_interface 00040 { 00041 00042 bool OmplRosRPYIKStateTransformer::initialize() 00043 { 00044 srand ( time(NULL) ); // initialize random seed 00045 seed_state_.joint_state.name = kinematics_solver_->getJointNames(); 00046 seed_state_.joint_state.position.resize(kinematics_solver_->getJointNames().size()); 00047 // arm_navigation_msgs::printJointState(seed_state_.joint_state); 00048 00049 solution_state_.joint_state.name = kinematics_solver_->getJointNames(); 00050 solution_state_.joint_state.position.resize(kinematics_solver_->getJointNames().size()); 00051 // arm_navigation_msgs::printJointState(solution_state_.joint_state); 00052 00053 real_vector_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->getSubSpaceIndex("real_vector"); 00054 00055 if(real_vector_index_ > -1) 00056 { 00057 x_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index_)->getDimensionIndex("x"); 00058 y_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index_)->getDimensionIndex("y"); 00059 z_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index_)->getDimensionIndex("z"); 00060 00061 pitch_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index_)->getDimensionIndex("pitch"); 00062 roll_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index_)->getDimensionIndex("roll"); 00063 yaw_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index_)->getDimensionIndex("yaw"); 00064 } 00065 else 00066 { 00067 ROS_ERROR("Could not find real vector state space"); 00068 return false; 00069 } 00070 /*Map any real joints onto the actual physical joints */ 00071 scoped_state_.reset(new ompl::base::ScopedState<ompl::base::CompoundStateSpace>(state_space_)); 00072 if(!ompl_ros_interface::getOmplStateToRobotStateMapping(*scoped_state_,seed_state_,ompl_state_to_robot_state_mapping_,false)) 00073 { 00074 ROS_ERROR("Could not get mapping between ompl state and robot state"); 00075 return false; 00076 } 00077 return true; 00078 } 00079 00080 bool OmplRosRPYIKStateTransformer::configureOnRequest(const arm_navigation_msgs::GetMotionPlan::Request &request, 00081 arm_navigation_msgs::GetMotionPlan::Response &response) 00082 { 00083 00084 return true; 00085 } 00086 00087 bool OmplRosRPYIKStateTransformer::inverseTransform(const ompl::base::State &ompl_state, 00088 arm_navigation_msgs::RobotState &robot_state) 00089 { 00090 geometry_msgs::Pose pose; 00091 omplStateToPose(ompl_state,pose); 00092 generateRandomState(seed_state_); 00093 00094 (*scoped_state_) = ompl_state; 00095 ompl_ros_interface::omplStateToRobotState(*scoped_state_,ompl_state_to_robot_state_mapping_,seed_state_); 00096 int error_code; 00097 00098 ROS_DEBUG_STREAM("Inner pose is " << 00099 pose.position.x << " " << 00100 pose.position.y << " " << 00101 pose.position.z << " " << 00102 pose.orientation.x << " " << 00103 pose.orientation.y << " " << 00104 pose.orientation.z << " " << 00105 pose.orientation.w); 00106 00107 if(kinematics_solver_->searchPositionIK(pose, 00108 seed_state_.joint_state.position, 00109 1.0, 00110 solution_state_.joint_state.position, 00111 error_code)) 00112 { 00113 robot_state.joint_state = solution_state_.joint_state; 00114 return true; 00115 } 00116 return false; 00117 } 00118 00119 bool OmplRosRPYIKStateTransformer::forwardTransform(const arm_navigation_msgs::RobotState &joint_state, 00120 ompl::base::State &ompl_state) 00121 { 00122 return true; 00123 } 00124 00125 void OmplRosRPYIKStateTransformer::omplStateToPose(const ompl::base::State &ompl_state, 00126 geometry_msgs::Pose &pose) 00127 { 00128 00129 btVector3 tmp_pos(ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[x_index_], 00130 ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[y_index_], 00131 ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[z_index_]); 00132 btQuaternion tmp_rot; 00133 tmp_rot.setRPY(ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[roll_index_], 00134 ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[pitch_index_], 00135 ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[yaw_index_]); 00136 btTransform tmp_transform(tmp_rot,tmp_pos); 00137 tf::poseTFToMsg(tmp_transform,pose); 00138 } 00139 00140 00141 arm_navigation_msgs::RobotState OmplRosRPYIKStateTransformer::getDefaultState() 00142 { 00143 arm_navigation_msgs::RobotState robot_state; 00144 if(kinematics_solver_) 00145 { 00146 robot_state.joint_state.name = kinematics_solver_->getJointNames(); 00147 robot_state.joint_state.position.resize(kinematics_solver_->getJointNames().size()); 00148 } 00149 else 00150 { 00151 ROS_ERROR("Kinematics solver not defined"); 00152 } 00153 return robot_state; 00154 } 00155 00156 void OmplRosRPYIKStateTransformer::generateRandomState(arm_navigation_msgs::RobotState &robot_state) 00157 { 00158 std::vector<const planning_models::KinematicModel::JointModel*> joint_models = physical_joint_model_group_->getJointModels(); 00159 for(unsigned int i=0; i < robot_state.joint_state.name.size(); i++) 00160 { 00161 std::pair<double,double> bounds; 00162 joint_models[i]->getVariableBounds(robot_state.joint_state.name[i],bounds); 00163 robot_state.joint_state.position[i] = generateRandomNumber(bounds.first,bounds.second); 00164 } 00165 } 00166 00167 double OmplRosRPYIKStateTransformer::generateRandomNumber(const double &min, const double &max) 00168 { 00169 int rand_num = rand()%100+1; 00170 double result = min + (double)((max-min)*rand_num)/101.0; 00171 return result; 00172 } 00173 00174 00175 }