Set robot state with gravity compensation

Note

The source code for this example can be found in [orca_root]/examples/gazebo/04-set_robot_state_gravity_compensation.cc, or alternatively on github at: https://github.com/syroco/orca/blob/dev/examples/gazebo/04-set_robot_state_gravity_compensation.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
#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_kinematics = std::make_shared<RobotDynTree>();
    robot_kinematics->loadModelFromFile(urdf_url);
    robot_kinematics->print();

    // Set the gazebo model init pose
    // auto joint_names = robot_kinematics->getJointNames();
    // std::vector<double> init_joint_positions(robot_kinematics->getNrOfDegreesOfFreedom(),0);

    // gzrobot.setModelConfiguration(joint_names,init_joint_positions);
    // or like this
    // gzrobot.setModelConfiguration({"joint_2","joint_5"},{1.5,0.0});

    // Update the robot on at every iteration
    gzrobot.setCallback([&](uint32_t n_iter,double current_time,double dt)
    {
        robot_kinematics->setRobotState(gzrobot.getWorldToBaseTransform().matrix()
                            ,gzrobot.getJointPositions()
                            ,gzrobot.getBaseVelocity()
                            ,gzrobot.getJointVelocities()
                            ,gzrobot.getGravity()
                        );
        gzrobot.setJointGravityTorques(robot_kinematics->getJointGravityTorques());
    });

    // 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
    std::cout << "Simulation running... (GUI with \'gzclient\')" << "\n";
    gzserver.run();
    return 0;
}