Example 2: Using the IHMC Whole Body Controller Core to Control the Robot Arm in Joint Space - ihmcrobotics/ihmc-open-robotics-software-tutorials GitHub Wiki

Introduction

Now that we are familiar with the SCS, we can introduce the IHMC whole-body Controller Core (WBCC) and replicate the same feedback control concepts using the IHMC WBCC.

As in the robot-arm-one example, we will use the same 7-DoF robot arm which is now controlled using the IHMC WBCC to track the same sine wave trajectories for the shoulder yaw and elbow pitch. This example highlights the protocol for setting the controller core and the possibility to run it with 3 different modes:

1. Inverse Dynamics: Given desired accelerations and contact states, the controller core can compute desired joint torques. A QP solver is shaped to reconcile these objectives and to output the 'optimal' joint accelerations and external wrenches. This can then be consumed by an inverse dynamics calculator, converting to desired joint effort. This is the framework that we use on the humanoid robots: Atlas and Valkyrie.
2. Inverse Kinematics: Given desired velocities, the controller core can integrate these velocities to output both desired joint velocities and positions. As with the Inverse Dynamics, a QP solver is shaped to reconcile these objectives and output desired joint velocities. An integrator is also available which uses the desired velocities to calculate the desired joint positions. We use this framework in our whole-body inverse kinematics solver and our whole-body trajectory planner.
3. Virtual Model Control: This control mode generalizes the "Jacobian transpose" control approach. Desired joint torques, desired end-effector virtual forces/torques, desired rate of change of momentum, and rigid-body parameters can be formulated as objectives. A QP solver is used to optimize the ground force distribution among the supporting bodies and outputs desired joint effort. We use this control framework in our locomotion controller for quadrupedal robots.

Even though this example applies the controller core on a simple system, the procedure for controlling a much more complicated robot system remains extremely similar.

Understanding the Contents of the Project

Now let's take a look at the robot-arm-two project in the package explorer. Here is the set of classes you should see in this project:

1. We reuse the robot definition from the last example to define the robot -> RobotArmOneDefinition.
2. RobotArmTwoOptimizationSettings.java contains all of the relevant parameters for configuring the optimization solver.
3. RobotArmTwoSimulation.java sets up and starts the simulation environment. The controlMode variable of type WholeBodyControllerCoreMode in this class allows for the switching between the three control frameworks highlighted above (inverse dynamics, inverse kinematics, and virtual model).
4. RobotArmTwoController.java defines a feedback controller similar to the previous example, but this time using the controller core. The way the controller core knows what to do is through the sending of commands to it. It is kind of like how your brain tells you to stand up, walk two paces, etc. and your muscles must react by executing the commands. The controller core is sent the commands in RobotArmTwoController, and relays that information to the joints of the robot arm. In this example, we are only interested in controlling the robot in jointspace, so we make a template of seven JointspaceFeedbackControlCommands, one per joint, and then send that template when creating the controller core. Then, in RobotArmTwoController.doControl(), the specific joint commands are calculated and sent to the controller core:

for (OneDoFJointBasics joint : controlledJoints)
{
jointCommands.addJointCommand(joint);
}
allPossibleCommands.addCommand(jointCommands);
return new WholeBodyControllerCore(toolbox, new FeedbackControllerTemplate(allPossibleCommands), registry);

Now would be a good time to examine the code to fully understand how the overall project is working.

Run the Simulation

Now let's run the simulation. Launch the RobotArmTwoSimulation.java class (Right Click -> Run As -> Java Application) to enter the SCS simulation. If you press the Simulate button, you should more or less see the same view as in the previous example, but now the tracking performance of the controller is much better:

Experiment with the Simulation

Feel free to study the code and play with the SCS GUI. For example, try observing the tracking of the elbow pitch joint. For this, add a new graph. Search for the elbow pitch desired position variable desiredPositionElbowPitch and drag it to the new graph, then do the same with the variable q_elbowPitch which represents the current joint position. The same variables are available for monitoring the joint velocity tracking: desiredVelocityElbowPitch and qd_elbowPitch.

You can also try the other control modes and observe how the performance changes.

When you are ready, let's go on with the next example that introduces how to achieve an objective in taskspace Example 3: Using the IHMC Whole Body Controller Core to Control the Robot Arm in Task Space.