Joint generalized force always returns "0"

[nicholaspalomo]

Hey!

I've been using ScenarI/O / Gazebo ScenarI/O from this project.

Something I've noticed is that the joint generalized force always seems to return "0". Here's the cartpole example, modified in order to reproduce the behavior:

example.cpp:

#include <scenario/gazebo/GazeboSimulator.h>
#include <scenario/gazebo/Joint.h>
#include <scenario/gazebo/Model.h>
#include <scenario/gazebo/World.h>

#include <chrono>
#include <string>
#include <thread>

int main(int argc, char* argv[])
{
    // Create the simulator
    auto gazebo = scenario::gazebo::GazeboSimulator(
        /*stepSize=*/0.001, /*rtf=*/1.0, /*stepsPerRun=*/1);

    // Initialize the simulator
    gazebo.initialize();

    // Get the default world
    auto world = gazebo.getWorld();

    // Insert the ground plane
    const std::string groundPlaneSDF = "ground_plane/ground_plane.sdf";
    world->insertModel(groundPlaneSDF);

    // Select the physics engine
    world->setPhysicsEngine(scenario::gazebo::PhysicsEngine::Dart);

    // Open the GUI
    gazebo.gui();
    std::this_thread::sleep_for(std::chrono::seconds(3));
    gazebo.run(/*paused=*/true);

    // Insert a pendulum
    const std::string pendulumURDF = "pendulum/pendulum.urdf";
    world->insertModel(/*modelFile=*/pendulumURDF);
    gazebo.run(/*paused=*/true);

    // Get the pendulum
    auto pendulum = world->getModel(/*modelName=*/"pendulum");

    // Reset the pole position
    auto pivot = pendulum->getJoint("pivot");
    auto pivotGazebo = std::static_pointer_cast<scenario::gazebo::Joint>(pivot);
    pivotGazebo->resetPosition(0.001);

    pendulum->setJointControlMode(scenario::core::JointControlMode::Force);

    // Simulate 30 seconds
    for (size_t i = 0; i < 30.0 / gazebo.stepSize(); ++i) {
        gazebo.run();

        pivot->setGeneralizedForceTarget(1.0);

        for(auto force : pivotGazebo->jointGeneralizedForce()) {
            std::cout << "[pivotGazebo] Joint generalized force: " << force << std::endl;
        }

        std::cout << "[pivot] Joint generalized force: " << pivot->generalizedForce() << std::endl;
    }

    // Close the simulator
    std::this_thread::sleep_for(std::chrono::seconds(3));
    gazebo.close();

    return 0;
}

For me, the values printed to the terminal are:

...
[pivotGazebo] Joint generalized force: 0
[pivot] Joint generalized force: 0
[pivotGazebo] Joint generalized force: 0
[pivot] Joint generalized force: 0
[pivotGazebo] Joint generalized force: 0
[pivot] Joint generalized force: 0
[pivotGazebo] Joint generalized force: 0
[pivot] Joint generalized force: 0
...

Am I missing something perhaps in calling the methods to get the joint generalized force?

I'm not sure if this is a bug or if I am incorrectly using the library, hence why I wanted to create a discussion here instead. Thanks!
@diegoferigo

[nicholaspalomo]

Additionally, I was just looking through some of the issues/PRs in this repo. Is the problem above related to #133 and #141 ?

If so, then it looks like this may give me what I need:

...
    pendulum->setJointControlMode(scenario::core::JointControlMode::Force);
    pivot->enableHistoryOfAppliedJointForces(true, 1);

    // Simulate 30 seconds
    for (size_t i = 0; i < 30.0 / gazebo.stepSize(); ++i) {
        gazebo.run();

        pivot->setGeneralizedForceTarget(1.0);

        for(auto force : pivotGazebo->jointGeneralizedForce()) {
            std::cout << "[jointGeneralizedForce] Joint generalized force: " << force << std::endl;
        }

        std::cout << "[generalizedForce] Joint generalized force: " << pivot->generalizedForce() << std::endl;

        for(auto force : pivot->historyOfAppliedJointForces()) {
            std::cout << "[historyOfAppliedJointForces] Joint generalized force: " << force << std::endl;
        }
    }
...

The output is now:

...
[jointGeneralizedForce] Joint generalized force: 0
[generalizedForce] Joint generalized force: 0
[historyOfAppliedJointForces] Joint generalized force: 1
[jointGeneralizedForce] Joint generalized force: 0
[generalizedForce] Joint generalized force: 0
[historyOfAppliedJointForces] Joint generalized force: 1
[jointGeneralizedForce] Joint generalized force: 0
[generalizedForce] Joint generalized force: 0
[historyOfAppliedJointForces] Joint generalized force: 1
...

[diegoferigo]

Hi @nicholaspalomo, the situation of joint torques is somewhat complicated.

• In upstream, there's no support of providing joint torques.
• The DART plugin does not extract the joint forces, and in the best case scenario it could provide the references applied before the physics step. Doing so, however, does not mean that what you extract is the real joint torque, due to e.g. friction dissipation (this means that the real joint torque is less than the reference).
• In ScenarIO, I tried to implement something more complete but it requires [dart] Compute Inverse Dynamics for all skeletons to populate joint forces gazebosim/gz-physics#143 that is still pending. I have a fork in diegoferigo/ign-physics@fix/compute_joint_force_edifice but this is still ongoing.
• In the first implementation Extract last applied joint forces #141, also Joint::generalizedForce (better, the corresponding method at that time) was returning the reference, but then, after the work of the fork, I changed the behavior to return the force computed by ID that required using my ign-physics fork. With the upstream version, it returns 0.
• As you found out, currently it is possible to extract the previously applied references using Joint:historyOfAppliedJointForces, this was a workaround I had to do for a previous research task I did because I needed to get the joint torques over a window. These are the torque references that do not take into account friction, though this is the closest values you can get today.

Hopefully the situation will be fixed soon, but as far as I understood in upstream there are few points to clarify before finalizing this feature.

[nicholaspalomo]

Hi, @diegoferigo,

Thanks for the explanation. That makes sense, yes. I think, for now, historyOfAppliedJointForces is good enough for my application. Thank you for working on this!