41 int main(
int argc,
char** argv)
43 ros::init(argc, argv,
"kinematics_cache");
59 ros::NodeHandle private_handle(
"~");
61 if (!pr2_kinematics_cache.
init(opt,
"pr2_arm_kinematics/PR2ArmKinematicsPlugin",
"right_arm",
"torso_lift_link",
62 "r_wrist_roll_link", 0.01))
67 geometry_msgs::Pose point;
68 point.position.x = 0.0;
69 point.position.y = -1.0;
70 point.position.z = -1.0;
72 unsigned int max_num = 200;
74 for (
unsigned int i = 0; i < max_num; ++i)
76 point.position.x = opt.
origin.x + i / 100.0;
77 for (
unsigned int j = 0; j < max_num; ++j)
79 point.position.y = opt.
origin.y + j / 100.0;
80 for (
unsigned int k = 0; k < max_num; ++k)
82 point.position.z = opt.
origin.z + k / 100.0;
83 unsigned int num_solutions(0);
85 ROS_ERROR(
"Outside grid");
86 else if (num_solutions > 0)
87 ROS_INFO(
"Num solutions: %d", num_solutions);
bool init(const kinematics_cache::KinematicsCache::Options &opt, const std::string &kinematics_solver_name, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization)
Initialization function.
geometry_msgs::Point origin
bool generateCacheMap(double timeout)
Generate the cache map spending timeout (seconds) on the generation process)
unsigned int max_solutions_per_grid_location
int main(int argc, char **argv)
boost::array< double, 3 > resolution
bool getNumSolutions(const geometry_msgs::Pose &pose, unsigned int &num_solutions) const
Get number of candidate solutions for a particular pose.
boost::array< double, 3 > workspace_size