Get force references after stepping Gazebo

[diegoferigo]

Right now Ignition Gazebo does not have the support of exposing the generalized force that was applied during the current physics step.

In fact, the component containing the force reference (JointForceCmd) is zeroed in the last phase of the step of the Physics system:

gym-ignition/plugins/Physics/Physics.cpp

Lines 1042 to 1047 in 86a77cc

	// Clear pending commands
	_ecm.Each<components::JointForceCmd>(
	[&](const Entity&, components::JointForceCmd* _force) -> bool {
	std::fill(_force->Data().begin(), _force->Data().end(), 0.0);
	return true;
	});

A possible solution would be creating a new component, that could be JointForce, that is zeroes before the physics step and is filled right after using the data of JointForceCmd.

This feature is extremely useful for instance to get the force applied by joint controllers implemented as plugins. In fact, in this setting, it's not the user code that sends the force reference to the simulator. Instead, the force is computed in C++ and directly sent to the simulator, and there's no other way to extract it without the introducing a new component and passing through the ECM.

@traversaro comments?

[traversaro]

Just to understand, why it is necessary for upstream to zero the JointForceCmd component? It is because it is use in an additive fashion, i.e. the system that use it add their desired value instead of just writing it? And so this is the only component for which it is necessary to do this as the others are not set to zero? In the long term I think that having the JontForceCmd that behaves like other components make more sense, but as I guess it would not be easy for upstream to change, you solution sounds fine.

[diegoferigo]

Just to understand, why it is necessary for upstream to zero the JointForceCmd component?

The JointForceCmd is zeroed after the physics step. I think that they do not remove it because a zero applied force should not have any effects in the next step if no other force reference is passed from above. But I'm just guessing. The component could be removed, and the current logic might also have been implemented as it is for performance reasons.

It is because it is use in an additive fashion, i.e. the system that use it add their desired value instead of just writing it?

I have to double check this, but I think that if multiple systems want to add a force to the same joint, it's the system that has the responsibility to read the current cmd and add its own value instead of overriding it. I understand that sometimes this is not desiderable, because if you're using a third-party system that does not follow this logic, the cmd of systems that are processed before are lost.

As a reference, here below you can find the code that applies the force before the physics step:

gym-ignition/plugins/Physics/Physics.cpp

Lines 636 to 649 in 86a77cc

	if (force) {
	if (force->Data().size() != jointIt->second->GetDegreesOfFreedom()) {
	ignwarn << "There is a mismatch in the degrees of freedom between "
	<< "Joint [" << _name->Data() << "(Entity=" << _entity
	<< ")] and its JointForceCmd component. The joint has "
	<< force->Data().size() << " while the component has "
	<< jointIt->second->GetDegreesOfFreedom() << ".\n";
	}
	std::size_t nDofs =
	std::min(force->Data().size(), jointIt->second->GetDegreesOfFreedom());
	for (std::size_t i = 0; i < nDofs; ++i) {
	jointIt->second->SetForce(i, force->Data()[i]);
	}
	}

[traversaro]

Just to understand, why it is necessary for upstream to zero the JointForceCmd component?

The JointForceCmd is zeroed after the physics step. I think that they do not remove it because a zero applied force should not have any effects in the next step if no other force reference is passed from above. But I'm just guessing. The component could be removed, and the current logic might also have been implemented as it is for performance reasons.

Ack, this may be non-intuitive because it force any system to run at the same frequency of the physics update step, but I guess this is not the right place to discuss this.

[diegoferigo]

Ack, this may be non-intuitive because it force any system to run at the same frequency of the physics update step, but I guess this is not the right place to discuss this.

All systems run by design at the same rate. However, systems can read the simulated time and skip updates. We already do something similar for our PID controllers, since users can specify an update rate that differs from the physics rate.

[diegoferigo]

Closed via #141