Set robot state¶
Note
The source code for this example can be found in [orca_root]/examples/gazebo/03-set_robot_state.cc
, or alternatively on github at: https://github.com/syroco/orca/blob/dev/examples/gazebo/03-set_robot_state.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 | #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** argv)
{
// Get the urdf file from the command line
if(argc < 2)
{
std::cerr << "Usage : " << argv[0] << " /path/to/robot-urdf.urdf" << "\n";
return -1;
}
std::string urdf_url(argv[1]);
// Instanciate the gazebo server with de dedfault empty world
GazeboServer gzserver(argc,argv);
// This is equivalent to GazeboServer gz("worlds/empty.world")
// Insert a model onto the server and create the GazeboModel from the return value
// You can also set the initial pose, and override the name in the URDF
auto gzrobot = GazeboModel(gzserver.insertModelFromURDFFile(urdf_url));
// Create an ORCA robot
auto robot = std::make_shared<RobotDynTree>();
robot->loadModelFromFile(urdf_url);
robot->print();
// Update the robot on at every iteration
gzrobot.setCallback([&](uint32_t n_iter,double current_time,double dt)
{
robot->setRobotState(gzrobot.getWorldToBaseTransform().matrix()
,gzrobot.getJointPositions()
,gzrobot.getBaseVelocity()
,gzrobot.getJointVelocities()
,gzrobot.getGravity()
);
});
// Run the main simulation loop.
// This is a blocking call that runs the simulation steps
// It can be stopped by CTRL+C
// You can optionally add a callback that happends after WorldUpdateEnd
gzserver.run();
return 0;
}
|