Simple controller

Note

The source code for this example can be found in [orca_root]/examples/basic/01-simple_controller.cc, or alternatively on github at: https://github.com/syroco/orca/blob/dev/examples/basic/01-simple_controller.cc

Objective

In this example we want to show the basics of using ORCA. Here, we create a minimal controller with one task and some common constraints.

Introduction

First we need to include the appropriate headers and use the right namespaces. When you are getting started the easiest solution is to use the helper header orca.h and helper namespace orca::all which include all the necessary headers and opens up all their namespaces. This helps with reducing the verbosity of the examples here but is not recommended for production builds because it will cause code bloat.

#include <orca/orca.h>
using namespace orca::all;

We then create our main() function…

int main(int argc, char const *argv[])

and parse the command line arguments:

if(argc < 2)
{
    std::cerr << "Usage : " << argv[0] << " /path/to/robot-urdf.urdf (optionally -l debug/info/warning/error)" << "\n";
    return -1;
}
std::string urdf_url(argv[1]);

orca::utils::Logger::parseArgv(argc, argv);

ORCA provides a utility class called Logger which, as its name implies, helps log output. See the API documentation for more information on logging levels.

Setup

Now we get to the good stuff. We start by creating a robot model which gives us access to the robot’s kinematics and dynamics.

auto robot_model = std::make_shared<RobotModel>();
robot->loadModelFromFile(urdf_url);
robot->setBaseFrame("base_link");
robot->setGravity(Eigen::Vector3d(0,0,-9.81));

We first instantiate a shared_ptr to the class RobotModel. We can pass a robot name, but if we don’t, it is extracted from the urdf, which is loaded from a file in robot->loadModelFromFile(urdf_url);. If the URDF is parsed then we need to set the base frame in which all transformations (e.g. end effector pose) are expressed in robot->setBaseFrame("base_link");. Finally we manually set the gravity vector robot->setGravity(Eigen::Vector3d(0,0,-9.81)); (this is optional).

The next step is to set the initial state of the robot. For your convenience, ORCA provides a helper class called EigenRobotState which stores the whole state of the robot as eigen vectors/matrices. This class is totally optional, it is just meant to keep consistency for the sizes of all the vectors/matrices. You can use it to fill data from either a real robot or simulated robot.

EigenRobotState eigState;
eigState.resize(robot->getNrOfDegreesOfFreedom());
eigState.jointPos.setZero();
eigState.jointVel.setZero();
robot->setRobotState(eigState.jointPos,eigState.jointVel);

First we resize all the vectors/matrices to match the robot configuration and set the joint positions and velocities to zero. Initial joint positions are often non-zero but we are lazy and setZero() is so easy to type. Finally, we set the robot state, robot->setRobotState(eigState.jointPos,eigState.jointVel);. Now the robot is considered ‘initialized’.

Note

Here we only set \boldsymbol{q}, \dot{\boldsymbol{q}} because in this example we are dealing with a fixed base robot.

Creating the Controller

With the robot created and initialized, we can construct a Controller:

// Instanciate an ORCA Controller
orca::optim::Controller controller(
    "controller"
    ,robot
    ,orca::optim::ResolutionStrategy::OneLevelWeighted
    ,QPSolver::qpOASES
);

To do so we pass a name, "controller", the robot model, robot, a ResolutionStrategy, orca::optim::ResolutionStrategy::OneLevelWeighted, and a solver, QPSolver::qpOASES.

Note

As of now, the only supported solver is qpOASES, however OSQP will be integrated in a future release.

Note

Other ResolutionStrategy options include: MultiLevelWeighted, and Generalized. Please be aware that these strategies are not yet officially supported.

If your robot’s low level controller takes into account the gravity and coriolis torques already (Like with KUKA LWR) then you can tell the controller to remove these components from the torques computed by the solver. Setting them to false keeps the components in the solution (this is the default behavior).

controller.removeGravityTorquesFromSolution(true);
controller.removeCoriolisTorquesFromSolution(true);

Adding Tasks

With the controller created we can now start adding tasks. In this introductory example, we add only a Cartesian acceleration task for the end-effector.

auto cart_task = std::make_shared<CartesianTask>("CartTask_EE");
controller.addTask(cart_task);

A shared_ptr to a CartesianTask is created with a unique name, CartTask_EE. The task is then added to the controller to initialize it.

For this task, we want to control link_7,

cart_task->setControlFrame("link_7");

And set its desired pose:

Eigen::Affine3d cart_pos_ref;
cart_pos_ref.translation() = Eigen::Vector3d(1.,0.75,0.5); // x,y,z in meters
cart_pos_ref.linear() = Eigen::Quaterniond::Identity().toRotationMatrix();

We also set the desired cartesian velocity and acceleration to zero.

Vector6d cart_vel_ref = Vector6d::Zero();
Vector6d cart_acc_ref = Vector6d::Zero();

Note

Rotation is done with a Matrix3x3 and it can be initialized in a few ways. Note that each of these methods produce equivalent Rotation matrices in this case.

Example 1: create a quaternion from Euler anglers ZYZ convention

Eigen::Quaterniond quat;
quat = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())
     * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
     * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
cart_pos_ref.linear() = quat.toRotationMatrix();

Example 2: create a quaternion from RPY convention

cart_pos_ref.linear() = quatFromRPY(0,0,0).toRotationMatrix();

Example 3: create a quaternion from Kuka Convention

cart_pos_ref.linear() = quatFromKukaConvention(0,0,0).toRotationMatrix();

Example 4: use an Identity quaternion

cart_pos_ref.linear() = Eigen::Quaterniond::Identity().toRotationMatrix();

The desired values are set on the servo controller because CartesianTask expects a cartesian acceleration, which is computed automatically by the servo controller.

cart_task->servoController()->setDesired(cart_pos_ref.matrix(),cart_vel_ref,cart_acc_ref);

Now set the servoing PID

Vector6d P;
P << 1000, 1000, 1000, 10, 10, 10;
cart_task->servoController()->pid()->setProportionalGain(P);
Vector6d D;
D << 100, 100, 100, 1, 1, 1;
cart_task->servoController()->pid()->setDerivativeGain(D);

Adding Constraints

Now we add some constraints. We start with a joint torque constraint for all the actuated DoF. To create it we first get the number of actuated joints from the model.

const int ndof = robot->getNrOfDegreesOfFreedom();

The joint torque limit is usually given by the robot manufacturer and included in most robot descriptions, but for now it is not parsed directely from the URDF - so we need to add it manually.

auto jnt_trq_cstr = std::make_shared<JointTorqueLimitConstraint>("JointTorqueLimit");
controller.addConstraint(jnt_trq_cstr);
Eigen::VectorXd jntTrqMax(ndof);
jntTrqMax.setConstant(200.0);
jnt_trq_cstr->setLimits(-jntTrqMax,jntTrqMax);

We first create a shared_ptr with a unique name, auto jnt_trq_cstr = std::make_shared<JointTorqueLimitConstraint>("JointTorqueLimit"); and add it to the controller controller.addConstraint(jnt_trq_cstr);. We then set the torque limits to \pm{}200 Nm.

Contrary to torque limits, joint position limits are automatically extracted from the URDF model. Note that you can set them if you want by simply doing jnt_pos_cstr->setLimits(jntPosMin,jntPosMax).

auto jnt_pos_cstr = std::make_shared<JointPositionLimitConstraint>("JointPositionLimit");
controller.addConstraint(jnt_pos_cstr);

Joint velocity limits are usually given by the robot manufacturer but like the torque limits, must be added manually for now.

auto jnt_vel_cstr = std::make_shared<JointVelocityLimitConstraint>("JointVelocityLimit");
controller.addConstraint(jnt_vel_cstr);
Eigen::VectorXd jntVelMax(ndof);
jntVelMax.setConstant(2.0);
jnt_vel_cstr->setLimits(-jntVelMax,jntVelMax);

With the tasks anc constraints created and added to the controller, we can begin the control loop.

Control Loop

The control loop is where the robot model is updated using the current state information from the real or simulated robot, the control problem is formulated and solved, and the resultant joint torques are sent to the robot actuators. For this example, we simply calculate the joint torques \boldsymbol{\tau} at each control time step and do nothing with them. This is because we are not interacting with a real robot or a simulated robot.

double dt = 0.001;
double current_time = 0;

controller.activateTasksAndConstraints();

for (; current_time < 2.0; current_time +=dt)
{
    // Here you can get the data from your robot (API is robot-specific)
    // Something like :
        // eigState.jointPos = myRealRobot.getJointPositions();
        // eigState.jointVel = myRealRobot.getJointVelocities();

    robot->setRobotState(eigState.jointPos,eigState.jointVel);
    controller.update(current_time, dt);
    if(controller.solutionFound())
    {
        const Eigen::VectorXd& trq_cmd = controller.getJointTorqueCommand();

        // Send torques to the REAL robot (API is robot-specific)
        // myRealRobot.set_joint_torques(trq_cmd);
    }
    else
    {
        // WARNING : Optimal solution is NOT found
        // Perform some fallback strategy (see below)
    }
}

First, since we are manually stepping the time, we initialize the current_time to zero and the dt=0.001.

The next important step is to activate the tasks and constraints: controller.activateTasksAndConstraints();. This must be done before the controller update is called, or else no solution will be found.

Now that the tasks and constraints are activated, we step into the control loop, which increments current_time from 0.0 to 2.0 seconds by dt:

for (; current_time < 2.0; current_time +=dt)

At the begining of each loop, we must first retrieve the robot’s state information so that we can update our robot model being used in the controller. This step depends on the robot-specific API being used and is up to the user to implement.

Note

In future examples we demonstrate how to do this with the Gazebo simulator.

After we get the appropriate state information from our robot (in this case, the joint positions and velocities) we update the robot model: robot->setRobotState(eigState.jointPos,eigState.jointVel);. With the model updated we now update the controller, controller.update(current_time, dt);. The controller update first updates all of the tasks and constraints, then formulates the optimal control problem, then solves said problem. If the controller found a solution to the optimal control problem then controller.solutionFound() will return true and this tells you that you can get that result and use it to control your robot. Here we extract the optimal control torques, const Eigen::VectorXd& trq_cmd = controller.getJointTorqueCommand(); and then send them to our robot, using robot specific functions.

Note

In this example, we extract only the optimal torques, but you of course have access to the full solution:

// The whole optimal solution [AccFb, Acc, Tfb, T, eWrenches]
const Eigen::VectorXd& full_solution = controller.getSolution();
// The optimal joint torque command
const Eigen::VectorXd& trq_cmd = controller.getJointTorqueCommand();
// The optimal joint acceleration command
const Eigen::VectorXd& trq_acc = controller.getJointAccelerationCommand();

If the controller fails to find a solution to the problem then controller.solutionFound() returns false, and you must implement some fallback strategy. By fallback, we mean some strategy to be used when we have no idea what torques to send to the robot. A simple but effective strategy, is to simply brake the robot and stop its motion.

Important

If the optimal control problem has no solution it is generally because the tasks and constraints are ill-defined and not because no solution exists. For this reason, one can implement fallback strategies which are slightly more intelligent than simply stopping the robot. For example: - Compute KKT Solution and send to the robot (solutions without inequality constraints) - PID around the current position (to slow to a halt) - Switch controllers - etc.

Shutting Things Down

Once we are finished using the controller and want to bring everything to a stop, we need to gradually deactivate the tasks and constraints to avoid any erratic behaviors at the end of the motion. To do so, we start by deactivating the tasks and constraints:

controller.deactivateTasksAndConstraints();

We then need to update the controller so the tasks and constraints can slowly ramp down to total deactivation.

while(!controller.tasksAndConstraintsDeactivated())
{
    current_time += dt;
    controller.update(current_time,dt);
}

Our controller is now deactivated and can be deleted or destroyed without any issues.

Typically at the end of the execution you would either stop the robot or put it into some robot-specific control mode (position control, gravity compensation, etc.).

Conclusion

In this example you have seen all of the necessary steps to getting an ORCA controller up and running. In the next examples we will look at more realistic examples where the controller interacts with a robot/simulation.

Full Code Listing

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
// This file is a part of the ORCA framework.
// Copyright 2017, ISIR / Universite Pierre et Marie Curie (UPMC)
// Copyright 2018, Fuzzy Logic Robotics
// Main contributor(s): Antoine Hoarau, Ryan Lober, and
// Fuzzy Logic Robotics <info@fuzzylogicrobotics.com>
//
// ORCA is a whole-body reactive controller framework for robotics.
//
// This software is governed by the CeCILL-C license under French law and
// abiding by the rules of distribution of free software.  You can  use,
// modify and/ or redistribute the software under the terms of the CeCILL-C
// license as circulated by CEA, CNRS and INRIA at the following URL
// "http://www.cecill.info".
//
// As a counterpart to the access to the source code and  rights to copy,
// modify and redistribute granted by the license, users are provided only
// with a limited warranty  and the software's author,  the holder of the
// economic rights,  and the successive licensors  have only  limited
// liability.
//
// In this respect, the user's attention is drawn to the risks associated
// with loading,  using,  modifying and/or developing or reproducing the
// software by the user in light of its specific status of free software,
// that may mean  that it is complicated to manipulate,  and  that  also
// therefore means  that it is reserved for developers  and  experienced
// professionals having in-depth computer knowledge. Users are therefore
// encouraged to load and test the software's suitability as regards their
// requirements in conditions enabling the security of their systems and/or
// data to be ensured and,  more generally, to use and operate it in the
// same conditions as regards security.
//
// The fact that you are presently reading this means that you have had
// knowledge of the CeCILL-C license and that you accept its terms.

/** @file
 @copyright 2018 Fuzzy Logic Robotics <info@fuzzylogicrobotics.com>
 @author Antoine Hoarau
 @author Ryan Lober
*/


#include <orca/orca.h>
using namespace orca::all;

int main(int argc, char const *argv[])
{
    // Get the urdf file from the command line
    if(argc < 2)
    {
        std::cerr << "Usage : " << argv[0] << " /path/to/robot-urdf.urdf (optionally -l debug/info/warning/error)" << "\n";
        return -1;
    }
    std::string urdf_url(argv[1]);

    //  Parse logger level as --log_level (or -l) debug/warning etc
    orca::utils::Logger::parseArgv(argc, argv);

    // Create the kinematic model that is shared by everybody. Here you can pass a robot name
    auto robot_model = std::make_shared<RobotModel>();

     //  If you don't pass a robot name, it is extracted from the urdf
    robot_model->loadModelFromFile(urdf_url);

    // All the transformations (end effector pose for example) will be expressed wrt this base frame
    robot_model->setBaseFrame("base_link");

    // Sets the world gravity (Optional)
    robot_model->setGravity(Eigen::Vector3d(0,0,-9.81));

    // This is an helper function to store the whole state of the robot as eigen vectors/matrices. This class is totally optional, it is just meant to keep consistency for the sizes of all the vectors/matrices. You can use it to fill data from either real robot and simulated robot.
    RobotState eigState;

    // resize all the vectors/matrices to match the robot configuration
    eigState.resize(robot_model->getNrOfDegreesOfFreedom());

    // Set the initial state to zero (arbitrary). @note: here we only set q,qot because this example asserts we have a fixed base robot
    eigState.jointPos.setZero();
    eigState.jointVel.setZero();

    // Set the first state to the robot
    robot_model->setRobotState(eigState.jointPos,eigState.jointVel);
    // Now is the robot is considered 'initialized'


    // Instanciate an ORCA Controller
    orca::optim::Controller controller(
        "controller"
        ,robot_model
        ,orca::optim::ResolutionStrategy::OneLevelWeighted
        ,QPSolverImplType::qpOASES
    );
    // Other ResolutionStrategy options: MultiLevelWeighted, Generalized

    
    // Create the servo controller that the cartesian task needs
    auto cart_acc_pid = std::make_shared<CartesianAccelerationPID>("servo_controller");
    
    // Set the pose desired for the link_7
    Eigen::Affine3d cart_pos_ref;

    // Setting the translational components.
    cart_pos_ref.translation() = Eigen::Vector3d(1.,0.75,0.5); // x,y,z in meters

    // Rotation is done with a Matrix3x3 and it can be initialized in a few ways. Note that each of these methods produce equivalent Rotation matrices in this case.

    // Example 1 : create a quaternion from Euler anglers ZYZ convention
    Eigen::Quaterniond quat;
    quat = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())
         * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
         * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
    cart_pos_ref.linear() = quat.toRotationMatrix();

    // Example 2 : create a quaternion from RPY convention
    cart_pos_ref.linear() = quatFromRPY(0,0,0).toRotationMatrix();

    // Example 3 : create a quaternion from Kuka Convention
    cart_pos_ref.linear() = quatFromKukaConvention(0,0,0).toRotationMatrix();

    // Example 4 : use an Identity quaternion
    cart_pos_ref.linear() = Eigen::Quaterniond::Identity().toRotationMatrix();

    // Set the desired cartesian velocity and acceleration to zero
    Vector6d cart_vel_ref = Vector6d::Zero();
    Vector6d cart_acc_ref = Vector6d::Zero();

    // Now set the servoing PID
    Vector6d P;
    P << 1000, 1000, 1000, 10, 10, 10;
    cart_acc_pid->pid()->setProportionalGain(P);
    Vector6d D;
    D << 100, 100, 100, 1, 1, 1;
    cart_acc_pid->pid()->setDerivativeGain(D);

    cart_acc_pid->setControlFrame("link_7");
    // The desired values are set on the servo controller. Because cart_task->setDesired expects a cartesian acceleration. Which is computed automatically by the servo controller
    cart_acc_pid->setDesired(cart_pos_ref.matrix(),cart_vel_ref,cart_acc_ref);
    
    // Cartesian Task
    auto cart_task = controller.addTask<CartesianTask>("CartTask_EE");
    // Set the servo controller to the cartesian task
    cart_task->setServoController(cart_acc_pid);
    
    // Get the number of actuated joints
    const int ndof = robot_model->getNrOfDegreesOfFreedom();

    // Joint torque limit is usually given by the robot manufacturer
    auto jnt_trq_cstr = controller.addConstraint<JointTorqueLimitConstraint>("JointTorqueLimit");
    Eigen::VectorXd jntTrqMax(ndof);
    jntTrqMax.setConstant(200.0);
    jnt_trq_cstr->setLimits(-jntTrqMax,jntTrqMax);

    // Joint position limits are automatically extracted from the URDF model.
    // Note that you can set them if you want. by simply doing jnt_pos_cstr->setLimits(jntPosMin,jntPosMax).
    auto jnt_pos_cstr = controller.addConstraint<JointPositionLimitConstraint>("JointPositionLimit");

    // Joint velocity limits are usually given by the robot manufacturer
    auto jnt_vel_cstr = controller.addConstraint<JointVelocityLimitConstraint>("JointVelocityLimit");
    Eigen::VectorXd jntVelMax(ndof);
    jntVelMax.setConstant(2.0);
    jnt_vel_cstr->setLimits(-jntVelMax,jntVelMax);


    double dt = 0.5;
    double current_time = 0;

    controller.activateTasksAndConstraints();


    // If your robot's low level controller takes into account the gravity and coriolis torques already (Like with KUKA LWR) then you can tell the controller to remove these components from the torques computed by the solver. Setting them to false keeps the components in the solution (this is the default behavior).
    controller.removeGravityTorquesFromSolution(true);
    controller.removeCoriolisTorquesFromSolution(true);

    // Now you can run the control loop
    for (; current_time < 2.0; current_time +=dt)
    {
        // Here you can get the data from you REAL robot (API is robot-specific)
        // Something like :
            // eigState.jointPos = myRealRobot.getJointPositions();
            // eigState.jointVel = myRealRobot.getJointVelocities();

        // Now update the internal kinematic model with data from the REAL robot
        std::cout << "Setting robot state to : \n" 
            << "Joint Pos : " << eigState.jointPos.transpose() << '\n'
            << "Joint Vel : " << eigState.jointVel.transpose() << '\n';
            
        robot_model->setRobotState(eigState.jointPos,eigState.jointVel);

        // Step the controller + solve the internal optimal problem
        std::cout << "Updating controller..." ;
        controller.update(current_time, dt);
        std::cout << "OK" << '\n';
        
        // Do what you want with the solution
        if(controller.solutionFound())
        {
            // The whole optimal solution [AccFb, Acc, Tfb, T, eWrenches]
            const Eigen::VectorXd& full_solution = controller.getSolution();
            // The optimal joint torque command
            const Eigen::VectorXd& trq_cmd = controller.getJointTorqueCommand();
            // The optimal joint acceleration command
            const Eigen::VectorXd& trq_acc = controller.getJointAccelerationCommand();

            // Send torques to the REAL robot (API is robot-specific)
            //real_tobot->set_joint_torques(trq_cmd);
        }
        else
        {
            // WARNING : Optimal solution is NOT found
            // Switching to a fallback strategy
            // Typical are :
            // - Stop the robot (robot-specific method)
            // - Compute KKT Solution and send to the robot (dangerous)
            // - PID around the current position (dangerous)

            // trq = controller.computeKKTTorques();
            // Send torques to the REAL robot (API is robot-specific)
            // real_tobot->set_joint_torques(trq_cmd);
        }
    }

    // Print the last computed solution (just for fun)
    const Eigen::VectorXd& full_solution = controller.getSolution();
    const Eigen::VectorXd& trq_cmd = controller.getJointTorqueCommand();
    const Eigen::VectorXd& trq_acc = controller.getJointAccelerationCommand();
    std::cout << "Full solution : " << full_solution.transpose() << '\n';
    std::cout << "Joint Acceleration command : " << trq_acc.transpose() << '\n';
    std::cout << "Joint Torque command       : " << trq_cmd.transpose() << '\n';

    // At some point you want to close the controller nicely
    controller.deactivateTasksAndConstraints();


    // Let all the tasks ramp down to zero
    while(!controller.tasksAndConstraintsDeactivated())
    {
        current_time += dt;
        controller.update(current_time,dt);
    }

    // All objets will be destroyed here
    return 0;
}