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 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 .
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 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;
}
|