Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Return all IK solutions with MoveIt plugin #553

Closed
wants to merge 3 commits into from
Closed

Return all IK solutions with MoveIt plugin #553

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a05bebe
Implemented kinematics base class function for getting all IK solutions
marip8 Apr 24, 2018
a9461bf
Updated IK methods to share code
marip8 Nov 19, 2020
ed0ff1e
Added simple unit test for the getPositionFK and getPositionIK methods
marip8 Nov 19, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 15 additions & 0 deletions ur_kinematics/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,3 +77,18 @@ install(DIRECTORY include/${PROJECT_NAME}/
install(FILES ur_moveit_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

#############
## Testing ##
#############
if(${CATKIN_ENABLE_TESTING})
find_package(rostest REQUIRED)
add_rostest_gtest(utest
test/kinematics_plugin.test
test/utest.cpp
)
target_link_libraries(utest
${catkin_LIBRARIES}
ur5_moveit_plugin
)
endif()
51 changes: 39 additions & 12 deletions ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h
View file
Open in desktop
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,30 +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;
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 @@ -185,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 @@ -199,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
2 changes: 2 additions & 0 deletions ur_kinematics/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@

<exec_depend>rospy</exec_depend>

<test_depend>rostest</test_depend>

<export>
<moveit_core plugin="${prefix}/ur_moveit_plugins.xml"/>
</export>
Expand Down
Loading