Skip to content

Conversation

srmainwaring
Copy link
Contributor

@srmainwaring srmainwaring commented Jun 29, 2025

🎉 New feature

Closes #634

Summary

Allow joint properties to be applied to ball joints in the dart sim physics engine.

As suggested in #634, the properties from the first <axis> field in the ball joint are applied to all DoF if this element is set. Otherwise the defaults are used, which is current behaviour.

Test it

  • Add an <axis> element to a model using a ball joint and set some of the properties (eg. <limit>).
  • Prior to the change the property would have no effect, after the change the joint range should be restricted.

Example: wind.sdf

  • Modify the wind.sdf model to include <axis> elements for each sphere restricting the movement:
     <joint name="sphere_1_joint" type="ball">
        <parent>roof</parent>
        <child>sphere_1</child>
        <axis>
          <limit>
            <lower>0</lower>
            <upper>0</upper>
          </limit>
          <dynamics>
            <damping>0.01</damping>
          </dynamics>
        </axis>
      </joint>

Figure: Expected wind.sdf behaviour with ball joint limit. Wind sock does not move.
ball_joint_with_limit

The motivation for this change is to stabilise a complex closed kinematic chain mechanism employing multiple ball joints (a helicopter rotor head). Without this change the simulation is not stable, and the joints vibrated so badly the simulated model does not maintain integrity (companion change to adjust the dynamic joint erp and cfm properties is also required to get satisfactory behaviour. That is be the subject of the follow up PRs:

Figure: Application of ball joint properties to a helicopter rotor head demonstrating swash plate collective and cyclic operation.

heli_flapping_blades_1

  • Add test

Checklist

  • Signed all commits for DCO
  • Added tests
  • Added example and/or tutorial
  • Updated documentation (as needed)
  • Updated migration guide (as needed)
  • Consider updating Python bindings (if the library has them)
  • codecheck passed (See contributing)
  • All tests passed (See test coverage)
  • While waiting for a review on your PR, please help review another open pull request to support the maintainers
  • Was GenAI used to generate this PR? If so, make sure to add "Generated-by" to your commits. (See this policy for more info.)

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by and Generated-by messages.

@github-actions github-actions bot added the 🏛️ ionic Gazebo Ionic label Jun 29, 2025
srmainwaring added a commit to srmainwaring/SITL_Models that referenced this pull request Jun 29, 2025
- Use gazebosim/gz-physics#753

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
ahcorde
ahcorde previously requested changes Jul 1, 2025
@github-project-automation github-project-automation bot moved this from Inbox to In review in Core development Jul 1, 2025
@srmainwaring srmainwaring requested a review from ahcorde July 2, 2025 10:02
Copy link

codecov bot commented Jul 2, 2025

Codecov Report

Attention: Patch coverage is 0% with 9 lines in your changes missing coverage. Please review.

Please upload report for BASE (gz-physics8@db1d5e0). Learn more about missing BASE report.

Files with missing lines Patch % Lines
dartsim/src/SDFFeatures.cc 0.00% 9 Missing ⚠️
Additional details and impacted files
@@              Coverage Diff               @@
##             gz-physics8     #753   +/-   ##
==============================================
  Coverage               ?   79.19%           
==============================================
  Files                  ?      144           
  Lines                  ?     8801           
  Branches               ?        0           
==============================================
  Hits                   ?     6970           
  Misses                 ?     1831           
  Partials               ?        0           

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

srmainwaring added a commit to srmainwaring/SITL_Models that referenced this pull request Jul 3, 2025
RotorHead
- Adjust pose of model in world
- Add further links and joints.
- Add lever coupling links and joints.
- Simplify collisions.
- Compose ball joint from 2 revolute joints.
- Simplify collisions for linkages.
- Ensure all ball joints have 3DoF
- Add servo linkage visuals.
- Limit movement of lever joints.
- Lever coupling top joints are revolute, not ball.
- Add guide rod and retainer to prevent lower swashplate rotation.
- Update friction and damping.
- Recompute ball joint inertials.
- Add servos and rods.
- Update joint damping.
- Update link and joint names.
- Align swashplate joints.
- Replace collision guide with joints.
- Add rotor blades.
- Add lift-drag plugins.
- Correct joint orientations.
- Reorient servo links.
- Reduce mass of rotor head.
- Adjust rotor lift-drag.
- Rename rotor head model
- Add ccw version of rotor head.
- Update orientation in ccw rotor head.
- Velocity controllers were not using force and PIDs
- Update PIDs for joint and position controllers.
- Fit lift-drag coefficients to NACA0012 profile.
- Add blade visuals for CW and CCW rotors.
- Move servo2 to be near centre of rotor head.
- Remove debug info.
- Remove debug colours of servo rods and joints.
- Add credit for visuals

Helicopter
- Add helicopter model in world.
- Add tail rotor.
- Add stabiliser bars and wheels.
- Add ArduPilot plugin.
- Swap servo_1 and servo_3 to be consistent with AP ordering.
- Adjust CoG.
- Adjust IMU position to main rotor.
- Add separate model and config files.
- Use standalone model for helicopter.
- Place on runway model.
- Rename helicopter world
- Update to SDF version 1.11
- Add intermeshing dual rotor example.
- Add meshes.
- Increase friction on stabilizers.
- Correct orientation of tail rotor blade visual and collision.
- Correct dual heli control elements.
- Increase mass of base link for dual heli.
- Add params for dual transverse helicopter.
- Add gimbal for testing attitude control.
- Allow blades to flap on rotor heads
- This is essential for correct operation.
- Blades hinge at blade grips with range +/- 8 deg.
- Adjust blade flapping damping and spring constant.
- Remove oscillation / resonance when rolling at higher collective.
- Reduce blade hinge spring stiffness.
- Set ball joint properties
- Use gazebosim/gz-physics#753
- Set physics params in detachable joints
- Use gazebosim/gz-physics#754
- Use gazebosim/gz-sim#2960
- Adjust flapping hinge position
- Move hinge towards rotor hub.
- This position gives a flapping lag of about 80 deg behind the blade pitch.
- Retune ATC_RAT PIDs for flapping blades.
- Add visuals for frame and rotors
- Add mesh visuals based on TRex-450.
- Remove stabilising links and wheels.
- Reposition tail rotor to match rescaled model.
- Update rotor velocity command topic.
- Add config for the ros_gz bridge
- Retune model for adjusted CoM and tail position
- Add credit for visuals

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
srmainwaring added a commit to srmainwaring/SITL_Models that referenced this pull request Jul 3, 2025
RotorHead
- Adjust pose of model in world
- Add further links and joints.
- Add lever coupling links and joints.
- Simplify collisions.
- Compose ball joint from 2 revolute joints.
- Simplify collisions for linkages.
- Ensure all ball joints have 3DoF
- Add servo linkage visuals.
- Limit movement of lever joints.
- Lever coupling top joints are revolute, not ball.
- Add guide rod and retainer to prevent lower swashplate rotation.
- Update friction and damping.
- Recompute ball joint inertials.
- Add servos and rods.
- Update joint damping.
- Update link and joint names.
- Align swashplate joints.
- Replace collision guide with joints.
- Add rotor blades.
- Add lift-drag plugins.
- Correct joint orientations.
- Reorient servo links.
- Reduce mass of rotor head.
- Adjust rotor lift-drag.
- Rename rotor head model
- Add ccw version of rotor head.
- Update orientation in ccw rotor head.
- Velocity controllers were not using force and PIDs
- Update PIDs for joint and position controllers.
- Fit lift-drag coefficients to NACA0012 profile.
- Add blade visuals for CW and CCW rotors.
- Move servo2 to be near centre of rotor head.
- Remove debug info.
- Remove debug colours of servo rods and joints.
- Add credit for visuals

Helicopter
- Add helicopter model in world.
- Add tail rotor.
- Add stabiliser bars and wheels.
- Add ArduPilot plugin.
- Swap servo_1 and servo_3 to be consistent with AP ordering.
- Adjust CoG.
- Adjust IMU position to main rotor.
- Add separate model and config files.
- Use standalone model for helicopter.
- Place on runway model.
- Rename helicopter world
- Update to SDF version 1.11
- Add intermeshing dual rotor example.
- Add meshes.
- Increase friction on stabilizers.
- Correct orientation of tail rotor blade visual and collision.
- Correct dual heli control elements.
- Increase mass of base link for dual heli.
- Add params for dual transverse helicopter.
- Add gimbal for testing attitude control.
- Allow blades to flap on rotor heads
- This is essential for correct operation.
- Blades hinge at blade grips with range +/- 8 deg.
- Adjust blade flapping damping and spring constant.
- Remove oscillation / resonance when rolling at higher collective.
- Reduce blade hinge spring stiffness.
- Set ball joint properties
- Use gazebosim/gz-physics#753
- Set physics params in detachable joints
- Use gazebosim/gz-physics#754
- Use gazebosim/gz-sim#2960
- Adjust flapping hinge position
- Move hinge towards rotor hub.
- This position gives a flapping lag of about 80 deg behind the blade pitch.
- Retune ATC_RAT PIDs for flapping blades.
- Add visuals for frame and rotors
- Add mesh visuals based on TRex-450.
- Remove stabilising links and wheels.
- Reposition tail rotor to match rescaled model.
- Update rotor velocity command topic.
- Add config for the ros_gz bridge
- Retune model for adjusted CoM and tail position
- Add credit for visuals

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
srmainwaring added a commit to srmainwaring/SITL_Models that referenced this pull request Jul 3, 2025
- Use gazebosim/gz-physics#753

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
srmainwaring added a commit to srmainwaring/SITL_Models that referenced this pull request Jul 3, 2025
- Use gazebosim/gz-physics#753

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Copy link
Contributor

@iche033 iche033 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I added expanded one of the dartsim tests to include loading a simple ball joint model in 0cb06bf. Please take a look and integrate the changes if they look good to you. Feel free to expand the test.

Comment on lines 252 to 256
const ModelInfo &_modelInfo,
const ::sdf::Joint &_sdfJoint,
dart::dynamics::BodyNode * const _parent,
dart::dynamics::BodyNode * const _child,
const Eigen::Isometry3d &_T_joint)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

a couple of unused param warning here:

Suggested change
const ModelInfo &_modelInfo,
const ::sdf::Joint &_sdfJoint,
dart::dynamics::BodyNode * const _parent,
dart::dynamics::BodyNode * const _child,
const Eigen::Isometry3d &_T_joint)
const ModelInfo &,
const ::sdf::Joint &_sdfJoint,
dart::dynamics::BodyNode * const _parent,
dart::dynamics::BodyNode * const _child,
const Eigen::Isometry3d &)

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Updated in fa15dff

@srmainwaring
Copy link
Contributor Author

srmainwaring commented Jul 11, 2025

I added expanded one of the dartsim tests to include loading a simple ball joint model in 0cb06bf. Please take a look and integrate the changes if they look good to you. Feel free to expand the test.

Thanks! LGTM, added in b6beef1

Hmmm... on second thoughts seem to be some issues with the test? Having linker issues with testing on macOS - so hard to run locally. Will look into this on VM, but looks like sdformat does not like the element for ball joints?

@iche033
Copy link
Contributor

iche033 commented Jul 12, 2025

oh weird. I just pulled in the latest changes and ran the tests locally but everything passed. Need to investigate

@srmainwaring
Copy link
Contributor Author

oh weird. I just pulled in the latest changes and ran the tests locally but everything passed. Need to investigate

I've managed to get the tests running on macOS again (removed a stray conflicting version of gtest in /usr/local) - and all fine locally - so not sure what's up with CI?

srmainwaring and others added 4 commits July 15, 2025 19:54
- Allow dynamics properties to be applied to ball joints in dart sim.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Signed-off-by: Ian Chen <ichen@openrobotics.org>
@srmainwaring srmainwaring force-pushed the prs/8/pr-ball-joint-properties branch from b6beef1 to e9c0a6a Compare July 15, 2025 18:54
@iche033
Copy link
Contributor

iche033 commented Jul 15, 2025

As a quick test, I commented out the <axis> tag in my branch (iche033/pr-ball-joint-properties), and Ubuntu github action passed. For some reason, an extra entity gets added if <axis> exists. Still debugging

@iche033 iche033 mentioned this pull request Jul 17, 2025
10 tasks
Signed-off-by: Ian Chen <ichen@openrobotics.org>
@iche033
Copy link
Contributor

iche033 commented Jul 18, 2025

ok finally had more time to look into this. I pushed a change to make CIs green. It was not related to the ball joint axis. I was probably working with outdated branch. The number of entities got bumped to 8 in #723 so it makes sense now that we need to bump it again to 9 after adding the model with ball joint.

@iche033 iche033 dismissed ahcorde’s stale review July 18, 2025 20:08

comment addressed

@iche033 iche033 merged commit f1643e4 into gazebosim:gz-physics8 Jul 18, 2025
14 checks passed
@github-project-automation github-project-automation bot moved this from In review to Done in Core development Jul 18, 2025
@srmainwaring srmainwaring deleted the prs/8/pr-ball-joint-properties branch July 23, 2025 10:13
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
🏛️ ionic Gazebo Ionic
Projects
Status: Done
Development

Successfully merging this pull request may close these issues.

dartsim: Support joint dynamics parameters on Ball joints
3 participants