KinematicsBase support for multiple tip frames and IK requests with multiple poses #149
Conversation
| @@ -179,6 +180,50 @@ class KinematicsBase | |||
| const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const = 0; | |||
|
|
|||
| /** | |||
| * @brief Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles | |||
| * required to reach it them. This is useful for e.g. biped robots that need to perform whole-body IK. | |||
There was a problem hiding this comment.
Typo. Also try to limit lines to 120 chars max.
There was a problem hiding this comment.
I don't see any misspellings here?
The lines I actually edited from the original function descriptions are within 120 characters I believe
There was a problem hiding this comment.
What does "required to reach it them" mean?
There was a problem hiding this comment.
man it was late, thanks
|
Looks OK. See comments inline. |
|
I fixed all your feedback comments, thanks for the review. |
|
What is the status of this PR? I have future pull requests depending on this one. |
| search_discretization); | ||
| } | ||
|
|
||
| logError("moveit.kinematics_base: This kinematic solver does not support initialization with more than one tip frames"); |
There was a problem hiding this comment.
I think instead of the error here you can just always set tip_frames_ to the array specified in the argument and not fail.
There was a problem hiding this comment.
This is the default implementation for initialize() with multiple tip frames, and it is intended to be overridden by the inheriting class if that class supports multiple tip frames. This error message exists for backwards compatibility, to allow older IK solvers like IKFast to be able to accept an initialize() function with an array of tips, so long as that array contains only one tip entry.
There was a problem hiding this comment.
I can add a comment about this in the code if you agree.
|
+1 |
| @@ -40,6 +40,7 @@ | |||
| #include <geometry_msgs/PoseStamped.h> | |||
| #include <moveit_msgs/MoveItErrorCodes.h> | |||
| #include <boost/function.hpp> | |||
| #include <console_bridge/console.h> | |||
There was a problem hiding this comment.
Why is this console_bridge/console.h include needed?
There was a problem hiding this comment.
I added several logError messages to alert user when kinematic base is attempted to be used as a multi or single tip-frame IK solver when it shouldn't be. See use cases below in this PR.
|
This does look promising, but we are thinking to do a moveit release of existing changes first and then merge this new stuff afterwards, so it has more soak time before release. In the meantime, it would be great to see your code that will use these API changes, and to see any changes this requires in existing kinematics implementations (should be none). Do your other changes change RobotState, for instance? If so, you should probably roll those changes into this pull request so we can see both ends of the change at once. |
| logError("robot_state: State message is missing required frames: [%s]", missing_frames.str().c_str()); | ||
| return false; | ||
| } | ||
|
|
There was a problem hiding this comment.
Why does this check for missing.empty() need to be removed?
There was a problem hiding this comment.
I'm not sure why this appeared in this PR, but has to do with the more recent PR here: #152. I'll try to sync branches now.
|
I'm in no hurry to release these changes, but it would be nice to get it merged before it gets forgotten about. Code that uses API: this is the PR for moveit_ros: Allow planning groups to have more than one tip. Unfortunately the IK solver we are using with this is written in LISP and uses some non-public robot models of HRP2 (JSK Lab at UTokyo). I'll work on getting a public demo released. I do have some changes to RobotState and JointModelGroup that I forgot to PR, will do now. |
| * @param timeout The amount of time (in seconds) available to the solver | ||
| * @param consistency_limits the distance that any joint in the solution can be from the corresponding joints in the current seed state | ||
| * @param solution the solution vector | ||
| * @param desired_pose_callback A callback function for the desired link pose - could be used, e.g. to check for collisions for the end-effector |
There was a problem hiding this comment.
This line needs to go away, there is no such parameter in this function.
There was a problem hiding this comment.
Done. Note: those incorrect comments were already there / are in hydro-devel
| @@ -83,3 +106,14 @@ bool kinematics::KinematicsBase::setRedundantJoints(const std::vector<std::strin | |||
| { | |||
| return (!str.empty() && str[0] == '/') ? removeSlash(str.substr(1)) : str; | |||
| } | |||
|
|
|||
| const std::string kinematics::KinematicsBase::supportsGroup(const moveit::core::JointModelGroup *jmg) const | |||
There was a problem hiding this comment.
I did a pull request against your branch with a change to this API. I didn't change the stuff that calls it though, that still needs to be done.
There was a problem hiding this comment.
I have not applied this function anywhere else in MoveIt! yet, I'm waiting on this PR to be approved first.
|
Once the supportsGroup() call is changed like so: https://github.com/davetcoleman/moveit_core/pull/3 I am ready to merge this. |
| timeout, | ||
| consistency_limits, | ||
| solution, | ||
| solution_callback, |
There was a problem hiding this comment.
This should not always call the version of searchPositionIK() which takes a solution_callback function. Instead it should check if solution_callback is defined; if so, call the version with, if not, call the version without. This fixes a "todo" in pull request #155.
There was a problem hiding this comment.
Good catch, thanks. Fixed.
|
As much as I want to see this merged, I also want to point out that this plugin interface would be a good candidate for a complete overhaul in order to improve performance and reduce function heading duplication. This might be a good time to address these issues - I've heard some others with similar thoughts. I think we could still do this while maintaining backwards compatibility. Improvements
I do realize, however, this might cause some dependency issues and perhaps this is too difficult. |
|
The complete overhaul which you mention is something we've been wanting to do for a while now, but haven't had time to work on. Some of the delays you saw trying to get your stuff merged was due to us thinking about the complete-overhaul option. I also am frustrated by the complexity of the code that loads kinematic solvers. I have written plugin loaders before for RViz and it can be much simpler, it seems to me. You could create a github "issue" called "improve KinematicsBase API" or something and list your points. Then we can discuss further actions there. I don't think we should wait to merge these changes until the big overhaul is done though. Let's get these in and see how we like them, and let their various features inform the new API when we have time to write it. |
|
There is a big list of changes (which we have been keeping track of) that needs to go into the kinematics plugins as hersh mentioned. It is not something we will do lightly though since it will touch every aspect of MoveIt! and will require significant re-factoring to preserve backwards compatibility. As hersh suggests, lets get this group of changes in and we can prototype other stuff later. |
|
Agreed, I'd really like to get these current PRs merged first. Issue moved to here. |
Changed KinematicsBase::supportsGroup() to use a more standard call signature.
|
https://github.com/davetcoleman/moveit_core/pull/3 has been merged. This should be ready to merge as soon as Travis finishes. |
| * @param lock_redundant_joints if setRedundantJoints() was previously called, keep the values of the joints marked as redundant the same as in the seed | ||
| * @return True if a valid solution was found, false otherwise | ||
| */ | ||
| virtual bool searchPositionIK(const std::vector<geometry_msgs::Pose> &ik_poses, |
There was a problem hiding this comment.
It would be nice to have index of the actually moving tip in the parameter list.
Like this:
virtual bool searchPositionIK(const std::vector<geometry_msgs::Pose> &ik_poses,
int search_tip_index,
const std::vector<double> &ik_seed_state,
...);There was a problem hiding this comment.
@davetcoleman can correct me, but I believe the idea is that all the tips in the ik_poses vector are supposed to be moving, not just one. The size and indexing of the ik_poses array should match that of the tip_frames vector sent to initialize() or setValues(). If there are more "tips" in the group than in tip_frames, the solver might be programmed to keep those other tips at their same poses.
There was a problem hiding this comment.
@hersh is correct, the idea is that you can have more than one tip (or end effector) moving at the same time and your IK solver needs to be able to solve for all of them at once.
There was a problem hiding this comment.
But now function searchPositionIK is called frommoveit :: core :: RobotState :: setFromIK and only one "tip" at the input does not correspond ik_seed_state. Am I wrong? In the future this will change?
There was a problem hiding this comment.
This pull request will be merged once KinematicBase is modified, and will fix this issue.
There was a problem hiding this comment.
Maybe it is incorrect, but I want to use this functionality to control collaborative manipulators. I move one 'tip' and others move together, so as not to drop the payload.
There was a problem hiding this comment.
I believe this new functionality will enable you to do that - your 'master' tip will be used to calculate the 'slave' tip pose (using reference frame conversions), then you will send both of these poses to the IK solver and it will return the valid joint positions for this non-kinematic chain
|
This PR has been open for a month and a half, can we merge it now? I'm afraid I'm going to start forgetting/lose motivation to address all the downstream PRs that are waiting on this one. |
|
ping. @sachinchitta -- do we have something that will be broken by merging this? |
|
I am currently testing this PR combined with the one in moveit_ros. There are a couple of tests that I rely on for kinematics - they are in moveit_pr2 (pr2_kinematics_tests/kinematics). With these two PRs, the tests are currently failing in the initialize step. You can duplicate this by running: The other test to try is kdl_plugin_test.launch. Let me know when they work and I will look again into merging these two PRs. |
|
Please look into merging this again, per: moveit/moveit_ros#450 |
|
I tested this and #155 yesterday with fresh compiles of all the moveit stuff and the tests passed. |
KinematicsBase support for multiple tip frames and IK requests with multiple poses
|
Hurray for finally merging this! |
|
Yeah, well, there is still some issue with building it. Somehow when building the tests, the /opt/ros/hydro version of kinematics_base.h is used. Only when I deleted that .h file out of /opt/ros/hydro did it compile correctly. I think for a release build, it would be fine since everything is built on a clean system. But for most of us doing a "git pull" from a catkin workspace, it will cause problems. |
|
I don't understand why it is doing that, but that is probably an issue greater than just this pull request, right? Also, once we release an updated MoveIt to debian that will go away. |
|
I'm working on it now. Seems like a side-effect of the order of include_directories() calls in pr2_moveit_tests/CMakeLists.txt. |
This PR only modifies KinematicsBase functionality to make review easier, though changes will be needed elsewhere in robot_state and moveit_ros to enable full non-chain IK support.
This introduces 3 new functions that optionally allow multiple tips and poses to be passed into KinematicsBase:
searchPositionIK()- allows a vector of poses to be passed insetValues()- allows a vector of frame tips to be passed ininitialize()- allows a vector of frame tips to be passed ingetTipFrames()- returns a vector of tips instead of just oneThe old
std::string tip_frame_member variable is a bit messy in that we cannot remove it without breaking older code like generated IKFast solver code. Thus, this variable must remain in for the time being. The results in a little bit of duplicate information if you are using the IK solver in the normal case of a chain solver in that your tip frame name is saved in both the string and in the vector. I've done my best to document the deprecated status of this variable throughout the code and in the future I think it should be removed.I have tested these modifications against KDL, IKFast, and our custom full body IK solver.
The following PRs depends on these changes:
Allow planning groups to have more than one tip
Allow joint model group to have use IK solvers with multiple tip frames