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
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 | // 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>
#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 gz_server(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 gz_model = GazeboModel(gz_server.insertModelFromURDFFile(urdf_url));
// Create an ORCA robot
auto robot_model = std::make_shared<RobotModel>();
robot_model->loadModelFromFile(urdf_url);
robot_model->print();
// Update the robot on at every iteration
gz_model.executeAfterWorldUpdate([&](uint32_t n_iter,double current_time,double dt)
{
std::cout << "Gazebo iteration " << n_iter << " current time: " << current_time << " dt: " << dt << '\n';
robot_model->setRobotState(gz_model.getWorldToBaseTransform().matrix()
,gz_model.getJointPositions()
,gz_model.getBaseVelocity()
,gz_model.getJointVelocities()
,gz_model.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
gz_server.run();
return 0;
}
|