-
Notifications
You must be signed in to change notification settings - Fork 125
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
Updated with recent ros2_control changes #34
Conversation
Signed-off-by: ahcorde <ahcorde@gmail.com>
Signed-off-by: ahcorde <ahcorde@gmail.com>
Signed-off-by: ahcorde <ahcorde@gmail.com>
Signed-off-by: ahcorde <ahcorde@gmail.com>
Signed-off-by: ahcorde <ahcorde@gmail.com>
Signed-off-by: ahcorde <ahcorde@gmail.com>
Thanks @ahcorde!!! It works!! I just couldnt seem to figure out how to get the startup node configuration right. I even tried to wrap my head around how the switch_controllers() call would work here but just couldnt see it. I still dont quite see it but I will dig into it this weekend. Do you mind holding off on the PR for a bit? I had some fairly big updates as well, mostly based on ros2_control fixes but also some things I found while studying the code.
There is a lot of code in default_robot_hw_sim and the logic is fairly complex, 90% of the complexity is in the setup init(). I had to write out a separate text doc to wrap my head around it all. I think a large chunk of the complexity is in the logic between the various control mode setup. How do you feel about me refactoring out the 3 control modes to separate classes? Like PositionJointXX, VelocityJointXX and EffortJointXX where XX is either Interface, Controller or nothing. I can create a draft PR and you can help review. FYI I also have a PR pending in ros2_controls that will remove the shared_ptr parts of the joint handles. It can still work with shared_ptr but not required anymore. |
As per my comment in #35 I take back my request to hold off and I think this PR should move forward. |
Signed-off-by: ahcorde <ahcorde@gmail.com>
Hi @guru-florida, thank for your contibution! I have included your suggestions in the code. It's included/restored the pid controller. Can you have a look ? If you want to make more modification please build on top of this PR. |
Regarding the refactoring, I'm fine with your suggested changes, but I think we should wait until the |
Nice! Thank you! Updates look good but I found a bug in one of my changes....the readSim() called in initSim() needs to be moved outside the loop at the end...readSim() iterates all joints and when number of joints >1 the readSim is called before other joints have been initialized. I believe there is a "components" update coming soon and hopefully things will stabilize after that. I'll probably be helping out conversion to components on the joint limits and transmission when that happens. I have a pending PR on transmission parsing too. These are basically the branches you stated in the #3 with updates to sync with latest ros2_controls master, uncrustification, linting, etc. FYI I am working on getting my humanoid URDF up and simulating with Gazebo now. |
Signed-off-by: ahcorde <ahcorde@gmail.com>
thank you @guru-florida for finding the bug! Looking forward to see your humanoid! Any gif/image that you can share at this moment ? |
It seems that UpdatesIt seems that it is not a gazebo problem. Values are passed along to the joint_pos_stateh_ and other state handlers correctly, but becomes zero after publishing on /joint_states. It might not be this PR's problem It seems that both joint_trajectory_controller and joint_state_controller's position state handlers are not able to update their respective joint state value. I am a newbie to ros2_control, I really need some help regarding this. |
@Briancbn I can reproduce your issue. I'm looking into it |
Signed-off-by: ahcorde <ahcorde@gmail.com>
@Briancbn This last commit should fix the issue. Let me know if this works for you too. |
I just had a look. By the looks of your code change, it looks like you are re-fetching the handles for each of the joint interfaces in the readSim. Did you find the joint handles were being invalidated somehow? ....Those joint handles are pointing directly to doubles in the DynamicJointState msg....Oh @%$!! The ol' growth of std::vector reallocating memory and thus invalidating iterators before it. For now, I am going to test setting up the handles as normal, but delay the fetching of those references until after the initSim loop completes. At that point the msg vectors dont grow. FYI This bug was also popping up in the limit handles enforce_limits() which also binds to those handles. This IMO is a design flaw in RobotHardware and registering joint-handles as it presents unexpected behavior to the API user due to an inner implementation detail of std:vector. Possibly this will be solved with the new components work going into ros2_controls but we can bring it up in the next working group meeting. |
Thanks so much it works! Probably not the best way tho, hahaha.
I also notice this as well, for now I am just commenting this line out. |
@ahcorde The transmission parser code has been merged into ros2_control master. I have the changes to update this branch. I will issue a PR from my branch to this one. |
* removed plugin's parseTransmissionsFromURDF since it just calls a 1-liner and returns true always, thus not handling errors. plugin now calls transmission parser directly and catches error exception * moved declaration of parameters to precede joint PID constructor as the PID constructor's ParameterInterface will cause parameter events to fire (and occasionally cause a SEGV). Also added defaults to force floating type on parameter to fix log errors due to wrong parameter type * updated transmission info member names to sync with ros2_controls master * split initSim() joint loop to first register all joint handles, then handles are fetched, then a second loop continues joint limit and PID setup * uncrustify and cpplint fixes * fix for CI, disabled ddengster branch checkout and overwrite since master now contains TransmissionInfo
Signed-off-by: ahcorde <ahcorde@gmail.com>
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.
It builds fine, and the position demo works for me.
But the velocity demo doesn't work. What I did:
ros2 launch gazebo_ros2_control_demos cart_example_velocity.launch.py &
# wait for simulation to launch
ros2 run gazebo_ros2_control_demos example_velocity
Am I missing something?
@ahcorde I added PR #40 to fix the velocity controller example. Tested with position, position_pid and velocity examples. @chapulina Thank you for taking the time to review. |
* removed plugin's parseTransmissionsFromURDF since it just calls a 1-liner and returns true always, thus not handling errors. plugin now calls transmission parser directly and catches error exception * moved declaration of parameters to precede joint PID constructor as the PID constructor's ParameterInterface will cause parameter events to fire (and occasionally cause a SEGV). Also added defaults to force floating type on parameter to fix log errors due to wrong parameter type * updated transmission info member names to sync with ros2_controls master * split initSim() joint loop to first register all joint handles, then handles are fetched, then a second loop continues joint limit and PID setup * uncrustify and cpplint fixes * fix for CI, disabled ddengster branch checkout and overwrite since master now contains TransmissionInfo * removed duplicate registering of opmode handle * fixed velocity example due to declare_parameter default values pushing velocity controller into PID mode
@guru-florida thank you for the fix ;) @chapulina do you mind to try again ? |
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.
Nice, all the demos work for me now! I didn't work closely at the code, but functionality-wise LGTM!
Updated with recent ros2_control changes
Signed-off-by: ahcorde ahcorde@gmail.com