Updated with recent ros2_control changes #34

ahcorde · 2020-10-14T18:58:19Z

Updated with recent ros2_control changes

Signed-off-by: ahcorde ahcorde@gmail.com

Signed-off-by: ahcorde <ahcorde@gmail.com>

guru-florida · 2020-10-16T21:41:07Z

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.

We dont need to keep the local std::vectors for all the joint states and commands anymore. The new RobotHardware class has register_joint() that create JointHandles directly on the DynamicJointState message.
I have some changes that enable the joint saturation and soft limits including the velocity based ones. I moved the limits config code from init back into registerJointLimits() method where I saw some previous ifdef'd out code and updated with the new limit handles.
added comments

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.

guru-florida · 2020-10-19T03:14:40Z

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>

ahcorde · 2020-10-19T11:38:51Z

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.

ahcorde · 2020-10-19T11:40:25Z

Regarding the refactoring, I'm fine with your suggested changes, but I think we should wait until the ros2_control and ros2_controllers API is more stable.

guru-florida · 2020-10-20T06:25:49Z

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>

ahcorde · 2020-10-20T08:31:00Z

thank you @guru-florida for finding the bug!

Looking forward to see your humanoid! Any gif/image that you can share at this moment ?

Briancbn · 2020-10-21T03:17:03Z

It seems that /joint_states and /states are not updating properly for the postion demo. It is turning only zeros, although the prismatic joint are changing. I suspect it is due to ModelPtr or JointPtr are passed to the RobotHWSim properly.

Updates

It 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.

ahcorde · 2020-10-21T16:33:41Z

@Briancbn I can reproduce your issue. I'm looking into it

Signed-off-by: ahcorde <ahcorde@gmail.com>

ahcorde · 2020-10-22T09:04:32Z

@Briancbn This last commit should fix the issue. Let me know if this works for you too.

guru-florida · 2020-10-23T18:18:38Z

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.

Briancbn · 2020-10-24T08:48:17Z

@Briancbn This last commit should fix the issue. Let me know if this works for you too.

Thanks so much it works! Probably not the best way tho, hahaha.

FYI This bug was also popping up in the limit handles enforce_limits() which also binds to those handles.

I also notice this as well, for now I am just commenting this line out.

guru-florida · 2020-10-27T01:03:03Z

@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.

guru-florida · 2020-10-27T04:38:43Z

@ahcorde PR #38 is ready for merging into this branch. Also fixed up CI script to no longer do the ddengster branch checkout and overwrite. We have a gazebo plugin that runs directly on ros2_control masters!!! ....at least until they break it....and they will. :)

* 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>

chapulina

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?

guru-florida · 2020-11-03T03:41:12Z

@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

ahcorde · 2020-11-03T10:07:23Z

@guru-florida thank you for the fix ;)

@chapulina do you mind to try again ?

chapulina

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

7089664

Signed-off-by: ahcorde <ahcorde@gmail.com>

ahcorde requested a review from chapulina October 14, 2020 18:58

ahcorde self-assigned this Oct 14, 2020

ahcorde added 3 commits October 14, 2020 21:12

make linters happy

5bfa75b

Signed-off-by: ahcorde <ahcorde@gmail.com>

make linters happy

379a895

Signed-off-by: ahcorde <ahcorde@gmail.com>

Included velocity controller

e7d7bb1

Signed-off-by: ahcorde <ahcorde@gmail.com>

ahcorde mentioned this pull request Oct 15, 2020

Reproduced current status of gazebo_ros2_control #3

Closed

ahcorde added 2 commits October 16, 2020 11:42

Included joint limit

1925122

Signed-off-by: ahcorde <ahcorde@gmail.com>

make linters happy

550bb8e

Signed-off-by: ahcorde <ahcorde@gmail.com>

guru-florida mentioned this pull request Oct 18, 2020

Import/ahcorde update for ros2_controls and refactoring #35

Closed

Included suggestions

a6030db

Signed-off-by: ahcorde <ahcorde@gmail.com>

Briancbn mentioned this pull request Oct 20, 2020

Segfault: sim_joint_ out of range #36

Closed

readSim outside the joint loop

4c7e633

Signed-off-by: ahcorde <ahcorde@gmail.com>

Fixed joint handlers

05db9aa

Signed-off-by: ahcorde <ahcorde@gmail.com>

guru-florida and others added 2 commits October 27, 2020 09:09

Clean up CI and Dockerfile

7ea40c2

Signed-off-by: ahcorde <ahcorde@gmail.com>

ahcorde mentioned this pull request Oct 27, 2020

Can't build on Foxy distro #39

Closed

guru-florida mentioned this pull request Oct 30, 2020

JointStateHandle not available ros-controls/ros2_control#215

Closed

chapulina reviewed Nov 2, 2020

View reviewed changes

ahcorde requested a review from chapulina November 3, 2020 10:06

chapulina approved these changes Nov 7, 2020

View reviewed changes

chapulina merged commit 3dfe04d into master Nov 9, 2020

chapulina deleted the ahcorde/update/ros2_control branch November 9, 2020 20:00

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Updated with recent ros2_control changes #34

Updated with recent ros2_control changes #34

ahcorde commented Oct 14, 2020

guru-florida commented Oct 16, 2020

guru-florida commented Oct 19, 2020

ahcorde commented Oct 19, 2020

ahcorde commented Oct 19, 2020

guru-florida commented Oct 20, 2020

ahcorde commented Oct 20, 2020

Briancbn commented Oct 21, 2020 •

edited

Loading

ahcorde commented Oct 21, 2020

ahcorde commented Oct 22, 2020

guru-florida commented Oct 23, 2020

Briancbn commented Oct 24, 2020 •

edited

Loading

guru-florida commented Oct 27, 2020

guru-florida commented Oct 27, 2020

chapulina left a comment

guru-florida commented Nov 3, 2020

ahcorde commented Nov 3, 2020

chapulina left a comment

Updated with recent ros2_control changes #34

Updated with recent ros2_control changes #34

Conversation

ahcorde commented Oct 14, 2020

guru-florida commented Oct 16, 2020

guru-florida commented Oct 19, 2020

ahcorde commented Oct 19, 2020

ahcorde commented Oct 19, 2020

guru-florida commented Oct 20, 2020

ahcorde commented Oct 20, 2020

Briancbn commented Oct 21, 2020 • edited Loading

Updates

ahcorde commented Oct 21, 2020

ahcorde commented Oct 22, 2020

guru-florida commented Oct 23, 2020

Briancbn commented Oct 24, 2020 • edited Loading

guru-florida commented Oct 27, 2020

guru-florida commented Oct 27, 2020

chapulina left a comment

Choose a reason for hiding this comment

guru-florida commented Nov 3, 2020

ahcorde commented Nov 3, 2020

chapulina left a comment

Choose a reason for hiding this comment

Briancbn commented Oct 21, 2020 •

edited

Loading

Briancbn commented Oct 24, 2020 •

edited

Loading