-
Notifications
You must be signed in to change notification settings - Fork 947
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.
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
Servo: Enable usage of transforms published during runtime #2894
base: master
Are you sure you want to change the base?
Conversation
Thanks for helping in improving MoveIt and open source robotics! |
@@ -294,17 +296,19 @@ void ServoCalcs::calculateSingleIteration() | |||
have_nonzero_twist_stamped_ = latest_nonzero_twist_stamped_; | |||
have_nonzero_joint_command_ = latest_nonzero_joint_cmd_; | |||
|
|||
planning_scene_monitor_->requestPlanningSceneState(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why keep requesting the whole planning scene on each iteration .? see https://github.com/ros-planning/moveit2/pull/673/files for how we fixed this issue for moveit2
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'm don't know how to do that, can you elaborate a bit more?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This doesn't work. See my other comment here
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@udopa I'm making some changes to sync with what @JafarAbdi has done. If I have permission, I'll push directly to your branch. If not, I'll open a PR against your branch and it will be up to you to review/merge it
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Getting pretty close to merge-ready in my opinion. Thanks!
To test this, I added a static tf publisher to my Servo launch file:
And set
Is that how you intended for it to be used? Maybe it only works with subframes, not tf frames. That's still a nice improvement. |
I tested both scenarios, with and without the call
Without it I was not able to send commands in any attached/published frame. With it, it's possible to use a both. -> I would do the call in every iteration (or maybe every n-th iteration?). I guess this is why your test didn't work. Testing & Intended way of useI didn't change anything in the servo configuration file at all. This works because in case the frame is not one of the ones defined in the config file, the calculations will be done here L492. This is the config I used: ## MoveIt properties
move_group_name: welding_endeffector # Often 'manipulator' or 'arm'
planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world'
## Other frames
ee_frame_name: wrist_3_link # The name of the end effector link, used to return the EE pose
robot_link_command_frame: base # commands must be given in the frame of a robot link. Usually either the base or end effector For using the new feature I published a frame using the static tf broadcaster from inside a ros node and then send the commands. I also made sure to first start all servo components and publish the frame afterwards, so that servo should not know the new frame at the beginning. So basically like you did. |
Co-authored-by: AndyZe <andyz@utexas.edu>
I'm going to be out for a week. So don't think I'm ignoring this PR- I consider it pretty important. |
@AndyZe any further work required for this PR to be merged? Any hint why the CI is failing? |
I don't think these are much related, feature-wise.
I think it's a nice idea to support arbitrary frames and that's what this PR seems to aim at.
Looking at the patch I'm not convinced locking the scene that often is a good idea at all, but I can't review this properly right now.
My new helper function just simplifies transforming poses for attached objects into poses for a link the RobotModel knows about.
Possibly, this could be done here as well, but it would be a different approach.
|
@udopa it seems like the MoveIt For me, the easiest path is to bring this functionality you've requested into MoveIt2. But if you want to follow through with this PR, I'll still review it. I'm kind of curious how you said it was working for you and if I missed something when testing, though. |
Codecov Report
@@ Coverage Diff @@
## master #2894 +/- ##
==========================================
- Coverage 61.56% 61.14% -0.42%
==========================================
Files 370 370
Lines 33147 33159 +12
==========================================
- Hits 20405 20271 -134
- Misses 12742 12888 +146
Continue to review full report at Codecov.
|
@AndyZe I did some more testing and realized, that it only works on with my own code. I will try to push a working minimal example for you in the upcoming days. |
Cool. Feel free to wipe out my commits on your branch, or close this PR and open a new one, if you need to. |
The example i have it not yet working because somehow servo_server_sub_pub doesn't get recognized. I will take a look at it shortly :) |
@AndyZe I finally realized why it worked for me and didn't for you. Additionally to the published frame with a You should be able to test my code by running the |
@AndyZe Any news on this issue? Do you need some more information? Quick feedback would be good |
e1b0ec1
to
fdc3b1c
Compare
Description
Please explain the changes you made, including a reference to the related issue if applicable
This is related to #2891 and enables sending TwistStamped to Servo with frame_ids that were not known during startup resulting in more flexibility. With this approach, the following additional frames can be used (and were tested):
@AndyZe Here is the PR as discussed
The changes are in 3 places:
Using only getFrameTransform for getting the transform right did not work for me, this is why I still use the version with the inverse as shown here.
The version with robot_state does not recognize the frames from the static tf broadcaster, which is why I did not use it in the end.
Furthermore, I think this code could also be used instead of the transformations (
tf_moveit_to_robot_cmd_frame_
andtf_moveit_to_ee_frame_
).As far as I can see it, these transformations are also calculated every single iteration, but the frame ids have to be defined manually before starting servo (in the servo config file). They are also not used any further. Therefore I suppose that
tf_moveit_to_robot_cmd_frame_
andtf_moveit_to_ee_frame_
can be completely replaced with atf_moveit_to_incoming_cmd_frame
and a default transformation (in case there is no frame_id declared in the cmd header). This also results in not needing to declare the frames in the config.Maintainers: What do you think about that?
Checklist