Closed kinematic chains only seem to work when joints are aligned with world axes

[wiartallajan]

Environment

• OS Version: Ubuntu 22.04
• Binary build of ROS2 Humble with Gazebo Harmonic as explained in the official gazebo docs

Description

• Expected behavior:

The five bar linkage with an additional vertical joint should move freely while the vertical joint can be controlled, rotating the plane of motion of the five bar linkage around the z-axis.

• Actual behavior:

Gazebo crashes as soon as the vertical joint is actuated.

Steps to reproduce

1. Create a test workspace

mkdir test_ws
cd test_ws
mkdir src
colcon build

2. Clone & build the example respository

cd src
git clone https://github.com/wiartallajan/ros2_closed_loop_demo.git
cd ..
colcon build
. install/Setup.bash

3. Run the provided examples

The modeled five bar linkage was based on the solution for closed kinematic chains using the DetachableJoint plugin proposed in #25.

3.1. [Working] Five bar linkage in x-z-plane without vertical joint

ros2 launch ros2_closed_loop_demo_bringup launch_closed_loop_demo.launch.py 

This is the base model closed_loop_demo. All joint axis of the model are aligned with the y-axis (0 1 0). The model is inserted into the world without any rotational transformation. The five bar linkage moves as expected.

3.2. [Failing] Rotated five bar linkage without vertical joint

ros2 launch ros2_closed_loop_demo_bringup launch_closed_loop_rotated.launch.py 

The same closed_loop_demo model from exampe 3.1 is reused. But now the model is inserted into the world with some rotational transformation <pose>0 0 2 0 0 1</pose>. The five bar linkage bugs around (see section Output), the loop closure is violated.

When changing the include pose in the file ros2_closed_loop_demo/ros2_closed_loop_demo_bringup/launch/launch_closed_loop_rotated.launch.py from the provided <pose>0 0 2 0 0 1</pose> to something arbitrary else, e.g. <pose>0 0 2 0 0 2.53</pose>, Gazebo doesn't even launch but crashes immediately with the same output as provided in the output of 3.3 (see section below).

3.3. [Failing] Five bar linkage with vertical joint

ros2 launch ros2_closed_loop_demo_bringup launch_closed_loop_addJoint.launch.py 

This example uses the modified closed_loop_addJoint model. The joint_D0 from the model of exampe 3.1 was changed from fixed to a revolute joint with axis (0 0 1).

After launching gazebo, load the Joint Position Controller plugin and try to move joint_D0.

Output

3.1. [Working] Five bar linkage in x-z-plane without vertical joint

In this example, everything works as expected.

3.2. [Failing] Rotated five bar linkage without vertical joint

Here, the model jumps around (without any external input!) and the detachableJoint looks broken

3.3. [Failing] Fivebar linkage with vertical joint

As long as the vertical joint joint_D0 is not moved via the Joint Position Controller plugin, the model behavior equals that of example 3.1:

But as soon at the joint is moved, Gazebo crashes with the following output:

[ruby $(which gz) sim-2] gz sim server: ./dart/dynamics/RevoluteJoint.cpp:186: virtual void dart::dynamics::RevoluteJoint::updateRelativeTransform() const: Assertion `math::verifyTransform(mT)' failed.
[ruby $(which gz) sim-2] Stack trace (most recent call last):
[ruby $(which gz) sim-2] #31   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7d0e9dc31fc5, in 
[ruby $(which gz) sim-2] #30   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7d0e9dc2fc34, in 
[ruby $(which gz) sim-2] #29   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7d0e9db7ba4e, in 
[ruby $(which gz) sim-2] #28   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7d0e9daa69ac, in rb_protect
[ruby $(which gz) sim-2] #27   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7d0e9dc3ec61, in rb_yield
[ruby $(which gz) sim-2] #26   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7d0e9dc3a30c, in rb_vm_exec
[ruby $(which gz) sim-2] #25   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7d0e9dc34c96, in 
[ruby $(which gz) sim-2] #24   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7d0e9dc31fc5, in 
[ruby $(which gz) sim-2] #23   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7d0e9dc2fc34, in 
[ruby $(which gz) sim-2] #22   Object "/usr/lib/x86_64-linux-gnu/ruby/3.0.0/fiddle.so", at 0x7d0e9ddcd44b, in 
[ruby $(which gz) sim-2] #21   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7d0e9dbfd088, in rb_nogvl
[ruby $(which gz) sim-2] #20   Object "/usr/lib/x86_64-linux-gnu/ruby/3.0.0/fiddle.so", at 0x7d0e9ddccd6b, in 
[ruby $(which gz) sim-2] #19   Object "/lib/x86_64-linux-gnu/libffi.so.8", at 0x7d0e9ddbe492, in 
[ruby $(which gz) sim-2] #18   Object "/lib/x86_64-linux-gnu/libffi.so.8", at 0x7d0e9ddc1e2d, in 
[ruby $(which gz) sim-2] #17   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8-gz.so.8.9.0", at 0x7d0e9d8a077d, in runServer
[ruby $(which gz) sim-2] #16   Object "/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7d0e9813ff05, in 
[ruby $(which gz) sim-2] #15   Object "/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7d0e981564ba, in gz::sim::v8::SimulationRunner::Run(unsigned long)
[ruby $(which gz) sim-2] #14   Object "/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7d0e981553a0, in gz::sim::v8::SimulationRunner::Step(gz::sim::v8::UpdateInfo const&)
[ruby $(which gz) sim-2] #13   Object "/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7d0e9814b67a, in gz::sim::v8::SimulationRunner::UpdateSystems()
[ruby $(which gz) sim-2] #12   Object "/usr/lib/x86_64-linux-gnu/gz-sim-8/plugins/libgz-sim-physics-system.so", at 0x7d0e8e317bd6, in gz::sim::v8::systems::Physics::Update(gz::sim::v8::UpdateInfo const&, gz::sim::v8::EntityComponentManager&)
[ruby $(which gz) sim-2] #11   Object "/usr/lib/x86_64-linux-gnu/gz-sim-8/plugins/libgz-sim-physics-system.so", at 0x7d0e8e318251, in gz::sim::v8::systems::PhysicsPrivate::Step(std::chrono::duration<long, std::ratio<1l, 1000000000l> > const&)
[ruby $(which gz) sim-2] #10   Object "/usr/lib/x86_64-linux-gnu/gz-physics-7/engine-plugins/libgz-physics-dartsim-plugin.so", at 0x7d0e4e20dde3, in gz::physics::dartsim::SimulationFeatures::WorldForwardStep(gz::physics::Identity const&, gz::physics::SpecifyData<gz::physics::RequireData<gz::physics::WorldPoses>, gz::physics::ExpectData<gz::physics::ChangedWorldPoses, gz::physics::Contacts, gz::physics::JointPositions> >&, gz::physics::CompositeData&, gz::physics::ExpectData<gz::physics::ApplyExternalForceTorques, gz::physics::ApplyGeneralizedForces, gz::physics::VelocityControlCommands, gz::physics::ServoControlCommands, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&)
[ruby $(which gz) sim-2] #9    Object "/usr/lib/x86_64-linux-gnu/gz-physics-7/engine-plugins/libgz-physics-dartsim-plugin.so", at 0x7d0e4e211051, in void gz::physics::CanWriteRequiredData<gz::physics::dartsim::SimulationFeatures, gz::physics::RequireData<gz::physics::WorldPoses> >::WriteRequiredData<gz::physics::SpecifyData<gz::physics::RequireData<gz::physics::WorldPoses>, gz::physics::ExpectData<gz::physics::ChangedWorldPoses, gz::physics::Contacts, gz::physics::JointPositions> > >(gz::physics::SpecifyData<gz::physics::RequireData<gz::physics::WorldPoses>, gz::physics::ExpectData<gz::physics::ChangedWorldPoses, gz::physics::Contacts, gz::physics::JointPositions> >&, gz::physics::WriteOptions const&) const
[ruby $(which gz) sim-2] #8    Object "/usr/lib/x86_64-linux-gnu/gz-physics-7/engine-plugins/libgz-physics-dartsim-plugin.so", at 0x7d0e4e20bf7a, in gz::physics::dartsim::SimulationFeatures::Write(gz::physics::WorldPoses&) const
[ruby $(which gz) sim-2] #7    Object "/lib/x86_64-linux-gnu/libdart.so.6.13", at 0x7d0e4dc2df72, in dart::dynamics::Frame::getWorldTransform() const
[ruby $(which gz) sim-2] #6    Object "/lib/x86_64-linux-gnu/libdart.so.6.13", at 0x7d0e4dc79669, in dart::dynamics::Joint::getRelativeTransform() const
[ruby $(which gz) sim-2] #5    Object "/lib/x86_64-linux-gnu/libdart.so.6.13", at 0x7d0e4dcce2f4, in dart::dynamics::RevoluteJoint::updateRelativeTransform() const
[ruby $(which gz) sim-2] #4    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7d0e9d639e95, in __assert_fail
[ruby $(which gz) sim-2] #3    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7d0e9d62871a, in 
[ruby $(which gz) sim-2] #2    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7d0e9d6287f2, in abort
[ruby $(which gz) sim-2] #1    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7d0e9d642475, in raise
[ruby $(which gz) sim-2] #0    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7d0e9d6969fc, in pthread_kill
[ruby $(which gz) sim-2] Aborted (Signal sent by tkill() 2079649 765412568)
[INFO] [ruby $(which gz) sim-2]: process has finished cleanly [pid 2079288]
[INFO] [launch]: process[ruby $(which gz) sim-2] was required: shutting down launched system

What I think is going on

Not having the skills to actually examine this any further (as I am a mechanical engineer not having extensive coding skills), my best guess is that as soon as the joint axes are rotated via transformations, some kind of numerical inaccuracies happen (e.g. simple rounding?) so that the previous parallel axes are not recognized as exactly parallel anymore.

Maybe a possible solution could be to integrate some kind of "error tolerance" in that regard, so that almost parallel axes (with a suitable small tolerance) are still treated as parallel?