00001 /* 00002 * Software License Agreement (Apache License) 00003 * 00004 * Copyright (c) 2016, Jonathan Meyer 00005 * 00006 * Licensed under the Apache License, Version 2.0 (the "License"); 00007 * you may not use this file except in compliance with the License. 00008 * You may obtain a copy of the License at 00009 * 00010 * http://www.apache.org/licenses/LICENSE-2.0 00011 * 00012 * Unless required by applicable law or agreed to in writing, software 00013 * distributed under the License is distributed on an "AS IS" BASIS, 00014 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00015 * See the License for the specific language governing permissions and 00016 * limitations under the License. 00017 */ 00018 00019 #ifndef IKFAST_MOVEIT_STATE_ADAPTER_H 00020 #define IKFAST_MOVEIT_STATE_ADAPTER_H 00021 00022 #include "descartes_moveit/moveit_state_adapter.h" 00023 00024 namespace descartes_moveit 00025 { 00026 class IkFastMoveitStateAdapter : public descartes_moveit::MoveitStateAdapter 00027 { 00028 public: 00029 virtual ~IkFastMoveitStateAdapter() 00030 { 00031 } 00032 00033 virtual bool initialize(const std::string& robot_description, const std::string& group_name, 00034 const std::string& world_frame, const std::string& tcp_frame); 00035 00036 virtual bool getAllIK(const Eigen::Affine3d& pose, std::vector<std::vector<double> >& joint_poses) const; 00037 00038 virtual bool getIK(const Eigen::Affine3d& pose, const std::vector<double>& seed_state, 00039 std::vector<double>& joint_pose) const; 00040 00041 virtual bool getFK(const std::vector<double>& joint_pose, Eigen::Affine3d& pose) const; 00042 00047 void setState(const moveit::core::RobotState& state); 00048 00049 protected: 00050 bool computeIKFastTransforms(); 00051 00059 descartes_core::Frame tool0_to_tip_; 00060 00065 descartes_core::Frame world_to_base_; 00066 }; 00067 00068 } // end namespace 'descartes_moveit' 00069 #endif