00001 00026 #ifndef SOLVER_STATE_H 00027 #define SOLVER_STATE_H 00028 00029 #include <vector> 00030 #include <Eigen/Core> 00031 #include <Eigen/Geometry> 00032 #include <constrained_ik/enum_types.h> 00033 #include <moveit/planning_scene/planning_scene.h> 00034 #include <industrial_collision_detection/collision_detection/collision_robot_industrial.h> 00035 #include <industrial_collision_detection/collision_detection/collision_world_industrial.h> 00036 00037 namespace constrained_ik 00038 { 00039 00041 struct SolverState 00042 { 00043 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00044 Eigen::Affine3d goal; 00045 Eigen::VectorXd joint_seed; 00047 int iter; 00048 Eigen::VectorXd joints; 00049 Eigen::VectorXd joints_delta; 00050 Eigen::Affine3d pose_estimate; 00051 std::vector<Eigen::VectorXd> iteration_path; 00052 double primary_sum; 00053 double auxiliary_sum; 00054 bool auxiliary_at_limit; 00055 initialization_state::InitializationState condition; 00056 planning_scene::PlanningSceneConstPtr planning_scene; 00057 collision_detection::CollisionRobotIndustrialConstPtr collision_robot; 00058 collision_detection::CollisionWorldIndustrialConstPtr collision_world; 00059 moveit::core::RobotStatePtr robot_state; 00060 std::string group_name; 00067 SolverState(const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed); 00068 SolverState(){} 00069 00075 void reset(const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed); 00076 00077 }; 00078 00079 } // namespace constrained_ik 00080 00081 #endif // SOLVER_STATE_H 00082