PID effort control of joints (+mimic) #122

livanov93 · 2023-02-22T16:27:02Z

Depends on #121

Description

PR introduces mapping all types of command interfaces (position, velocity, effort) to ForceJointCmd component.:

For regular joints it does that in the following manner:

introduces pid controllers for position and velocity
creates parameter node for each hardware interface so the PID values can be tuned in runtime
- https://github.com/livanov93/ign_ros2_control/blob/pid-with-force-output/ign_ros2_control/src/ign_ros2_control_parameters.yaml
- https://github.com/livanov93/ign_ros2_control/blob/e8ac35e2cb2ffc223d33dfed23334908ca291a34/ign_ros2_control/src/ign_system.cpp#L784-L785
enables cascade control using https://github.com/livanov93/ign_ros2_control/blob/e8ac35e2cb2ffc223d33dfed23334908ca291a34/ign_ros2_control/src/ign_ros2_control_parameters.yaml#L10-L16

For mimic joints it does that in the following manner:

Using the MimicJointSystem that can be found here which is pure ignition/gz system that is disconnected from ROS ecosystem.
- it has PID controller which is trying to satisfy desired mimic behavior defined by multiplier and offset. It maps position error to force output which is applied to the joint.
- PID parameters can't be changed in runtime, they must be specified within sdf tags
Using the same approach as for regular joints within ign_ros2_control::IgnitionSystemInterface here
- cascade control is also an option for mimic joints

ign_ros2_control/src/MimicJointSystem.cc

ign_ros2_control/src/ign_system.cpp

AndyZe

I've been hearing good things from testing. For example, this can actually handle contact with the environment. That's a big win 👍

ign_ros2_control/CMakeLists.txt

ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh

ign_ros2_control/src/MimicJointSystem.cc

ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh

ign_ros2_control/include/ign_ros2_control/ign_system.hpp

ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf

AndyZe · 2023-03-20T21:03:41Z

Can you rename the PR, "PID effort control of a mimic joint" or something like that?

…er by default.

…r removal.

livanov93 · 2023-03-24T14:52:06Z

@AndyZe @kylc Please take a look into changes I implemented and comments I had on your questions/suggestions.
Thanks!

AndyZe · 2023-03-24T15:37:59Z

@livanov93 it would help reviewers a lot in the future if you put a nice, long description in the PR. Especially on a 1,000+ line PR like this. Why did you make the design decisions you have made? Why did you choose to have 2 modes, ABS and PID? Then your reviewers won't have to ask ;)

AndyZe · 2023-03-24T15:36:07Z

ign_ros2_control/include/ign_ros2_control/ign_system.hpp

@@ -25,6 +25,10 @@
 #include "rclcpp_lifecycle/state.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"

+#include "ign_ros2_control_parameters.hpp"


TODO still (nitpick)

ign_ros2_control/src/MimicJointSystem.cc

livanov93 · 2023-03-24T15:57:35Z

@livanov93 it would help reviewers a lot in the future if you put a nice, long description in the PR. Especially on a 1,000+ line PR like this. Why did you make the design decisions you have made? Why did you choose to have 2 modes, ABS and PID? Then your reviewers won't have to ask ;)

I was writing it at the moment you wrote this. See here.

kylc

@livanov93 Thank you for addressing my feedback. This looks good to me!

AndyZe

I won't block this any more. I think it's leaps and bounds ahead of what we had before.

ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh

ign_ros2_control/src/MimicJointSystem.cc

danzimmerman · 2023-03-28T04:48:11Z

I've been hearing good things from testing. For example, this can actually handle contact with the environment. That's a big win 👍

@AndyZe @livanov93 This seems like a great change.

I'm curious what you're seeing re: joint position accuracy. Sounds from some other issues (#113, for example) there are/were kinematic accuracy issues and sag with the prior position controllers anyway.

I'd imagine the cascaded controller coming in here could be better at achieving low joint position error than the effort_controllers/JointTrajectoryController was for ROS 1 UR simulation in Gazebo Classic. I observed pretty large errors there, while the regular position controller (which I think just drives ODE joint motors) was dead on, but of course couldn't touch anything rigid:

ros-industrial/universal_robot#521 (comment)

It'll be really nice to have a simulated robot that can make contact with a rigid environment. I was getting back to working on some ideas about elastic mounting, etc. in Gazebo Classic but maybe I should shift focus and try this out in new Gazebo first.

livanov93 · 2023-04-03T09:06:10Z

@ahcorde friendly ping for a review.

ahcorde

Sorry for the late response I was out some weeks, I made a initial pass and I will test it right today

ahcorde · 2023-05-12T12:23:39Z

ign_ros2_control/CMakeLists.txt

@@ -39,10 +44,30 @@ elseif("$ENV{IGNITION_VERSION}" STREQUAL "fortress")
  set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
  message(STATUS "Compiling against Ignition Fortress")

+  find_package(ignition-cmake2 REQUIRED)


duplicated code in the if and else

ahcorde · 2023-05-12T12:24:56Z

ign_ros2_control/src/MimicJointSystem.cc

+#include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/JointType.hh"


alphabetize

ahcorde · 2023-05-12T12:25:56Z

ign_ros2_control/src/MimicJointSystem.cc

+      ignerr << "MimicJointSystem  Failed to initialize because [" <<
+        this->dataPtr->model.Name(_ecm)
+             << "] (Entity=" << _entity << ")] is not a model.. ";


Suggested change

ignerr << "MimicJointSystem Failed to initialize because [" <<

this->dataPtr->model.Name(_ecm)

<< "] (Entity=" << _entity << ")] is not a model.. ";

ignerr << "MimicJointSystem Failed to initialize because ["

<< this->dataPtr->model.Name(_ecm)

<< "] (Entity=" << _entity << ")] is not a model.. ";

ahcorde · 2023-05-12T12:27:13Z

ign_ros2_control/src/MimicJointSystem.cc

+      ignwarn << "Detected jump back in time [" << std::chrono::duration_cast <
+        std::chrono::seconds > (_info.dt).count()
+              << "s]. System may not work properly." << std::endl;


Suggested change

ignwarn << "Detected jump back in time [" << std::chrono::duration_cast <

std::chrono::seconds > (_info.dt).count()

<< "s]. System may not work properly." << std::endl;

ignwarn << "Detected jump back in time ["

<< std::chrono::duration_cast <std::chrono::seconds > (_info.dt).count()

<< "s]. System may not work properly." << std::endl;

ahcorde · 2023-05-12T12:28:14Z

ign_ros2_control/src/ign_system.cpp

@@ -16,6 +16,8 @@

 #include <ignition/msgs/imu.pb.h>

+#include <iostream>
+


Suggested change

ahcorde · 2023-05-12T12:28:35Z

ign_ros2_control/src/ign_system.cpp

+#include <ignition/gazebo/components/JointVelocityReset.hh>
+
+#include <ignition/gazebo/components/JointAxis.hh>
+#include <ignition/gazebo/components/JointType.hh>
+#include <ignition/gazebo/components/Joint.hh>


alphabetize

Suggested change

#include <ignition/gazebo/components/JointVelocityReset.hh>

#include <ignition/gazebo/components/JointAxis.hh>

#include <ignition/gazebo/components/JointType.hh>

#include <ignition/gazebo/components/Joint.hh>

#include <ignition/gazebo/components/JointVelocityReset.hh>

#include <ignition/gazebo/components/JointAxis.hh>

#include <ignition/gazebo/components/JointType.hh>

#include <ignition/gazebo/components/Joint.hh>

ahcorde · 2023-05-12T12:29:59Z

ign_ros2_control/src/ign_system.cpp

@@ -417,15 +623,9 @@ void IgnitionSystem::registerSensors(
      }

      static const std::map<std::string, size_t> interface_name_map = {
-        {"orientation.x", 0},


to reduce the changes, can you restore these changes? Same with other similar changes

ahcorde · 2023-05-12T12:30:22Z

ign_ros2_control/src/ign_system.cpp

 {
  RCLCPP_WARN(this->nh_->get_logger(), "On init...");
+


Suggested change

ahcorde

add gripper_controllers to ign_ros2_control package.xml

ahcorde

add gripper_controllers to ign_ros2_control package.xml

livanov93 added 2 commits February 22, 2023 17:25

Introduce PID for position control.

bb54c12

Add generate parameter library.

0104d7a

livanov93 force-pushed the pid-with-force-output branch from 55f6364 to 0104d7a Compare February 23, 2023 23:29

livanov93 added 2 commits February 24, 2023 11:06

Refresh params every loop step.

c018f7c

Reorganize calcs.

cb75106

livanov93 force-pushed the pid-with-force-output branch from 5e6d70c to cb75106 Compare February 24, 2023 11:48

Add pid params via hardware parameters.

6d5eda3

AndyZe mentioned this pull request Mar 1, 2023

Add full PID control, plus small bug fixes #117

Closed

livanov93 added 7 commits March 2, 2023 09:44

Add pid sim params into demos urdfs.

87496ff

Testing clamping.

c11b195

Change pid params. Cascade control. Comment mimic stuff for now.

c3bd601

Debug output.

29e9943

Adapt pid params for examples.

25187ea

Move all control to force - based output.

01e6f68

Introduce MimicJointSystem plugin.

ea36469

livanov93 force-pushed the pid-with-force-output branch from de3e194 to ea36469 Compare March 16, 2023 20:29

Add mimic joint vel pid only support.

65b7388

kylc suggested changes Mar 17, 2023

View reviewed changes

Round commanded force.

1a89e06

livanov93 marked this pull request as ready for review March 20, 2023 12:21

livanov93 requested a review from ahcorde as a code owner March 20, 2023 12:21

livanov93 mentioned this pull request Mar 20, 2023

Introduce Simulation PIDs parameters UniversalRobots/Universal_Robots_ROS2_Description#54

Open

AndyZe reviewed Mar 20, 2023

View reviewed changes

livanov93 changed the title ~~Pid with force output~~ PID effort control of joints (+mimic) Mar 21, 2023

livanov93 added 5 commits March 24, 2023 15:16

Remove suffix for state interfaces as it confuses robot_state_publish…

9bbb874

…er by default.

Add cascade control parameter. Code refactoring.

e40142c

Identation, alphabetizing, usage description comment, empty destructo…

6c671e4

…r removal.

Merge branch 'humble' into pid-with-force-output

3b47f98

Fix error.

1831835

Namespace fix.

e8ac35e

AndyZe reviewed Mar 24, 2023

View reviewed changes

livanov93 added 2 commits March 24, 2023 17:50

Adapt pid values for simple position example.

64f6b67

Suggestions and fixes for humble CI. write method return value handling.

6c5ecef

livanov93 requested review from AndyZe and kylc and removed request for kylc March 27, 2023 08:37

livanov93 force-pushed the pid-with-force-output branch from cf8b3b5 to b00a6bf Compare March 27, 2023 09:37

Satisfy ament tools.

a100537

livanov93 force-pushed the pid-with-force-output branch from b00a6bf to a100537 Compare March 27, 2023 09:53

Fix linters by bypassing for specific lines.

496dde5

livanov93 force-pushed the pid-with-force-output branch from 06c7536 to 496dde5 Compare March 27, 2023 10:48

livanov93 requested a review from kylc March 27, 2023 10:53

kylc approved these changes Mar 27, 2023

View reviewed changes

AndyZe approved these changes Mar 27, 2023

View reviewed changes

ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh Outdated Show resolved Hide resolved

ign_ros2_control/src/MimicJointSystem.cc Outdated Show resolved Hide resolved

danzimmerman mentioned this pull request Mar 28, 2023

Add conditional joint friction to stabilize Gazebo Classic simulation. UniversalRobots/Universal_Robots_ROS2_Description#55

Closed

Remove suggestion comment.

460d47c

ahcorde requested changes May 12, 2023

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

PID effort control of joints (+mimic) #122

PID effort control of joints (+mimic) #122

livanov93 commented Feb 22, 2023 •

edited

AndyZe left a comment

AndyZe commented Mar 20, 2023 •

edited

livanov93 commented Mar 24, 2023

AndyZe commented Mar 24, 2023

AndyZe Mar 24, 2023

livanov93 commented Mar 24, 2023

kylc left a comment

AndyZe left a comment

danzimmerman commented Mar 28, 2023 •

edited

livanov93 commented Apr 3, 2023

ahcorde left a comment

ahcorde May 12, 2023

ahcorde May 12, 2023

ahcorde May 12, 2023

ahcorde May 12, 2023

ahcorde May 12, 2023

ahcorde May 12, 2023

ahcorde May 12, 2023

ahcorde May 12, 2023

ahcorde left a comment

ahcorde left a comment

		#include "ignition/gazebo/components/Name.hh"
		#include "ignition/gazebo/components/JointType.hh"

		@@ -16,6 +16,8 @@

		#include <ignition/msgs/imu.pb.h>

		#include <iostream>

PID effort control of joints (+mimic) #122

Are you sure you want to change the base?

PID effort control of joints (+mimic) #122

Conversation

livanov93 commented Feb 22, 2023 • edited

Description

AndyZe left a comment

Choose a reason for hiding this comment

AndyZe commented Mar 20, 2023 • edited

livanov93 commented Mar 24, 2023

AndyZe commented Mar 24, 2023

Choose a reason for hiding this comment

livanov93 commented Mar 24, 2023

kylc left a comment

Choose a reason for hiding this comment

AndyZe left a comment

Choose a reason for hiding this comment

danzimmerman commented Mar 28, 2023 • edited

livanov93 commented Apr 3, 2023

ahcorde left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ahcorde left a comment

Choose a reason for hiding this comment

ahcorde left a comment

Choose a reason for hiding this comment

livanov93 commented Feb 22, 2023 •

edited

AndyZe commented Mar 20, 2023 •

edited

danzimmerman commented Mar 28, 2023 •

edited