00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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 seed_state_.joint_state.name = kinematics_solver_->getJointNames();
00045 seed_state_.joint_state.position.resize(kinematics_solver_->getJointNames().size());
00046
00047
00048 solution_state_.joint_state.name = kinematics_solver_->getJointNames();
00049 solution_state_.joint_state.position.resize(kinematics_solver_->getJointNames().size());
00050
00051
00052 real_vector_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->getSubSpaceIndex("real_vector");
00053
00054 if(real_vector_index_ > -1)
00055 {
00056 x_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index_)->getDimensionIndex("x");
00057 y_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index_)->getDimensionIndex("y");
00058 z_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index_)->getDimensionIndex("z");
00059
00060 pitch_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index_)->getDimensionIndex("pitch");
00061 roll_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index_)->getDimensionIndex("roll");
00062 yaw_index_ = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index_)->getDimensionIndex("yaw");
00063 }
00064 else
00065 {
00066 ROS_ERROR("Could not find real vector state space");
00067 return false;
00068 }
00069
00070 scoped_state_.reset(new ompl::base::ScopedState<ompl::base::CompoundStateSpace>(state_space_));
00071 if(!ompl_ros_interface::getOmplStateToRobotStateMapping(*scoped_state_,seed_state_,ompl_state_to_robot_state_mapping_,false))
00072 {
00073 ROS_ERROR("Could not get mapping between ompl state and robot state");
00074 return false;
00075 }
00076 return true;
00077 }
00078
00079 bool OmplRosRPYIKStateTransformer::configureOnRequest(const motion_planning_msgs::GetMotionPlan::Request &request,
00080 motion_planning_msgs::GetMotionPlan::Response &response)
00081 {
00082
00083 return true;
00084 }
00085
00086 bool OmplRosRPYIKStateTransformer::inverseTransform(const ompl::base::State &ompl_state,
00087 motion_planning_msgs::RobotState &robot_state)
00088 {
00089 geometry_msgs::Pose pose;
00090 omplStateToPose(ompl_state,pose);
00091 (*scoped_state_) = ompl_state;
00092 ompl_ros_interface::omplStateToRobotState(*scoped_state_,ompl_state_to_robot_state_mapping_,seed_state_);
00093 int error_code;
00094 if(kinematics_solver_->getPositionIK(pose,
00095 seed_state_.joint_state.position,
00096 solution_state_.joint_state.position,
00097 error_code))
00098 {
00099 robot_state.joint_state = solution_state_.joint_state;
00100 return true;
00101 }
00102 return false;
00103 }
00104
00105 bool OmplRosRPYIKStateTransformer::forwardTransform(const motion_planning_msgs::RobotState &joint_state,
00106 ompl::base::State &ompl_state)
00107 {
00108 return true;
00109 }
00110
00111 void OmplRosRPYIKStateTransformer::omplStateToPose(const ompl::base::State &ompl_state,
00112 geometry_msgs::Pose &pose)
00113 {
00114
00115 btVector3 tmp_pos(ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[x_index_],
00116 ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[y_index_],
00117 ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[z_index_]);
00118 btQuaternion tmp_rot;
00119 tmp_rot.setRPY(ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[roll_index_],
00120 ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[pitch_index_],
00121 ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[yaw_index_]);
00122 btTransform tmp_transform(tmp_rot,tmp_pos);
00123 tf::poseTFToMsg(tmp_transform,pose);
00124 }
00125
00126
00127 motion_planning_msgs::RobotState OmplRosRPYIKStateTransformer::getDefaultState()
00128 {
00129 motion_planning_msgs::RobotState robot_state;
00130 if(kinematics_solver_)
00131 {
00132 robot_state.joint_state.name = kinematics_solver_->getJointNames();
00133 robot_state.joint_state.position.resize(kinematics_solver_->getJointNames().size());
00134 }
00135 else
00136 {
00137 ROS_ERROR("Kinematics solver not defined");
00138 }
00139 return robot_state;
00140 }
00141
00142 }