37 #include <geometry_msgs/msg/pose_stamped.hpp>
38 #include <kdl_parser/kdl_parser.hpp>
39 #include <tf2_kdl/tf2_kdl.hpp>
51 static const rclcpp::Logger LOGGER =
rclcpp::get_logger(
"moveit_constraint_samplers.test.pr2_arm_kinematics_plugin");
53 bool PR2ArmIKSolver::getCount(
int& count,
int max_count,
int min_count)
57 if (-count >= min_count)
62 else if (count + 1 <= max_count)
74 if (1 - count <= max_count)
79 else if (count - 1 >= min_count)
89 PR2ArmIKSolver::PR2ArmIKSolver(
const urdf::ModelInterface& robot_model,
const std::string& root_frame_name,
90 const std::string& tip_frame_name,
double search_discretization_angle,
int free_angle)
93 search_discretization_angle_ = search_discretization_angle;
94 free_angle_ = free_angle;
95 root_frame_name_ = root_frame_name;
107 const bool verbose =
false;
109 std::vector<std::vector<double> > solution_ik;
110 if (free_angle_ == 0)
113 RCLCPP_WARN(LOGGER,
"Solving with %f", q_init(0));
121 if (solution_ik.empty())
124 double min_distance = 1e6;
127 for (
int i = 0; i < static_cast<int>(solution_ik.size()); ++i)
131 RCLCPP_WARN(LOGGER,
"Solution : %d",
static_cast<int>(solution_ik.size()));
133 for (
int j = 0; j < static_cast<int>(solution_ik[i].size()); ++j)
135 RCLCPP_WARN(LOGGER,
"%d: %f", j, solution_ik[i][j]);
137 RCLCPP_WARN(LOGGER,
" ");
138 RCLCPP_WARN(LOGGER,
" ");
141 if (tmp_distance < min_distance)
143 min_distance = tmp_distance;
150 q_out.resize(
static_cast<int>(solution_ik[min_index].size()));
151 for (
int i = 0; i < static_cast<int>(solution_ik[min_index].size()); ++i)
153 q_out(i) = solution_ik[min_index][i];
164 const bool verbose =
false;
165 KDL::JntArray q_init = q_in;
166 double initial_guess = q_init(free_angle_);
168 rclcpp::Time start_time = rclcpp::Clock(RCL_ROS_TIME).now();
169 double loop_time = 0;
172 int num_positive_increments =
static_cast<int>(
174 int num_negative_increments =
static_cast<int>(
178 RCLCPP_WARN(LOGGER,
"%f %f %f %d %d \n\n", initial_guess,
pr2_arm_ik_.
solver_info_.limits[free_angle_].max_position,
180 num_negative_increments);
182 while (loop_time < timeout)
186 if (!getCount(count, num_positive_increments, -num_negative_increments))
188 q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
190 RCLCPP_WARN(LOGGER,
"%d, %f", count, q_init(free_angle_));
191 loop_time = rclcpp::Clock(RCL_ROS_TIME).now().seconds() - start_time.seconds();
193 if (loop_time >= timeout)
195 RCLCPP_WARN(LOGGER,
"IK Timed out in %f seconds", timeout);
200 RCLCPP_WARN(LOGGER,
"No IK solution was found");
201 return NO_IK_SOLUTION;
203 return NO_IK_SOLUTION;
206 bool getKDLChain(
const urdf::ModelInterface& model,
const std::string& root_name,
const std::string& tip_name,
207 KDL::Chain& kdl_chain)
211 if (!kdl_parser::treeFromUrdfModel(model, tree))
213 RCLCPP_ERROR(LOGGER,
"Could not initialize tree object");
216 if (!tree.getChain(root_name, tip_name, kdl_chain))
218 RCLCPP_ERROR(LOGGER,
"Could not initialize chain object for base %s tip %s", root_name.c_str(), tip_name.c_str());
226 Eigen::Isometry3f b = Eigen::Isometry3f::Identity();
227 for (
int i = 0; i < 3; ++i)
229 for (
int j = 0; j < 3; ++j)
241 for (
int i = 0; i < static_cast<int>(array_1.size()); ++i)
243 distance += (array_1[i] - array_2(i)) * (array_1[i] - array_2(i));
251 while (i <
static_cast<int>(
chain.getNrOfSegments()))
253 chain_info.link_names.push_back(
chain.getSegment(i).getName());
269 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
270 double search_discretization)
273 storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
274 const bool verbose =
false;
275 std::string xml_string;
278 RCLCPP_WARN(
LOGGER,
"Loading KDL Tree");
282 RCLCPP_ERROR(
LOGGER,
"Could not load kdl tree");
291 RCLCPP_ERROR(
LOGGER,
"Could not load ik");
304 RCLCPP_WARN(
LOGGER,
"PR2Kinematics:: joint name: %s", joint_name.c_str());
308 RCLCPP_WARN(
LOGGER,
"PR2Kinematics can solve IK for %s", link_name.c_str());
312 RCLCPP_WARN(
LOGGER,
"PR2Kinematics can solve FK for %s", link_name.c_str());
314 RCLCPP_WARN(
LOGGER,
"PR2KinematicsPlugin::active for %s", group_name.c_str());
322 const std::vector<double>& ,
323 std::vector<double>& ,
324 moveit_msgs::msg::MoveItErrorCodes& ,
331 const std::vector<double>& ik_seed_state,
double timeout,
332 std::vector<double>& solution,
333 moveit_msgs::msg::MoveItErrorCodes& error_code,
338 RCLCPP_ERROR(
LOGGER,
"kinematics not active");
339 error_code.val = error_code.PLANNING_FAILED;
343 geometry_msgs::msg::PoseStamped ik_pose_stamped;
344 ik_pose_stamped.pose = ik_pose;
346 tf2::Stamped<KDL::Frame> pose_desired;
348 tf2::fromMsg(ik_pose_stamped, pose_desired);
351 KDL::JntArray jnt_pos_in;
352 KDL::JntArray jnt_pos_out;
356 jnt_pos_in(i) = ik_seed_state[i];
359 int ik_valid =
pr2_arm_ik_solver_->cartToJntSearch(jnt_pos_in, pose_desired, jnt_pos_out, timeout);
360 if (ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
362 error_code.val = error_code.NO_IK_SOLUTION;
371 solution[i] = jnt_pos_out(i);
373 error_code.val = error_code.SUCCESS;
378 RCLCPP_WARN(
LOGGER,
"An IK solution could not be found");
379 error_code.val = error_code.NO_IK_SOLUTION;
385 const std::vector<double>& ,
double ,
386 const std::vector<double>& ,
387 std::vector<double>& ,
388 moveit_msgs::msg::MoveItErrorCodes& ,
395 const std::vector<double>& ,
double ,
396 std::vector<double>& ,
398 moveit_msgs::msg::MoveItErrorCodes& ,
405 const std::vector<double>& ,
double ,
406 const std::vector<double>& ,
407 std::vector<double>& ,
409 moveit_msgs::msg::MoveItErrorCodes& ,
416 const std::vector<double>& ,
417 std::vector<geometry_msgs::msg::Pose>& )
const
426 RCLCPP_ERROR(
LOGGER,
"kinematics not active");
435 RCLCPP_ERROR(
LOGGER,
"kinematics not active");
void storeValues(const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization)
static const rclcpp::Logger LOGGER
rclcpp::Node::SharedPtr node_
std::vector< std::string > tip_frames_
std::function< void(const geometry_msgs::msg::Pose &, const std::vector< double > &, moveit_msgs::msg::MoveItErrorCodes &)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
const urdf::ModelInterfaceSharedPtr & getURDF() const
Get the parsed URDF model.
int cartToJntSearch(const KDL::JntArray &q_in, const KDL::Frame &p_in, KDL::JntArray &q_out, double timeout)
PR2ArmIK pr2_arm_ik_
The PR2 inverse kinematics solver.
void updateInternalDataStructures() override
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out) override
bool active_
Indicates whether the solver has been successfully initialized.
void computeIKShoulderRoll(const Eigen::Isometry3f &g_in, double shoulder_roll_initial_guess, std::vector< std::vector< double > > &solution) const
compute IK based on an initial guess for the shoulder roll angle. h
bool init(const urdf::ModelInterface &robot_model, const std::string &root_name, const std::string &tip_name)
Initialize the solver by providing a urdf::Model and a root and tip name.
void computeIKShoulderPan(const Eigen::Isometry3f &g_in, double shoulder_pan_initial_guess, std::vector< std::vector< double > > &solution) const
compute IK based on an initial guess for the shoulder pan angle.
moveit_msgs::msg::KinematicSolverInfo solver_info_
get chain information about the arm.
bool isActive()
Specifies if the node is active or not.
moveit_msgs::msg::KinematicSolverInfo ik_solver_info_
const std::vector< std::string > & getJointNames() const override
Return all the joint names in the order they are used internally.
bool initialize(const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization) override
Initialization function for the kinematics.
const std::vector< std::string > & getLinkNames() const override
Return all the link names in the order they are represented internally.
pr2_arm_kinematics::PR2ArmIKSolverPtr pr2_arm_ik_solver_
moveit_msgs::msg::KinematicSolverInfo fk_solver_info_
bool getPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
Given a desired pose of the end-effector, compute the joint angles to reach it.
bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::msg::Pose > &poses) const override
Given a set of joint angles and a set of links, compute their pose.
std::shared_ptr< KDL::ChainFkSolverPos_recursive > jnt_to_pose_solver_
bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
Given a desired pose of the end-effector, search for the joint angles required to reach it....
rclcpp::Logger get_logger()
bool getKDLChain(const urdf::ModelInterface &model, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
double distance(const urdf::Pose &transform)
void getKDLChainInfo(const KDL::Chain &chain, moveit_msgs::msg::KinematicSolverInfo &chain_info)
double computeEuclideanDistance(const std::vector< double > &array_1, const KDL::JntArray &array_2)
Eigen::Isometry3f kdlToEigenMatrix(const KDL::Frame &p)
FilterFn chain(const std::vector< FilterFn > &filter_functions)
A set of options for the kinematics solver.