Skip to content

Commit

Permalink
Updated IK methods to share code
Browse files Browse the repository at this point in the history
  • Loading branch information
@marip8
marip8 committed Nov 19, 2020
1 parent a05bebe commit 169a2a0
Show file tree
Hide file tree
Showing 2 changed files with 176 additions and 205 deletions.
57 changes: 39 additions & 18 deletions ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,30 +119,36 @@ namespace ur_kinematics
const std::vector<double> &ik_seed_state,
std::vector<double> &solution,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const override;

virtual bool getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
std::vector<std::vector<double> >& solutions,
kinematics::KinematicsResult& result,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override;

virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
double timeout,
std::vector<double> &solution,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const override;

virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
double timeout,
const std::vector<double> &consistency_limits,
std::vector<double> &solution,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const override;

virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
double timeout,
std::vector<double> &solution,
const IKCallbackFn &solution_callback,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const override;

virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
Expand All @@ -151,36 +157,53 @@ namespace ur_kinematics
std::vector<double> &solution,
const IKCallbackFn &solution_callback,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;

virtual bool getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
std::vector<std::vector<double> >& solutions,
kinematics::KinematicsResult& result,
const kinematics::KinematicsQueryOptions& options) const;
const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const override;

virtual bool getPositionFK(const std::vector<std::string> &link_names,
const std::vector<double> &joint_angles,
std::vector<geometry_msgs::Pose> &poses) const;
std::vector<geometry_msgs::Pose> &poses) const override;

virtual bool initialize(const std::string &robot_description,
const std::string &group_name,
const std::string &base_name,
const std::string &tip_name,
double search_discretization);
double search_discretization) override;

/**
* @brief Return all the joint names in the order they are used internally
*/
const std::vector<std::string>& getJointNames() const;
const std::vector<std::string>& getJointNames() const override;

/**
* @brief Return all the link names in the order they are represented internally
*/
const std::vector<std::string>& getLinkNames() const;
const std::vector<std::string>& getLinkNames() const override;

protected:

/**
* @brief Searches for all valid joint solutions capable of achieving a given desired pose of the end-effector,
* This particular method is intended for "searching" for multiple IK solutions by stepping through the redundancy
* (or other numerical routines).
* @param ik_pose the desired pose of the link
* @param ik_seed_state an initial guess solution for the inverse kinematics
* @param timeout The amount of time (in seconds) available to the solver
* @param solutions the vector of valid joint state IK solutions
* @param solution_callback A callback solution for the IK solution
* @param error_code an error code that encodes the reason for failure or success
* @param consistency_limit The returned solutuion will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
* @return True if a valid solution was found, false otherwise
*/
bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
double timeout,
std::vector<std::vector<double>> &solution,
const IKCallbackFn &solution_callback,
moveit_msgs::MoveItErrorCodes &error_code,
const std::vector<double> &consistency_limits,
const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;


/**
* @brief Given a desired pose of the end-effector, search for the joint angles required to reach it.
* This particular method is intended for "searching" for a solutions by stepping through the redundancy
Expand All @@ -191,8 +214,6 @@ namespace ur_kinematics
* @param solution the solution vector
* @param solution_callback A callback solution for the IK solution
* @param error_code an error code that encodes the reason for failure or success
* @param check_consistency Set to true if consistency check needs to be performed
* @param redundancy The index of the redundant joint
* @param consistency_limit The returned solutuion will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
* @return True if a valid solution was found, false otherwise
*/
Expand All @@ -205,7 +226,7 @@ namespace ur_kinematics
const std::vector<double> &consistency_limits,
const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;

virtual bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices);
virtual bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices) override;

private:

Expand Down
Loading

0 comments on commit 169a2a0

Please sign in to comment.