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

Introduction

The next technique we're going to explore is how to use the Whole-Body Controller Core (WBCC) for task space control of the robot arm's end effector. As in the robot-arm-one and robot-arm-two examples, this example uses the same 7-DoF robot arm which is controlled using the IHMC WBCC. As with the robot-arm-two example, this example highlights the protocol for setting the controller and possibly running it with 3 different modes: inverse dynamics, inverse kinematics, and virtual model control. This time, the end-effector is controlled in taskspace to follow a 3D position and orientation trajectory.

Understanding the Contents of the Project

Let's take a look at robot-arm-three project in the package explorer. The files included in this project should be as follows:

1. RobotArmThreeSimulation.java defines the control mode for the WBCC which can be changed to test the different modes. This file, as before, sets up and starts the simulation environment.
2. RobotArmThreeController.java holds the main difference between robot-arm-two and robot-arm-three. If you recall from robot-arm-two, we used the JointspaceFeedbackCommand, which triggered a set of JointspaceFeedbackControllers to control the robot arm movement. The IHMC Whole-Body Controller Core also comes with a set of other feedback controllers that are available independently from the active control mode. PID controllers allow for controlling the robot joints directly using JointspaceFeedbackController, or control the robot end-effector in taskspace using OrientationFeedbackController/PointFeedbackController/SpatialFeedbackController. It also allows for controlling the robot's center of mass using CenterOfMassFeedBackController. In this example, we used the SpatialFeedbackControlCommand to define feedback control goals for a given rigid body (in this case, the arm's end effector) in task-space rather than defining a collection of joint space goals. In addition to now formulating an objective for the end-effector instead of the joints, note that we use the additional command: PrivilegedConfigurationCommand. This command triggers an extra feature of the WBCC that can exploit the redundancy of a robot. It could, for example, 'attract' the joints towards the middle of the robot's range of motion range while the end-effector objective is still held as the highest priority. In other words, by using the PrivilegedConfigurationCommand, you can provide the WBCC with a preferred robot joint configuration that is then added as a secondary objective which is optimized for without disturbing the main objectives.

We are reusing the model and optimization parameters from the previous exercise (the build files establish a link between the previous project and the current one).

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

Run the Simulation

If you launch the RobotArmThreeSimulation.java class (Right Click -> Run As -> Java Application), you should see a familiar SCS simulation. However, if you press the Simulate button you should see a different simulation from before: The robot arm will trace a trajectory through task space as animated in the simulation GUI:

This example also highlights a feature of SCS: the YoGraphicDefinition. These YoGraphicDefinitions can be used to dynamically display and update 3D graphics in the simulation's 3D view, which can be extremely useful in providing intuitive feedback while debugging a complicated controller. It is used here to display the trajectory that the robot is tracking and the end-effector coordinate system.

When you're ready, let's keep going with the next and final example Example 4: The Robot Walker.