49 const robot_model::RobotModelConstPtr& kinematic_model,
86 ROS_DEBUG_NAMED(
"kinematics_cache",
"Solution dimension: %d", (
int)solution_dimension_);
91 ros::WallTime start_time = ros::WallTime::now();
92 std::vector<std::string> fk_names;
93 std::vector<double> fk_values;
94 std::vector<geometry_msgs::Pose> poses;
100 while ((ros::WallTime::now() - start_time).toSec() <= timeout &&
107 ROS_ERROR_NAMED(
"kinematics_cache",
"Fk failed");
112 ROS_DEBUG_NAMED(
"kinematics_cache",
"Adding to cache failed for: %f %f %f", poses[0].position.x,
113 poses[0].position.y, poses[0].position.z);
117 ROS_DEBUG_NAMED(
"kinematics_cache",
"Cache map generated with %d valid points",
125 unsigned int grid_index;
128 ROS_DEBUG_NAMED(
"kinematics_cache",
"Failed to get grid index");
134 ROS_DEBUG_NAMED(
"kinematics_cache",
"Pose already has max number of solutions");
137 if (num_solutions == 0)
142 for (
unsigned int i = 0; i < joint_values.size(); ++i)
161 ROS_DEBUG_NAMED(
"kinematics_cache",
"X position %f,%d lies outside grid: %d %d", pose.position.x, x_index, 0,
167 ROS_DEBUG_NAMED(
"kinematics_cache",
"Y position %f,%d lies outside grid: %d %d", pose.position.y, y_index, 0,
173 ROS_DEBUG_NAMED(
"kinematics_cache",
"Z position %f,%d lies outside grid: %d %d", pose.position.z, z_index, 0,
177 ROS_DEBUG_NAMED(
"kinematics_cache",
"Grid indices: %d %d %d", x_index, y_index, z_index);
184 unsigned int grid_index;
194 ROS_DEBUG_NAMED(
"kinematics_cache",
"[Grid Index, Solution number location]: %d, %d", grid_index,
196 return (grid_index *
size_grid_node_ + solution_index * solution_dimension_);
200 std::vector<double>& solution)
const 202 unsigned int grid_index;
210 solution =
getSolution(grid_index, solution_index);
216 unsigned int grid_index;
221 for (
unsigned int i = 0; i < solution.size(); i++)
236 return solution_local;
241 std::ifstream file(filename.c_str());
244 ROS_DEBUG_NAMED(
"kinematics_cache",
"Could not open file: %s", filename.c_str());
247 std::string group_name;
248 std::getline(file, group_name);
249 if (group_name.empty())
251 ROS_ERROR_NAMED(
"kinematics_cache",
"Could not find group_name in file: %s", group_name.c_str());
257 ROS_ERROR_NAMED(
"kinematics_cache",
"Input file group name %s does not match solver group name %s",
263 std::string line_string;
264 std::getline(file, line_string);
265 std::stringstream line_stream(line_string);
268 std::getline(file, line_string);
271 line_stream << line_string;
274 std::getline(file, line_string);
277 line_stream << line_string;
280 std::getline(file, line_string);
283 line_stream << line_string;
288 std::getline(file, line_string);
291 line_stream << line_string;
294 std::getline(file, line_string);
297 line_stream << line_string;
300 std::vector<double> kinematics_cache_vector;
301 std::vector<unsigned int> num_solutions_vector;
302 std::getline(file, line_string);
303 std::getline(file, line_string);
306 line_stream << line_string;
308 while (line_stream >> d)
310 kinematics_cache_vector.push_back(d);
313 std::getline(file, line_string);
314 std::getline(file, line_string);
317 line_stream << line_string;
319 while (line_stream >> index)
321 num_solutions_vector.push_back(index);
328 ROS_DEBUG_NAMED(
"kinematics_cache",
"Read %d total points from file: %s", (
int)
num_solutions_vector_.size(),
335 ROS_DEBUG_NAMED(
"kinematics_cache",
"Writing %d total points to file: %s", (
int)
num_solutions_vector_.size(),
338 file.open(filename.c_str());
341 ROS_DEBUG_NAMED(
"kinematics_cache",
"Could not open file: %s", filename.c_str());
347 file << group_name << std::endl;
358 std::ostream_iterator<double>(file,
" "));
363 std::ostream_iterator<unsigned int>(file,
" "));
378 double distance_squared =
379 (pose.position.x * pose.position.x + pose.position.y * pose.position.y + pose.position.z * pose.position.z);
void updateDistances(const geometry_msgs::Pose &pose)
void setup(const KinematicsCache::Options &opt)
Setup the cache.
bool initialize(kinematics::KinematicsBaseConstPtr &solver, const planning_models::RobotModelConstPtr &kinematic_model, const KinematicsCache::Options &opt)
Initialize the cache class.
double cache_resolution_z_
double min_squared_distance_
geometry_msgs::Point origin
bool addToCache(const geometry_msgs::Pose &pose, const std::vector< double > &joint_values, bool overwrite=false)
Add a new solution to the cache at the given location.
geometry_msgs::Point cache_origin_
bool generateCacheMap(double timeout)
Generate the cache map spending timeout (seconds) on the generation process)
kinematics::KinematicsBaseConstPtr kinematics_solver_
unsigned int getSolutionLocation(unsigned int &grid_index, unsigned int &solution_index) const
Get the location of a solution given the grid index and solution index.
unsigned int max_solutions_per_grid_location
KinematicsCache::Options options_
bool readFromFile(const std::string &filename)
unsigned int cache_size_y_
double max_squared_distance_
double cache_resolution_x_
unsigned int kinematics_cache_size_
bool getGridIndex(const geometry_msgs::Pose &pose, unsigned int &grid_index) const
Get the grid index for a given pose.
unsigned int max_solutions_per_grid_location_
boost::shared_ptr< planning_models::RobotState *::JointStateGroup > joint_state_group_
bool writeToFile(const std::string &filename)
bool getSolution(const geometry_msgs::Pose &pose, unsigned int solution_index, std::vector< double > &solution) const
Get a candidate solution for a particular pose. Note that the pose will be projected onto the grid us...
boost::array< double, 3 > resolution
std::vector< double > kinematics_cache_vector_
double cache_resolution_y_
bool getNumSolutions(const geometry_msgs::Pose &pose, unsigned int &num_solutions) const
Get number of candidate solutions for a particular pose.
unsigned int solution_dimension_
planning_models::RobotModelConstPtr kinematic_model_
boost::array< double, 3 > workspace_size
unsigned int cache_size_z_
unsigned int kinematics_cache_points_with_solution_
unsigned int cache_size_x_
bool getSolutions(const geometry_msgs::Pose &pose, std::vector< std::vector< double > > &solutions) const
Get all candidate solutions for a particular pose. Note that the pose will be projected onto the grid...
unsigned int size_grid_node_
std::vector< unsigned int > num_solutions_vector_
std::pair< double, double > getMinMaxSquaredDistance()
planning_models::RobotState *Ptr kinematic_state_
const planning_models::RobotModel::JointModelGroup * joint_model_group_