diff --git a/examples/worlds/joint_controller.sdf b/examples/worlds/joint_controller.sdf index 7152726bfa..8e56f3715c 100644 --- a/examples/worlds/joint_controller.sdf +++ b/examples/worlds/joint_controller.sdf @@ -91,6 +91,7 @@ 0.2 0.8 0.2 1 + 0.8 0 0 1 @@ -177,6 +178,7 @@ 0.2 0.8 0.2 1 + 0.8 0 0 1 diff --git a/examples/worlds/joint_position_controller.sdf b/examples/worlds/joint_position_controller.sdf index c39256aa1b..8265059ad7 100644 --- a/examples/worlds/joint_position_controller.sdf +++ b/examples/worlds/joint_position_controller.sdf @@ -85,6 +85,7 @@ 0.2 0.8 0.2 1 + 0.8 0 0 1 diff --git a/tutorials/files/joint_controllers/JoinControllerForceMode.gif b/tutorials/files/joint_controllers/JoinControllerForceMode.gif new file mode 100644 index 0000000000..0754b1eb07 Binary files /dev/null and b/tutorials/files/joint_controllers/JoinControllerForceMode.gif differ diff --git a/tutorials/files/joint_controllers/JointController.png b/tutorials/files/joint_controllers/JointController.png new file mode 100644 index 0000000000..b88711e4cf Binary files /dev/null and b/tutorials/files/joint_controllers/JointController.png differ diff --git a/tutorials/files/joint_controllers/JointControllerVelMode1.gif b/tutorials/files/joint_controllers/JointControllerVelMode1.gif new file mode 100644 index 0000000000..1f7578027d Binary files /dev/null and b/tutorials/files/joint_controllers/JointControllerVelMode1.gif differ diff --git a/tutorials/files/joint_controllers/JointControllerVelMode2.gif b/tutorials/files/joint_controllers/JointControllerVelMode2.gif new file mode 100644 index 0000000000..26d5280212 Binary files /dev/null and b/tutorials/files/joint_controllers/JointControllerVelMode2.gif differ diff --git a/tutorials/files/joint_controllers/JointPositionController.gif b/tutorials/files/joint_controllers/JointPositionController.gif new file mode 100644 index 0000000000..a5ae36a176 Binary files /dev/null and b/tutorials/files/joint_controllers/JointPositionController.gif differ diff --git a/tutorials/files/joint_controllers/JointTrajectoryController.gif b/tutorials/files/joint_controllers/JointTrajectoryController.gif new file mode 100644 index 0000000000..c1419854a0 Binary files /dev/null and b/tutorials/files/joint_controllers/JointTrajectoryController.gif differ diff --git a/tutorials/files/joint_controllers/JointTrajectoryController.png b/tutorials/files/joint_controllers/JointTrajectoryController.png new file mode 100644 index 0000000000..f1445fac29 Binary files /dev/null and b/tutorials/files/joint_controllers/JointTrajectoryController.png differ diff --git a/tutorials/joint_controller.md b/tutorials/joint_controller.md new file mode 100644 index 0000000000..12b77666e2 --- /dev/null +++ b/tutorials/joint_controller.md @@ -0,0 +1,675 @@ +\page jointcontrollers Joint Controllers + +Gazebo provides three joint controller plugins which are `JointController`, `JointPositionController`, and `JointTrajectoryController`. + +Let's see a detailed description of each of them and an example usage to help users select the right joint controller for their usage. + +## 1) JointController + +- Joint controller which can be attached to a model with a reference to a single joint. +- Currently, only the first axis of a joint can be actuated. + +### Modes of JointController + +1) Velocity mode: +This mode lets the user control the desired joint velocity directly. + +2) Force mode: +A user who wants to control joint velocity using a PID controller can use this mode. + +**Note**: This force mode is for the user who is looking to manually tune PID gains for velocity control according to a specific use case (e.g. Custom models). For general testing purposes, velocity mode will give the best results. + +All the parameters related to this controller can be found \ref gz::sim::systems::JointController "here". + +The commanded velocity(cmd_vel) can be published or subscribed at the topic: `/model//joint//cmd_vel` by default. + +Message data type: `Double` + +### Example usage + +Let's see an example for both modes using a simple model having only one joint. + +For controlling joints one would require adding the Gazebo's joint controller plugin to the existing `.sdf` file. + +1) Save the SDF file in the desired directory or create one + +e.g. + +```bash +mkdir gz_tutorial +cd gz_turtorial +``` + +2) In this tutorial we will be using the following SDF file (this is just a slight modification of the original `joint_controller.sdf` [example](https://github.com/gazebosim/gz-sim/blob/gz-sim7/examples/worlds/joint_controller.sdf)). + +After changing the directory, name the SDF file as `example.sdf` + +- SDF file: + +```xml + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 0 0 0 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.0 0.1 0 0 0 + + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.1 0.05 + + + + 0.2 0.8 0.2 1 + 0.8 0 0 1 + + + + + + 0.25 0.1 0.05 + + + + + + + world + base_link + + + + 0 0 -0.5 0 0 0 + base_link + rotor + + 0 0 1 + + + + + +``` + +3) Run the following command to launch the gazebo simulation: + +```bash +gz sim -v 4 -r example.sdf +``` + +This is how the model will look: + +
+ \image html files/joint_controllers/JointController.png width=50% +
+ +4) Now let's add the Gazebo JointController plugin to the SDF file. Add the following line to your file just before the tag ``. + +**Note**: All the plugins discussed here should be between `` and `` tags. Ideally just before the `` tag for better readability. + +- Velocity mode + +```xml + + j1 + 1.0 + +``` + +The initial velocity is set to 1.0 rad/s. + +
+ \image html files/joint_controllers/JointControllerVelMode1.gif width=50% +
+ +One can change the joint velocity by publishing a message on the topic `/model/joint_controller_demo/joint/j1/cmd_vel` or can change the topic name within the plugin. + +To change the topic name add the following line before `` tag in the SDF file. + +```xml +topic_name +``` + +- Sending velocity commands + +```bash +gz topic -t "/topic_name" -m gz.msgs.Double -p "data: 10.0" +``` + +
+ \image html files/joint_controllers/JointControllerVelMode2.gif width=50% +
+ +- Force mode + +Replace the velocity mode plugin mentioned above with the following lines in the SDF file for force mode. + +```xml + + j1 + true + 0.2 + 0.01 + +``` + +This would look almost the same as velocity mode if PID gains are tuned properly. + +5) Checking Joint states. +Here the state of the joint is obtained using the Gazebo’s JointStatepublisher plugin. Please visit \ref gz::sim::systems::JointStatePublisher for more information. +- Add the following lines to the SDF file before `` tag: +```xml + + j1 + +``` +- To check joint state. + +```bash +gz topic -e -t /world/default/model/joint_controller_demo/joint_state +``` + +```bash +joint { + name: "j1" + id: 12 + parent: "base_link" + child: "rotor" + pose { + position { + z: -0.5 + } + orientation { + w: 1 + } + } + axis1 { + xyz { + z: 1 + } + limit_lower: -inf + limit_upper: inf + position: 35.115896338490096 + velocity: 1.0000051832309742 + } +} +``` + +An example where p_gain was set to 2.0 and the joint controller failed to reach the desired velocity and behaved absurdly due to improper gains is shown below. + +
+ \image html files/joint_controllers/JoinControllerForceMode.gif width=50% +
+ +```bash +joint { + name: "j1" + id: 12 + parent: "base_link" + child: "rotor" + pose { + position { + z: -0.5 + } + orientation { + w: 1 + } + } + axis1 { + xyz { + z: 1 + } + limit_lower: -inf + limit_upper: inf + position: 44282.754868627489 + velocity: -2891.1685359866523 + } +} +``` + +## 2) JointPositionController + +- Joint position controller which can be attached to a model with a reference to a single joint. +- One can mention the axis of the joint they want to control. + +JointPositionController uses a PID controller to reach a desired joint position. + +All the parameters related to this controller can be found \ref gz::sim::systems::JointPositionController "here". + +Commanded position(cmd_pos) can be published or subscribed at the topic: `/model//joint///cmd_pos` by default. + +Message data type: `Double`. + +### Example usage: + +For this let's use the previously discussed SDF file. + +1) Replace the JointController plugin with the JointPositionController plugin in SDF file. + +```xml + + j1 + topic_name + 1 + 0.1 + 0.01 + 1 + -1 + 1000 + -1000 + +``` + +2) Sending joint position command. + +```bash +gz topic -t "/topic_name" -m gz.msgs.Double -p "data: -1.0" +``` + +
+ \image html files/joint_controllers/JointPositionController.gif width=50% +
+ + +3) Checking joint state. + +```bash +gz topic -e -t /world/default/model/joint_controller_demo/joint_state +``` + +```bash +joint { + name: "j1" + id: 12 + parent: "base_link" + child: "rotor" + pose { + position { + z: -0.5 + } + orientation { + w: 1 + } + } + axis1 { + xyz { + z: 1 + } + limit_lower: -inf + limit_upper: inf + position: 0.99999991907580654 + velocity: 8.1005154347602952e-09 + } +} +``` + +## 3) JointTrajectoryController. + +- Joint trajectory controller, which can be attached to a model with reference to one or more 1-axis joints to follow a trajectory. + +JointTrajectoryController lets’s user specify the required position, velocity, and effort with respect to time. For velocity and position, this controller uses a PID controller. + +A detailed description and related parameter of JointTrajectoryController can be found \ref gz::sim::systems::JointTrajectoryController "here". + +The trajectory message can be published or subscribed at `/model/${MODEL_NAME}/joint_trajectory` by default. + +Message type: [`JointTrajectory`](https://gazebosim.org/api/msgs/7.2/classignition_1_1msgs_1_1JointTrajectory.html) + +### Example usage: + +Let’s set up a new model for this example. A two-linked manipulator arm which has a total of two joints to control ([`joint_trajectory_controller.sdf`](https://github.com/gazebosim/gz-sim/blob/gz-sim7/examples/worlds/joint_trajectory_controller.sdf) is the original example). Name it as `example2.sdf`. + +- SDF file: + +```xml + + + + 0.4 0.4 0.4 + false + + + + + + false + 5 5 5 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -1 -1 -1 + + + + + + + -0.1 0 0 0 1.5708 0 + true + + + + + 0 0 1 + 5 5 + + + + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + + + + + + + + 0 0 0 0 -3.14159 0 + + + world + RR_position_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0 0.5 0.5 1 + 0 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_position_control_link0 + RR_position_control_link1 + + 1 0 0 + + 0.5 + + + + + 0 0 0.1 0 0 0 + RR_position_control_link1 + RR_position_control_link2 + + 1 0 0 + + 0.25 + + + + + + + +``` + +1) Launching gazebo simulation. + +```bash +gz sim -v 4 -r example2.sdf +``` + +This is how the model will look: + +
+ \image html files/joint_controllers/JointTrajectoryController.png width=50% +
+ +2) Adding the JointTrajectoryController plugin and let’s do position control for both joints. + +Append the following lines before `` tag in the SDF file. + +```xml + + RR_position_control_joint1 + 0.7854 + 20 + 0.4 + 1.0 + -1 + 1 + -20 + 20 + + RR_position_control_joint2 + -1.5708 + 10 + 0.2 + 0.5 + -1 + 1 + -10 + 10 + +``` + +3) Sending trajectory message (Can also change the topic name). + +```bash +gz topic -t "topic_name" -m gz.msgs.JointTrajectory -p ' + joint_names: "RR_position_control_joint1" + joint_names: "RR_position_control_joint2" + points { + positions: -0.7854 + positions: 1.5708 + time_from_start { + sec: 0 + nsec: 250000000 + } + } + points { + positions: -1.5708 + positions: 0 + time_from_start { + sec: 0 + nsec: 500000000 + } + } + points { + positions: -1.5708 + positions: -1.5708 + time_from_start { + sec: 0 + nsec: 750000000 + } + } + points { + positions: 0 + positions: 0 + time_from_start { + sec: 1 + nsec: 0 + } + } +``` + +
+ \image html files/joint_controllers/JointTrajectoryController.gif width=50% +
+ +**Note**: by default velocity and position control are disabled if one wants to use this mode, they must specify the PID gains value according to usage. + +In case, PID gains are not specified then by default force mode will work. + +4) Checking the progress of the commanded trajectory. + +```bash +gz topic -e -t "/model/RR_position_control/joint_trajectory_progress" +``` + +This returns the progress of the commanded trajectory which is a value between (0,1]. + +Finally, JointTrajectoryController can be used for hybrid control (e.g. In manipulator robots) where value from position PID, velocity PID, and commanded force value are summed up and applied.