Using Gazebo to simulate an ORCA controller¶
Note
The source code for this example can be found in [orca_root]/examples/gazebo/05-orca_gazebo.cc
, or alternatively on github at: https://github.com/syroco/orca/blob/dev/examples/gazebo/05-orca_gazebo.cc
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 | #include <orca/orca.h>
#include <orca/gazebo/GazeboServer.h>
#include <orca/gazebo/GazeboModel.h>
using namespace orca::all;
using namespace orca::gazebo;
int main(int argc, char const *argv[])
{
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);
auto robot = std::make_shared<RobotDynTree>();
robot->loadModelFromFile(urdf_url);
robot->setBaseFrame("base_link");
robot->setGravity(Eigen::Vector3d(0,0,-9.81));
RobotState eigState;
eigState.resize(robot->getNrOfDegreesOfFreedom());
eigState.jointPos.setZero();
eigState.jointVel.setZero();
robot->setRobotState(eigState.jointPos,eigState.jointVel);
orca::optim::Controller controller(
"controller"
,robot
,orca::optim::ResolutionStrategy::OneLevelWeighted
,QPSolver::qpOASES
);
auto cart_task = std::make_shared<CartesianTask>("CartTask-EE");
controller.addTask(cart_task);
cart_task->setControlFrame("link_7"); //
Eigen::Affine3d cart_pos_ref;
cart_pos_ref.translation() = Eigen::Vector3d(0.5,-0.5,0.8); // x,y,z in meters
cart_pos_ref.linear() = Eigen::Quaterniond::Identity().toRotationMatrix();
Vector6d cart_vel_ref = Vector6d::Zero();
Vector6d cart_acc_ref = Vector6d::Zero();
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);
cart_task->servoController()->setDesired(cart_pos_ref.matrix(),cart_vel_ref,cart_acc_ref);
const int ndof = robot->getNrOfDegreesOfFreedom();
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);
auto jnt_pos_cstr = std::make_shared<JointPositionLimitConstraint>("JointPositionLimit");
controller.addConstraint(jnt_pos_cstr);
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);
GazeboServer gzserver(argc,argv);
auto gzrobot = GazeboModel(gzserver.insertModelFromURDFFile(urdf_url));
///////////////////////////////////////
///////////////////////////////////////
///////////////////////////////////////
///////////////////////////////////////
bool cart_task_activated = false;
cart_task->onActivatedCallback([&cart_task_activated](){
std::cout << "CartesianTask activated. Removing gravity compensation and begining motion." << '\n';
cart_task_activated = true;
});
gzrobot.setCallback([&](uint32_t n_iter,double current_time,double dt)
{
robot->setRobotState(gzrobot.getWorldToBaseTransform().matrix()
,gzrobot.getJointPositions()
,gzrobot.getBaseVelocity()
,gzrobot.getJointVelocities()
,gzrobot.getGravity()
);
// All tasks need the robot to be initialized during the activation phase
if(n_iter == 1)
controller.activateTasksAndConstraints();
controller.update(current_time, dt);
if (cart_task_activated)
{
if(controller.solutionFound())
{
gzrobot.setJointTorqueCommand( controller.getJointTorqueCommand() );
}
else
{
gzrobot.setBrakes(true);
}
}
else
{
gzrobot.setJointGravityTorques(robot->getJointGravityTorques());
}
});
std::cout << "Simulation running... (GUI with \'gzclient\')" << "\n";
gzserver.run();
return 0;
}
|