Example 3: Simple Pendulum With Controller - ihmcrobotics/ihmc-open-robotics-software-tutorials GitHub Wiki

Introduction

Welcome! In the previous tutorial we had a simple pendulum swinging under the action of gravity and slowing down to its equilibrium position due to its damping. In this tutorial we will illustrate the action of a controller on joints by forcing the pendulum to keep a horizontal position.

To this end we will write a simple PID controller that will adjust the torque of the joint at the fulcrum. We will be using the same project from the last example, but now we will be examining and including SimplePendulumController.java in the simulation. If you haven't yet followed Example 2: Simple Pendulum Without Controller, I invite you to do so now.

Understanding the Controller

First, let's understand the purpose of a controller. We wish to cause the fulcrum joint to follow a prescribed position trajectory, but the actuator is commanded in terms of torque, so we must use some kind of control system to compute appropriate actuator commands that will realize this desired motion. Almost always, these torques are determined by using feedback from the joint sensors about the current state of the joint (position, velocity) to compute the torque required. (Source: Introduction to Robotics Mechanics, and Control Third Edition by John J. Craig)

For this example, we are using a PID ("proportional, integral, derivative") control law to compute this torque. We will explain this further later in the page.

Understanding the Contents of the Project

Now let's break down SimplePendulumController before jumping back into the simulation. The class implements the interface Controller. The following six functions implemented the interface:

  • initialize(): Initializes the controller. We will not be using it in this example.
  • doControl(): This is where the meat of the controller code is written. The function is called each update, thus, constantly updating the actual values to match the desired values.
  • pause(): Park the controller jobs when pausing the simulation.
  • getYoRegistry(): Returns a YoVariable registry object that you need to instantiate the controller. This object gives you access to the control variables. YoVariables are variables that act the same as regular primitive types, but they are able to be monitored in SCS (Simulation Construction Set).
  • getName(): Returns the name of the robot controller. We will not be using it in this example.
  • emptyController(): Returns an empty controller.

Before we dive deeper into the controller, let's go over some components in SimplePendulumDefinition that we'll need to implement doControl() in SimplePendulumController.

SimplePendulumDefinition.java

We are trying to control the fulcrum joint, so the variables we are interested in are the fulcrum joint's position, velocity, and torque. These variables need to be taken from the robot pendulumRobot, which is created according to the SimplePendulumDefinition and added to the simulation in the SimplePendulumDefinition file:

Robot pendulumRobot = new Robot(pendulumDefinition, scs.getInertialFrame());
pendulumRobot = scs.addRobot(pendulumDefinition);

These variables need to be accessible for the SimplePendulumController in order to allow the controller to calculate the proper torque to approach a desired joint position. The class Robot implements the RobotInterface and represents an actual robot model according to the definition in SimplePendulumDefinition. By that it also has access to the robot joints and their current state. To give the controller access to these variables and allow it to write to them to do feedback control (e.g., set a desired effort on a joint based on its current state) we pass pendulumRobot.getControllerInput() and pendulumRobot.getControllerOutput() as input to setup the SimplePendulumController.

Note on naming: By convention, adding a prefix of 'q', 'qd', and 'tau', refers to the joint angle, joint velocity, and joint torque, respectively. E.g. q_FulcrumPin, qd_FulcrumPin, tau_FulcrumPin.

For a more thorough explanation, I invite you to read through the code in the file. The code in each project of this tutorial has been notated heavily for user clarification, and to fully understand how the robot is built (both in the simulation and on the controller side), it would be good practice to read through the provided class files before or after running the simulations.

SimplePendulumController.java

Now let's get back to the SimplePendulumController components. In the constructor, we instantiate and initialize control variables: proportional gain p_gain, derivative gain d_gain, and integral gain i_gain, and set them to 250, 100, and 10 respectively.

doControl() Method

Let's take a look at the doControl() method. This method will be called at every control-tick. The method is using a PID control law to evaluate the torque to apply. postionError is computed as the difference between the desired position of the pendulum joint and its current position. integralError is computed by summing the positionError multiplied by the duration of that error. Since we want to control the pendulum to stay at a certain angle, our desired angular velocity is 0. Thus, the derivative error is calculated as 0 - current angular velocity. These three components are summed in other to set the torque.

// P.I.D control law
torque = p_gain.getDoubleValue() * positionError + i_gain.getDoubleValue() * integralError + d_gain.getDoubleValue() * (0 - fulcrumJoint.getQd());

For a more thorough explanation, I invite you to read through the code in the file. The code in each project of this tutorial has been notated heavily for user clarification, and to fully understand how the robot is built (both in the simulation and on the controller side), it would be good practice to read through the provided class files before or after running the simulations.

SimplePendulumSimulation.java

The last step is to associate the controller to the robot pendulumRobot in SCS.

pendulumRobot.addController(penulumController);

Run the Simulation

Open SimplePendulumSimulation.java. Ensure that the line

pendulumRobot.addController(penulumController);

is no longer commented out. In the newly opened Java editor, right click anywhere on the code then in the context menu select Run As > Java Application. Soon enough, SCS should appear. As before, look for the q_fulcrumPin variable in the search tab of the Variable Viewer. Create a new graph and drag the q_fulcrumPin into it. Also, look for the tau_fulcrumPin variable and add it to a new graph. This latter variable represents the torque calculated by the controller to set the pendulum to its desired horizontal position. Set the DesiredPosRad to the desired joint angle in radians.

Now press the Simulate button. This time the pendulum should start moving and very rapidly stabilize toward its desired angle position value.

javaw_73ASO791ZD

Experiment with the Simulation

You can now experiment with different initial parameters. We encourage you to modify the gains to see the implication of each gain. If you would like another challenge, move on to Simple Pendulum: Feedforward Compensation to experiment with another controller for this pendulum.