JointPositionController not working for nested models

[srmainwaring]

Environment

• OS Version: macOS Monterey
• Source or binary build? Source: gz-sim7, 31f7d5b

Description

• Expected behavior: The JointPositionController should control joints of nested models.
• Actual behavior: The nested joints do not respond to commands.

Steps to reproduce

test_nested_model.sdf.zip

1. Run the Gazebo using the example world:

gz sim -v4 -r test_nested_model.sdf

2. In a separate terminal issue commands to move the rotors:

# Move rotor in model1
gz topic -t "/rotor1_cmd" -m gz.msgs.Double -p "data: 1"

# Move rotor in model2 (nested)
gz topic -t "/rotor2_cmd" -m gz.msgs.Double -p "data: 1"

# Move rotor in model3
gz topic -t "/rotor3_cmd" -m gz.msgs.Double -p "data: 1"

Output

The single yellow block is model1, the teal block is model3 and the yellow block attached to the teal block is model2 which is nested in model3. The rotors in the top level models (model1 and model2) both move. The rotor in the nested model (model2) does not respond.

When stepping through the joint controller plugin the JointPositionController::PreUpdate returns at L268 because jointPosComp->Data() is always empty for the nested joints.

gz-sim/src/systems/joint_position_controller/JointPositionController.cc

Lines 267 to 268 in 31f7d5b

	if (jointPosComp == nullptr || jointPosComp->Data().empty())
	return;

Server log:

gz sim -v4 -s -r test_nested_model.sdf
[Msg] Gazebo Sim Server v7.1.0
[Msg] Loading SDF world file[/Users/rhys/Code/ardupilot/simulation/ardupilot_gazebo/worlds/test_nested_model.sdf].
[Msg] Serving entity system service on [/entity/system/add]
[Dbg] [Physics.cc:869] Loaded [gz::physics::dartsim::Plugin] from library [/Volumes/MacPro2_DV1/Code/osrf/gz_garden_ws/install/lib/gz-physics-6/engine-plugins/libgz-physics-dartsim-plugin.dylib]
[Dbg] [SystemManager.cc:71] Loaded system [gz::sim::systems::Physics] for entity [1]
[Msg] Create service on [/world/test_nested_model/create]
[Msg] Remove service on [/world/test_nested_model/remove]
[Msg] Pose service on [/world/test_nested_model/set_pose]
[Msg] Pose service on [/world/test_nested_model/set_pose_vector]
[Msg] Light configuration service on [/world/test_nested_model/light_config]
[Msg] Physics service on [/world/test_nested_model/set_physics]
[Msg] SphericalCoordinates service on [/world/test_nested_model/set_spherical_coordinates]
[Msg] Enable collision service on [/world/test_nested_model/enable_collision]
[Msg] Disable collision service on [/world/test_nested_model/disable_collision]
[Msg] Material service on [/world/test_nested_model/visual_config]
[Msg] Material service on [/world/test_nested_model/wheel_slip]
[Dbg] [SystemManager.cc:71] Loaded system [gz::sim::systems::UserCommands] for entity [1]
[Dbg] [SystemManager.cc:71] Loaded system [gz::sim::systems::SceneBroadcaster] for entity [1]
[Dbg] [JointPositionController.cc:205] [JointPositionController] system parameters:
[Dbg] [JointPositionController.cc:206] p_gain: [2]
[Dbg] [JointPositionController.cc:207] i_gain: [0.1]
[Dbg] [JointPositionController.cc:208] d_gain: [0.01]
[Dbg] [JointPositionController.cc:209] i_max: [1]
[Dbg] [JointPositionController.cc:210] i_min: [-1]
[Dbg] [JointPositionController.cc:211] cmd_max: [1000]
[Dbg] [JointPositionController.cc:212] cmd_min: [-1000]
[Dbg] [JointPositionController.cc:213] cmd_offset: [0]
[Dbg] [JointPositionController.cc:214] Topic: [rotor1_cmd]
[Dbg] [JointPositionController.cc:215] initial_position: [0]
[Dbg] [SystemManager.cc:71] Loaded system [gz::sim::systems::JointPositionController] for entity [8]
[Dbg] [JointPositionController.cc:205] [JointPositionController] system parameters:
[Dbg] [JointPositionController.cc:206] p_gain: [2]
[Dbg] [JointPositionController.cc:207] i_gain: [0.1]
[Dbg] [JointPositionController.cc:208] d_gain: [0.01]
[Dbg] [JointPositionController.cc:209] i_max: [1]
[Dbg] [JointPositionController.cc:210] i_min: [-1]
[Dbg] [JointPositionController.cc:211] cmd_max: [1000]
[Dbg] [JointPositionController.cc:212] cmd_min: [-1000]
[Dbg] [JointPositionController.cc:213] cmd_offset: [0]
[Dbg] [JointPositionController.cc:214] Topic: [rotor3_cmd]
[Dbg] [JointPositionController.cc:215] initial_position: [0]
[Dbg] [SystemManager.cc:71] Loaded system [gz::sim::systems::JointPositionController] for entity [16]
[Dbg] [JointPositionController.cc:205] [JointPositionController] system parameters:
[Dbg] [JointPositionController.cc:206] p_gain: [2]
[Dbg] [JointPositionController.cc:207] i_gain: [0.1]
[Dbg] [JointPositionController.cc:208] d_gain: [0.01]
[Dbg] [JointPositionController.cc:209] i_max: [1]
[Dbg] [JointPositionController.cc:210] i_min: [-1]
[Dbg] [JointPositionController.cc:211] cmd_max: [1000]
[Dbg] [JointPositionController.cc:212] cmd_min: [-1000]
[Dbg] [JointPositionController.cc:213] cmd_offset: [0]
[Dbg] [JointPositionController.cc:214] Topic: [rotor2_cmd]
[Dbg] [JointPositionController.cc:215] initial_position: [0]
[Dbg] [SystemManager.cc:71] Loaded system [gz::sim::systems::JointPositionController] for entity [25]
[Msg] Loaded level [3]
[Msg] Serving world controls on [/world/test_nested_model/control], [/world/test_nested_model/control/state] and [/world/test_nested_model/playback/control]
[Msg] Serving GUI information on [/world/test_nested_model/gui/info]
[Msg] World [test_nested_model] initialized with [1ms] physics profile.
[Msg] Serving world SDF generation service on [/world/test_nested_model/generate_world_sdf]
[Msg] Serving world names on [/gazebo/worlds]
[Msg] Resource path add service on [/gazebo/resource_paths/add].
[Msg] Resource path get service on [/gazebo/resource_paths/get].
[Msg] Resource path resolve service on [/gazebo/resource_paths/resolve].
[Msg] Resource paths published on [/gazebo/resource_paths].
[Msg] Server control service on [/server_control].
[Msg] Found no publishers on /stats, adding root stats topic
[Msg] Found no publishers on /clock, adding root clock topic
[Dbg] [SimulationRunner.cc:508] Creating PostUpdate worker threads: 2
[Dbg] [SimulationRunner.cc:519] Creating postupdate worker thread (0)
[Err] [SDFFeatures.cc:1077] Asked to create a joint between links [base2_link] as parent and [rotor2_link] as child, but the child link already has a parent joint of type [RevoluteJoint].
[Msg] Serving scene information on [/world/test_nested_model/scene/info]
[Msg] Serving graph information on [/world/test_nested_model/scene/graph]
[Msg] Serving full state on [/world/test_nested_model/state]
[Msg] Serving full state (async) on [/world/test_nested_model/state_async]
[Msg] Publishing scene information on [/world/test_nested_model/scene/info]
[Msg] Publishing entity deletions on [/world/test_nested_model/scene/deletion]
[Msg] Publishing state changes on [/world/test_nested_model/state]
[Msg] Publishing pose messages on [/world/test_nested_model/pose/info]
[Msg] Publishing dynamic pose messages on [/world/test_nested_model/dynamic_pose/info]
[Dbg] [EntityComponentManager.cc:1619] Updated state thread iterators: 32 threads processing around 2 entities each.

The error:

[Err] [SDFFeatures.cc:1077] Asked to create a joint between links [base2_link] as parent and [rotor2_link] as child, but the child link already has a parent joint of type [RevoluteJoint].

Suggests that the physics engine may be attempting to build the nested model twice?

Related

The same issue is observed when composing models using the <include> element.

[srmainwaring]

Investigations

Work in progress trying to tack down the origin of the issue. Notes below.

• The issue, as far as the gz-sim physics plugin is concerned, appears to be that the nested joints are not in the Physics plugin's entityJointMap.
• The cause of this occurs at a lower level in the dart physics engine, somewhere in gz::physics::dartsim::EntityManagementFeatures where the nested joint is not found in the call to GetJoint. Possibly because this does not recurse down to nested models or for some other reason?

Notes

1. gz::sim::systems:: PhysicsPrivate:: CreateModelEntities

There are two places where nested models are constructed. The first for models that are direct children of the world:

gz-sim/src/systems/physics/Physics.cc

Lines 1123 to 1124 in 31f7d5b

	auto modelPtrPhys =
	nestedModelFeature->ConstructNestedModel(*root.Model());

and the second for models that are not (and so must be nested):

gz-sim/src/systems/physics/Physics.cc

Line 1175 in 31f7d5b

auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model);

The first call will construct models recursively, so that when the second call is made the nested model is already constructed.

2. gz::sim::systems::PhysicsPrivate::CreateJointEntities

For the non-nested joints, the function CreateJointEntities hits the test if they already exist, and proceeds to insert them into the entityJointMap:

gz-sim/src/systems/physics/Physics.cc

Lines 1680 to 1689 in 31f7d5b

	if (const auto existingJoint =
	basicModelPtrPhys->GetJoint(_name->Data()))
	{
	// No need to create this joint because it was already created when
	// parsing the model.
	this->entityJointMap.AddEntity(_entity, existingJoint);
	this->topLevelModelMap.insert(
	std::make_pair(_entity, topLevelModel(_entity, _ecm)));
	return true;
	}

For the nested joints this test fails (which is odd because we see below there is an error message reporting the joint already exists). In any case the function continues until it hits:

gz-sim/src/systems/physics/Physics.cc

Lines 1723 to 1734 in 31f7d5b

	auto jointPtrPhys = modelPtrPhys->ConstructJoint(joint);
	if (jointPtrPhys.Valid())
	{
	// Some joints may not be supported, so only add them to the map if
	// the physics entity is valid
	this->entityJointMap.AddEntity(_entity, jointPtrPhys);
	this->topLevelModelMap.insert(std::make_pair(_entity,
	topLevelModel(_entity, _ecm)));
	}
	return true;
	});

where the test for a valid physics pointer to a joint fails and the the inserting of the joint into the entityJointMap is skipped. To see why the attempt to create the joint failed we need to look into ConstructSdfJoint.

3. gz::physics::dartsim::SDFFeatures::ConstructSdfJoint

The attempt to create the nested joint fails here:

gz-physics6/dartsim/src/SDFFeatures.cc#L1074-L1083

    auto childsParentJoint = _child->getParentJoint();
    if (childsParentJoint->getType() != "FreeJoint")
    {
      gzerr << "Asked to create a joint between links "
             << "[" << _parent->getName() << "] as parent and ["
             << _child->getName() << "] as child, but the child link already "
             << "has a parent joint of type [" << childsParentJoint->getType()
             << "].\n";
      return this->GenerateInvalidId();
    }

because the joint has already been associated with a link, but for some reason is not recorded in the dart physics plugin as an existing link when queried using the entity management feature GetJointFromModel. So that's where to look next.

Update

Added a trace into Base.hh to print the contents of the EntityStore for models and joints:

• https://github.com/srmainwaring/gz-physics/tree/srmainwaring/6/1844-joint-pos-nested-models

[Dbg] [SDFFeatures.cc:1164] SDFFeatures::ConstructSdfJoint: Before ConstructRevoluteJoint
ModelID:     2
LocalName:   model3
Name:        model3
NumJoints:   1
  Joint:  base3_link_FreeJoint
ModelID:     3
LocalName:   model2
Name:        model3::model2
NumJoints:   2
  Joint:  base2_link_FreeJoint
  Joint:  rotor2_joint
JointID      5
Name:        base2_link_FreeJoint
JointID      8
Name:        rotor2_joint
JointID      10
Name:        rotor2_joint
JointID      12
Name:        base3_link_FreeJoint

[Dbg] [SDFFeatures.cc:1174] SDFFeatures::ConstructSdfJoint: After ConstructRevoluteJoint
ModelID:     2
LocalName:   model3
Name:        model3
NumJoints:   3
  Joint:  base3_link_FreeJoint
  Joint:  Joint
  Joint:  rotor2_joint
ModelID:     3
LocalName:   model2
Name:        model3::model2
NumJoints:   0
JointID      5
Name:        Joint
JointID      8
Name:        rotor2_joint
JointID      10
Name:        rotor2_joint
JointID      12
Name:        base3_link_FreeJoint

It appears that joints from the nested model are reassigned to the containing model by the function:

    // SDFFeatures.cc L1169-1170
    joint = ConstructSingleAxisJoint<dart::dynamics::RevoluteJoint>(
          _modelInfo, _sdfJoint, _parent, _child, T_joint);

    // SDFFeatures.cc L215
    return _child->moveTo<JointType>(_parent, properties);

Is this behaviour correct for nested models?

There is also something odd about having repeated entries in the store for the rotor_joint.

[azeey]

Thanks for sleuthing @srmainwaring. I'm wondering if this is a regression from #1560 or something that existed all along.

[srmainwaring]

I'm wondering if this is a regression from #1560 or something that existed all along

I think it was around before then. I rolled back gz-sim to the commit before #1560 and the issue remained.

I'm slowly zeroing in on possible causes - I suspect it may either be a recursion thing - where the dart model only reports its immediate child joints, or there is a double entry of the nested joint into the Base::AddJoint and one of them is not connected up to anything. It's a little daunting navigating through the type lists but I'm getting there...

For some context: support for nested models is a huge plus for supporting models with configurable peripherals. I'm currently bringing a number of models controlled by ArduPilot up to date, and without nested position controller support adding and maintaining models with peripherals like gimbals gets complicated very fast. I'm hoping for a non-ABI breaking fix that can go into Garden.

[srmainwaring]

@azeey - the issue arises in the function ConstructSingleAxisJoint in SDFFeatures where the joints from the nested model are reassigned to the container model. In the notes above, I've including a link to a fork containing some debug code for Base.hh to print the contents of the EntityStore as these are difficult to inspect in a debugger directly.

It's not clear to me what the correct fix is - any change is going to have knock on effects elsewhere. Options seem to be:

1. Don't reassign joints to the parent entity for nested models.
2. Continue to reassign them and update the search functions to resolve joints for nested models as though they were not reassigned.

I don't think Option 2 is a good choice - it would be very confusing and almost impossible to maintain (tracing the model state is quite difficult as it is). That leaves option 1, but I'm not familiar with DART to know how the object model will cope with this type of nesting.

[srmainwaring]

I've started a fix for this. The branches will probably change as I tidy things up and remove debug code but currently the changes are here:

• https://github.com/srmainwaring/gz-physics/tree/srmainwaring/6/1844-joint-pos-nested-models
• https://github.com/srmainwaring/gz-sim/tree/srmainwaring/7/joint-pos-controller

The main changes are broken down as follows:

gz-physics

• Update EntityManagementFeatures::GetJoint to search recursively up to the top-level containing model for a named joint (or joint by index), as the joints are moved to the container body nodes as the skeleton is assembled. gz-physics: b9b3ffd

gz-sim

• Update the JointPositionController to search using both scoped and unscoped joint names. gz-sim: 61bca3d

Outstanding issues

• ECM Entity Id is not correctly mapped to Physics Id when the container and nested model contain joints with the same unscoped name. gz-physics: ff02824
• Joints between two nested models are not resolved correctly if the top level model only contains models and joints between them (i.e no links at top level). gz-physics: 43e96f7.

[srmainwaring]

@azeey this should still be open as it requires #1851 to be merged as well.