ScenarI/O: SCENe interfAces for Robot Input / Output - New Ignition Gazebo ECM APIs

[diegoferigo]

Specifications:

• Stateless: The classes have been designed to be (almost) stateless. Gazebo's ECM should be used to store data.
• stdlib-centric APIs: All classes' methods return types (or struct with fields with types) compatible with stdlib. This simplifies binding the APIs to other languages since stdlib already has bindings support for multiple languages.
• "Interfaceable" APIs: Right now the scene classes are concrete and implemented for Gazebo. Though, they have been designed to be abstracted and become interfaces with multiple C++ backends (e.g. Yarp, Ros, etc) to achieve transparency for real-time setting.

Features:

• Expose all the upstream components
• Introduce new components to augment supported features
• Load world and model plugins during runtime

Note that the classes have not yet been optimized for speed. There are copies of data that could be avoided. We leave these optimizations to a future PR (we'd need to profile the execution to understand if this could be a bottleneck).

Note also that tests will not pass. This PR and all the next ones will target the refactoring branch, and tests will run in the final PR targeting devel.

---

Other simulator have two execution modalities: deterministic and non-determistic [1]. In deterministic mode, the user has to execute all the simulation step individually, effectively controlling the simulation. Instead, in non-deterministic mode, the simulator runs in the background and the user has to start by stepping it once (i.e. the step function is non-blocking).

Ignition Gazebo in theory could support both. However, due to the ECM-based architecture, the non-deterministic modalities presents non-trivial synchronization caveats. In fact, all systems (plugins) in Gazebo run in a dedicated thread, and they have PreUpdate Update PostUpdate callbacks that are called by the simulator. A special case is the Physics system, since its Physics::Update method will step the physics engine. The main synchronization problem in non-deterministic modality emerges when our bindings have to set data into the ECM. In fact, in order to be read by the active systems correctly, this data has to be stored in the ECM between the PostUpdate of the last system and the first PreUpdate of the first System of the new simulation step. Enforcing this logic is not currently possible. Therefore, the non-deterministic modality could not be robust [2].

[1] One example of non-deterministic mode is PyBullet's configured with setRealTimeSimulation enabled.
[2] I managed to get it running, but I experienced both data corruption and data storing in the wrong phase and not being applied by the Physics system. I tried to implement a synchronization logic with condition variables to wait for the Physics::PreUpdate stage. But what if other systems whose PreUpdate is executed before the Physics's one need that data (e.g. a controller plugin that has to read position references and store a force command)? The only solution I see now is implementing a callbacks queue populated by our bindings associated to a System which PreUpdate method is executed as first one. Right now is not possible, even though I don't exclude it could be doable in the future (I remember some discussion about treating Systems with a logic similar to CSS' z-index).

[diegoferigo]

The PR is ready to be reviewed. I still have few minor methods to implement, but the majority of the classes are ready. This is the first preliminary round only regarding the APIs. Then, in the PR that will merge this refactoring branch into devel we can discuss about the usage of these APIs and improve the user experience, if needed.

[traversaro]

Some comments. The main theme is that while it does not make a lot of sense to document 100% of the methods, especially the obvious ones, I think add a little piece of docs would drastically improve the usability of the interface and prevent future bugs and errors.

[traversaro]

I suggest to document this method. First of all, I understand the idea of using "force" to be joint type-independent, but this will be clearly confusing for most roboticist used to the concept of "joint torques". Documenting this will avoid confusion. An additional common source of confusion is the difference the torque that the simulation engine takes in input, from which the friction torques are removed to obtain the actual torque delivered to the multibody structure, and the torque tipically used in whole-body control, that instead just refer to the actual torque delivered to the multibody structure.

In formulas, for a fixed manipulator in simulation the "torque" is typically referred to (the plus or minus on $ \tau_{friction}$ depends on how you define $ \tau_{friction}$ , but it is not affecting the general idea):
$$
\tau_{sim} = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + J^T f_{ext} - \tau_{friction}(q, \dot{q})
$$
while in whole-body controllers with "torque" we typically refer to:
$$
\tau_{control} = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + J^T f_{ext}
$$
I think we should be clear on which force we are referring to, see https://bitbucket.org/osrf/gazebo/issues/1321/proper-joint-torque-feedback for a related issue.

cc @DanielePucci

[diegoferigo]

Thanks a lot for this comment! The past is coming back as far as I see 😆 That issue in enlightening. So far, I never considered friction in the design of the APIs. Now I am realizing that I should have.

An additional common source of confusion is the difference the torque that the simulation engine takes in input, from which the friction torques are removed to obtain the actual torque delivered to the multibody structure, and the torque tipically used in whole-body control, that instead just refer to the actual torque delivered to the multibody structure.

Right now Joint::force() is returning the force that gazebo has sent to the physics engine in the previous step (i.e. the JointForceCmd that could be filled by PID or custom controllers). The logic was introduced in #141. After the discussion of robotology/gazebo-yarp-plugins#471 we are aligned to think that it could be confusing.

I just checked and in the Physics::PostUpdate method we could ask the joint force using physics::Joint::getForce to the physics engine. Unfortunately it will return the value previously set with physics::Joint::setForce, so the value will also contain the fraction lost in friction. Due to this lack, I think it's better to remove Joint::force and Joint::setForce. I think that this joint force data should be provided by something similar to the sensors of SEAs or similar.

I propose the following APIs:

// Get the motor force references (content of existing `JointForceCmd` component).
double forceTarget(const size_t dof = 0) const;
std::vector<double> jointForceTarget() const;

// Set the joint force references (filling the `JointForceCmd` component).
// Controllers will use this method to set their output.
// TODO: should this force be additive?
bool setForceTarget(const double force, const size_t dof = 0);
bool setJointForceTarget(const std::vector<double>& force);

// Joint torques should be implemented through sensor plugins that
// provide a measurement in the joint side. If we want to avoid having too many
// threads (one per joint) we could also consider to move the logic in this
// Joint class or in the Physics system.
// bool force() const
// std::vector<double> jointForce() const;

// Return the reduction ratio (default = 1.0) that can be filled by a motor plugin
double gearReductionRatio() const;

// Setting the max force should happen in the motor side. Since users can get the
// gear ratio, I prefer having the following rather than having e.g. `maxForce()`.
// PIDs and custom controllers, that are operating in the joint side, will use  
// these limits (scaled by the gear ratio) to clip their output.
double maxMotorForce(const size_t dof = 0) const;
std::vector<double> jointMaxMotorForce() const;
bool setMaxMotorForce(const double maxForce);
bool setJointMaxMotorForce(const std::vector<double>& maxForce);

// TODO: not implemented now. If we need them in the future we can extend
//       the Joint APIs.
// double motorForceTarget(const size_t dof = 0) const;
// std::vector<double> jointMotorForceTarget() const;
// bool setMotorForceTarget(const double force, const size_t dof = 0);
// bool setJointMotorForceTarget(const std::vector<double>& force);

// These methods are meant to simplify getting the values to be used to
// calculate the usual regularization term that minimizes joint torques.
// They will return the content of `JointForceCmd` over a window.
bool historyOfAppliedJointForcesEnabled() const;
bool enableHistoryOfAppliedJointForces(const bool enable,
                                       const size_t maxHistorySize = 100);
std::vector<double> getHistoryOfAppliedJointForces() const;

What do you think @traversaro?

I tried to take into account that multiple Systems could operate on a joint in a single simulation step. The most complex scenario is the following:

• The joint is controlled in JointControlMode::Force using a custom controller (let's assume it is a whole-body controller implemented as a model plugin System) and has a plugin that simulates the second order dynamics (it could be either model-based with motor inertia, gearbox ratio and delay, or an actuatornet).
  1. The user sets joint references through the Model APIs (position, velocity, and acceleration). The Joint{Position,Velocity,Acceleration}Target components are filled.
  2. In the PreUpdate step, the custom controller System processes the targets and calls Joint::setJointForce, that fills the JointForceCmd component.
  3. In the PreUpdate step [1] the second order dynamics System reads the JointForceCmd component and changes it, e.g. introducing motor inertia effects and adding a delay.
  4. In the Update step the physics System reads the JointForceCmd and sets this value in the physics engine.

[1] Assuming we can fix the order of systems execution, that now is related to the loading order. I have to double check how this is processed.

Note: I just realized something very cool. Considering that: 1) we can decide the rate of Systems by setting the simulator update rate, 2) we control the physics system since it's vendored, if 3) we allow the physics system to run slower than the simulator (skipping few iterations), we can have the system that processes the second order dynamics running faster than the physics. @DanielePucci I remember that this was one of the limitations we had when we tried to setup the previous co-simulation architecture for motor's second-order dynamics.

Edit: probably also a second thread spawned from the system would work similarly. Performing extra useless steps to all the other systems, depending how the systems were implemented, could result in undefined behaviour.

[traversaro]

If we want to avoid having too many threads (one per joint)

Do you have any docs/code/issue pointer on the fact that a thread is created for each system? This is a critical performance bottleneck.

// Return the reduction ratio (default = 1.0) that can be filled by a motor plugin
double gearReductionRatio() const;

I am a bit confused by this. Is this something that exists in Ignition Gazebo? If instead is a parameter of a possible future motor/transmission model, then I have two observations:

• The motor plugins will have a lot of parameters, why expose only one in the main API?
• How can a parameter in the motor plugin influence of the value returned by Ignition Gazebo-related API?

we can have the system that processes the second order dynamics running faster than the physics. @DanielePucci I remember that this was one of the limitations we had when we tried to setup the previous co-simulation architecture for motor's second-order dynamics.

Not sure what you mean here. If you run co-simulation using the Gazebo physics as "master", you can easily run a second order "faster" (I guess this means with "shorter" timestep) by integrating it multiple times during the physics engine update timestep. For example, if hte main physics has a timestep of 1 ms, and you want to integrate the second order with a fixed integrator with a timestep of 0.1 ms, you just need to step the integrator of the second order dynamics 10 times in the callback of the main physics thread.

[traversaro]

we control the physics system since it's vendored

I totally understand the convenience of using the vendored physics system, but I think for second order dynamics ideally/if possible we should use a solution that works also with the upstream physics system, so that we can then re-use easily with systems simulated always with Ignition Gazebo but that interface with controllers in other ways, for example using gazebo-yarp-plugins.

[traversaro]

// Joint torques should be implemented through sensor plugins that
// provide a measurement in the joint side. If we want to avoid having too many
// threads (one per joint) we could also consider to move the logic in this
// Joint class or in the Physics system.
// bool force() const
// std::vector jointForce() const;

Why joint torques should be implemented via sensor plugins? I suspect that the solution suggested in https://bitbucket.org/osrf/gazebo/issues/1321/proper-joint-torque-feedback (i.e. get the wrench exchanged between two bodies and project it on the joint axis) should work without the need of custom plugins.

[diegoferigo]

Sorry, it is not really clear to me how the gearReductionRatio and the maxMotorForce would be interacting.

I had in mind:

$$ \tau_{joint}^{Max} = T * \tau_{motor}^{Max} $$

[DanielePucci]

Hi guys, I am not understanding why the terminology "force" should be independent from the joint-type, since if you tell me "force", I associate Newtons to it, so prismatic joint not revolute. Probably you can help me with that

Concerning second order dynamics, as I was mentioning today, I would like to converge towards a model exchange based simulation infrastructure in the future

CC
@diegoferigo @paolo-viceconte @CarlottaSartore

[traversaro]

Sorry, it is not really clear to me how the gearReductionRatio and the maxMotorForce would be interacting.

I had in mind:

$$ \tau_{joint}^{Max} = T * \tau_{motor}^{Max} $$

But in that case the Ignition Gazebo API will return for "\tau_{motor}" a completly different quantity then Gym Ignition. I think that this kind of "hidden" logic do not aid in clarity.

[diegoferigo]

Hi guys, I am not understanding why the terminology "force" should be independent from the joint-type, since if you tell me "force", I associate Newtons to it, so prismatic joint not revolute. Probably you can help me with that

In this domain we're talking of generalized forces. We can add the extra Generalized prefix to those methods if this makes them more clear, with the cost of APIs more verbose. Another terminology that could be used is Effort instead of Force, but it seems to me to be way less common.

But in that case the Ignition Gazebo API will return for "\tau_{motor}" a completly different quantity then Gym Ignition. I think that this kind of "hidden" logic do not aid in clarity.

Ok let's ignore this gear ratio than. My main point was that typically you know the maximum torque that a it could exerted at the motor level, not the joint level. Removing the Motor methods, if users have a motor plugin and want to set its maximum torque, they need to can only set the joint maximum torque calculated from the user code using the gear ratio (gathered somehow from the custom motor plugin, or hardcoded).

Here below the updated proposal:

// Get the motor force references (content of existing `JointForceCmd` component).
double generalizedForceTarget(const size_t dof = 0) const;
std::vector<double> jointGeneralizedForceTarget() const;

// Set the joint force references (filling the `JointForceCmd` component).
// Controllers will use this method to set their output.
// TODO: should this force be additive?
bool setGeneralizedForceTarget(const double force, const size_t dof = 0);
bool setJointGeneralizedForceTarget(const std::vector<double>& force);

// Joint torques should be implemented through sensor plugins that
// provide a measurement in the joint side. If we want to avoid having too many
// threads (one per joint) we could also consider to move the logic in this
// Joint class or in the Physics system.
// bool generalizedForce() const
// std::vector<double> jointGeneralizedForce() const;

double maxGeneralizedForce(const size_t dof = 0) const;
std::vector<double> jointMaxGeneralizedForce() const;
bool setMaxGeneralizedForce(const double maxForce);
bool setJointMaxGeneralizedForce(const std::vector<double>& maxForce);

// These methods are meant to simplify getting the values to be used to
// calculate the usual regularization term that minimizes joint torques.
// They will return the content of `JointForceCmd` over a window.
bool historyOfAppliedJointGeneralizedForcesEnabled() const;
bool enableHistoryOfAppliedJointGeneralizedForces(
    const bool enable, const size_t maxHistorySize = 100);
std::vector<double> getHistoryOfAppliedJointGeneralizedForces() const;

[diegoferigo]

Updated APIs in 7709620

[diegoferigo]

Related to gazebosim/gz-physics#3