JTC: maintain current configuration as hold position #531
Conversation
Co-authored: Marco Boneberger
github-actions
bot
requested review from
aprotyas,
arne48,
bmagyar,
destogl,
homalozoa,
jaron-l and
rosterloh
| @@ -350,6 +350,12 @@ controller_interface::return_type JointTrajectoryController::update( | |||
| } | |||
| } | |||
| } | |||
| else | |||
| { | |||
| // TODO(peterdavidfagan) temporary solution to keep the initial position | |||
There was a problem hiding this comment.
There is nothing more permanent than a temporary solution ;P
I put the comment there as I was unsure what kind of implications this has for other use-cases.
There was a problem hiding this comment.
I am happy to continue working on this pr based on feedback from ros-controls developers and my evolving understanding of the scripts.
There was a problem hiding this comment.
This doesn't feel like the right solution for the issue. First my understanding and then the proposed solution.
Issue
When JTC is started, there is no input message, i.e., no command to the robot. This makes issues in effort controlled robots because the gravity is not compensated. Is this correct?
Solution
In on_activate method, we have to make sure that initial values are set properly for the robot. In most use-cases is that the states are set as new command for a robot. I believe this should be sufficient. If you are using the robot with effort command interface, then we have to think what initial values are sensible.
| { | ||
| // TODO(peterdavidfagan) temporary solution to keep the initial position | ||
| // before the first trajectory goal arrives | ||
| set_hold_position(); |
There was a problem hiding this comment.
This doesn't feel right to be here.
There was a problem hiding this comment.
Thanks for the feedback @destogl I will review the current set of commits based on the above information. It may be tomorrow before I can make updates due to an item I am finishing this evening.
Almost. No input message results in sending zero torques to the robot. Gravity is compensated, so the robot does not move away in an ideal scenario. However, you can just push the robot around as the PID controller is not running. Also, if you have a weight attached to the EE, the robot will move down. Here is the scenario:
|
|
OK, so if I understand correctly, initial position setup should be set to the current position so that PID kicks in and robot is kept in place. Is this correct? |
|
Hi, at this point just posting this as a reference as it may still be useful. I was thinking of making a pull request out of my solution of issue #514 (comment) from a few weeks ago. But I noticed there is now this other pull request that seems to tackle the same problem. My solution was actually very similar, I think the only difference is that I used the actual current joint state instead of the last commanded state. Also, I set the new trajectory from the update method as it seemed safer from a synchronization perspective. |
| msg.header.stamp = rclcpp::Time(0); | ||
| msg.joint_names = params_.joints; | ||
| trajectory_msgs::msg::JointTrajectoryPoint point; | ||
| for (const auto & joint_position : last_commanded_state_.positions) |
There was a problem hiding this comment.
OK, it looks like last_commanded_state_ is always initialized first at L790 👍
| @@ -350,6 +350,12 @@ controller_interface::return_type JointTrajectoryController::update( | |||
| } | |||
| } | |||
| } | |||
| else | |||
There was a problem hiding this comment.
I would move this else before the if since it's just a few lines. Best to get it out of the way quickly for readability, rather than requiring a couple hundred lines of scrolling.
|
I know you probably need this in humble but we operate with a backports-only policy. First we need a PR into the master branch then we can backport. |
I was looking (and overseeing this PR first) for a solution for handling the canceling of goals. Having that in mind I vote for the solution of @c-rizz taking the current joint_state instead of the last command. Think of some manual abort because something did not go as planned (e.g, grasping using generous trajectory tolerances), then you don't want the robot still trying to reach the last commanded position. I created already a PR from @c-rizz to the master branch with #558, what do the others think? @peterdavidfagan @AndyZe @destogl ? |
|
Apologies for being inactive on this pr for a quite a while. As a result of #558 being merged I am going to close this pr. Thanks @christophfroehlich for posting this pr. |
|
Ok, I reopened the other PR. Could you have a look and review it please? |
This pr adds logic to ensure that the current robot configuration is used as a goal when the JTC hasn't already been passed a goal: frankaemika/franka_ros2#10 (comment).
In particular this pr adds changes made by @marcbone in frankaemika/franka_ros2@8671588#diff-14bf2d9185d549c84286510ef2771e8dd01c2e6d8e05b8dae3fbeb52313c401b.
@destogl I can update the master branch too and perform a backport if needed.