Skip to content

Geometry Test

Each robot's wheel radius and wheel separation are written down twice: in its URDF, the robot's model, and in the controller's geometry.yaml. test_geometry_consistency.py checks that the two agree, for DiffBot and Remo, without starting Gazebo or the robot. It also checks the wheel joints that the controller drives by name. CI runs it on every pull request, with the other tests.

The simulation test drives the robots in Gazebo and would notice large differences only: a wheel separation that's off by less than about 7 % can pass its turn. Neither test can show that the URDF matches the real wheels. That needs a measurement on the robot (diffbot#138 for Remo, diffbot#161 for DiffBot).

What it compares

For Remo, these are the values. The two values in each row must be the same:

Remo's URDFRemo's geometry.yaml
front_left_wheel
radius 0.033 m		⟷wheel_radius
0.033
front_right_wheel
radius 0.033 m		⟷wheel_radius
0.033
Wheel joints
0.144 m apart		⟷wheel_separation
0.144
  • The radii are the wheels' collision shapes: the simple spheres that Gazebo uses for contacts, not the meshes you see.
  • The separation is the straight-line distance between the two wheel joints, the points the wheels turn about. front_left_wheel_joint is at y = 0.072 m and front_right_wheel_joint at y = −0.072 m, with the same x and z.
  • The tolerance is 0.1 mm: a larger difference fails.

xacro computes the URDF's values from the YAML files of the robot's description package, for Remo front_wheel.yaml and base.yaml in remo_description, for DiffBot the same files in diffbot_description/config/diffbot. The test compares these nominal values only. A calibration of the real robot belongs in diff_drive_controller's multipliers, such as wheel_separation_multiplier, "used to account for a difference between the robot model and a real robot". The test leaves them alone.

Run it

In the dev container, in the built workspace:

1
2
3
cd ~/catkin_ws
source devel/setup.bash
python3 src/diffbot/diffbot_gazebo/test/test_geometry_consistency.py -v

It needs remo_description in the workspace, as the dev container has it, but it starts no ROS node and no Gazebo, so it takes a few seconds. In a workspace built into an install space, source install/setup.bash instead. The output:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
test_agreement (__main__.TestComparison) ... ok
test_effort_interface (__main__.TestComparison) ... ok
test_fixed_wheel_joint (__main__.TestComparison) ... ok
test_hardware_joints_missing (__main__.TestComparison) ... ok
test_hardware_joints_reversed (__main__.TestComparison) ... ok
test_nan_radius (__main__.TestComparison) ... ok
test_nan_separation (__main__.TestComparison) ... ok
test_no_right_wheel (__main__.TestComparison) ... ok
test_renamed_joints_and_links (__main__.TestComparison) ... ok
test_shared_transmission (__main__.TestComparison) ... ok
test_unknown_wheel_joint (__main__.TestComparison) ... ok
test_velocity_interface_second (__main__.TestComparison) ... ok
test_wheel_lists (__main__.TestComparison) ... ok
test_wrong_radius (__main__.TestComparison) ... ok
test_wrong_separation (__main__.TestComparison) ... ok
test_robot (__main__.TestModels) ... ok
test_simulation (__main__.TestModels) ... ok

----------------------------------------------------------------------
Ran 17 tests in 3.434s

OK

17 tests: 15 check the comparison itself with a small made-up model (step 4), and 2 check the robots as their launch files load them, each for DiffBot and Remo.

When the two disagree: with Remo's wheel_separation changed to 0.145 m on purpose, both model tests fail for Remo with the same message. The first failure:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
FAIL: test_robot (__main__.TestModels) (model='remo')
----------------------------------------------------------------------
Traceback (most recent call last):
  File "src/diffbot/diffbot_gazebo/test/test_geometry_consistency.py", line 507, in test_robot
    self.check(model, dump_params('diffbot_base', 'diffbot.launch', model=model))
  File "src/diffbot/diffbot_gazebo/test/test_geometry_consistency.py", line 493, in check
    self.assertEqual(mismatches(params), [],
AssertionError: Lists differ: ['the wheel joints are 0.1440 m apart, but wheel_separation is 0.1450 m'] != []

First list contains 1 additional elements.
First extra element 0:
'the wheel joints are 0.1440 m apart, but wheel_separation is 0.1450 m'

- ['the wheel joints are 0.1440 m apart, but wheel_separation is 0.1450 m']
+ [] : remo: URDF and controller disagree

======================================================================

In the message, 0.1440 m is the distance between the wheel joints in the URDF, and 0.1450 m is wheel_separation in geometry.yaml. Don't copy one into the other: measure the robot to find out which one is right (How to tune).

In CI, catkin build --catkin-make-args run_tests runs all of the workspace's tests, the simulation tests with Gazebo included, and catkin_test_results build reports them. diffbot_gazebo's CMakeLists.txt registers this file with catkin's catkin_add_nosetests, which adds Python tests that nose, a Python test runner, runs. nose runs the file's unittest classes and writes the results where catkin_test_results counts them.

How it works

The code excerpts show the files at the diffbot commit that this page pins, with their line numbers. The link above each block opens the same lines on GitHub.

The file starts with the names it uses throughout, lines 46–62:

test_geometry_consistency.py: names
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
MODELS = ('diffbot', 'remo')
TOLERANCE = 1e-4  # m
NAMESPACE = '/diffbot'
# The prefix of diff_drive_controller's parameters
CONTROLLER = NAMESPACE + '/mobile_base_controller/'
# The joints that diffbot_base's hardware interface drives, in the order of its motors
HARDWARE_JOINTS = NAMESPACE + '/hardware_interface/joints'
# The URDF that xacro expanded, as one string
ROBOT_DESCRIPTION = NAMESPACE + '/robot_description'
# diff_drive_controller commands the wheels' velocity. Gazebo's DefaultRobotHWSim takes the first
# interface of a joint's transmission and accepts these two spellings
VELOCITY_INTERFACES = ('hardware_interface/VelocityJointInterface', 'VelocityJointInterface')

# Parameters by name, as roslaunch --dump-params prints them
Params = Dict[str, Any]
Vector = Tuple[float, float, float]
Matrix = Tuple[Vector, Vector, Vector]

TOLERANCE is 0.1 mm. NAMESPACE and CONTROLLER are the beginnings of the parameter names that roslaunch reports (step 1). HARDWARE_JOINTS and ROBOT_DESCRIPTION are the full names of the hardware interface's joint list and of the URDF. VELOCITY_INTERFACES are the two spellings of the velocity interface that Gazebo accepts (velocity interface). Params, Vector and Matrix are short names for the types that the functions pass around.

1. The launch files supply the values

TestModels checks the real robots' configuration, lines 488–507:

test_geometry_consistency.py: TestModels
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
class TestModels(unittest.TestCase):
    """Each robot's URDF and controller configuration agree, as the launch files load them."""

    def check(self, model: str, params: Params) -> None:
        """Assert that the URDF in params agrees with the controller's parameters."""
        self.assertEqual(mismatches(params), [],
                         '{}: URDF and controller disagree'.format(model))

    def test_simulation(self) -> None:
        for model in MODELS:
            with self.subTest(model=model):
                params = dump_params('diffbot_gazebo', 'diffbot_view.launch', model=model)
                params.update(dump_params('diffbot_control', 'diffbot_control.launch',
                                          model=model, rqt_robot_steering='false'))
                self.check(model, params)

    def test_robot(self) -> None:
        for model in MODELS:
            with self.subTest(model=model):
                self.check(model, dump_params('diffbot_base', 'diffbot.launch', model=model))

It checks each robot in MODELS, DiffBot and Remo. test_simulation() combines the parameters of the two launch files that start the simulation: diffbot_view.launch loads the URDF, and diffbot_control.launch loads the controller's configuration. test_robot() takes the parameters of diffbot.launch, which loads both on the real robot. check() fails when mismatches() reports a problem.

dump_params() gets the parameters, lines 65–86:

test_geometry_consistency.py: dump_params()
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
def dump_params(package: str, launch_file: str, **launch_args: str) -> Params:
    """Return the parameters that a launch file sets, by parameter name.

    roslaunch --dump-params reads the launch file and the files it includes, and prints the
    parameters as YAML, without starting a node.
    """
    command = ['roslaunch', '--dump-params', package, launch_file]
    command += ['{}:={}'.format(name, value) for name, value in launch_args.items()]
    launch = ' '.join(command[2:])
    try:
        result = subprocess.run(command, stdout=subprocess.PIPE, stderr=subprocess.PIPE,
                                universal_newlines=True, check=True)
    except subprocess.CalledProcessError as error:
        raise RuntimeError(
            'roslaunch failed for {}: {}'.format(launch, error.stderr.strip())) from error
    params = yaml.safe_load(result.stdout)
    # roslaunch ends with 0 and prints nothing when it can't load a file, for example when a
    # package such as remo_description is missing
    if not isinstance(params, dict):
        raise RuntimeError(
            'roslaunch printed no parameters for {}: {}'.format(launch, result.stderr.strip()))
    return params

roslaunch --dump-params reads a launch file and the files it includes, and prints the parameters that it would set, as YAML. It starts no node, so these are the values the launch files would load, not values read from a running robot. When roslaunch can't load a file, for example because remo_description is missing, it prints nothing and still ends without an error. So an empty result is an error too.

What params holds: every parameter that the launch files set, by its full name. Two kinds matter for the test:

  • The controller's configuration, from diffbot_control.yaml and the robot's geometry.yaml: for example /diffbot/mobile_base_controller/wheel_radius, a number.
  • /diffbot/robot_description, the URDF that xacro expanded from the description package, all of it as one string.

mismatches() takes the whole dictionary, and wheel_setup() (step 2) reads the URDF from it. Both come from the real launch files, which is why they're what the robot gets. The simulation's dictionary combines two launch files, so one dictionary alone doesn't prove that two values come from the same file.

The controller's values, and the parameters that roslaunch reports

The controller's values: each robot's geometry.yaml sets them for diff_drive_controller. Its comment says where the URDF has the same values.

diffbot_control/config/diffbot/geometry.yaml
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
# DiffBot's nominal wheel geometry for diff_drive_controller's odometry and wheel commands,
# measured on the robot and kept consistent with its URDF. The hardware interface (diffbot_base)
# also reads wheel_radius to convert its speed limit to rad/s for host-side PID limits and
# motor-command scaling. The Teensy firmware fetches wheel_radius at startup, but its active
# motor-control loop does not use it.
# The values, and where the URDF has them:
# - wheel_radius: half the mean of the two wheels' diameters, 66.3 and 66.8 mm
#   (radius in diffbot_description/config/diffbot/front_wheel.yaml)
# - wheel_separation: from the middle of one tire to the middle of the other (in the URDF: base
#   y_size 0.08 + 2 * (y_offset 0.0165 + length / 2 0.0125), base.yaml and front_wheel.yaml)
# The launch files load it after diffbot_control.yaml, with the model argument
diffbot:
  mobile_base_controller:
    wheel_radius: 0.0333      # m
    wheel_separation: 0.138   # m
diffbot_control/config/remo/geometry.yaml
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
# Remo's nominal wheel geometry for diff_drive_controller's odometry and wheel commands,
# measured on the robot and kept consistent with its URDF. The hardware interface (diffbot_base)
# also reads wheel_radius to convert its speed limit to rad/s for host-side PID limits and
# motor-command scaling. The Teensy firmware fetches wheel_radius at startup, but its active
# motor-control loop does not use it.
# The values, and where the URDF has them:
# - wheel_radius: half the mean of the two wheels' diameters, 65.5 and 66.5 mm
#   (radius in remo_description/config/remo/front_wheel.yaml)
# - wheel_separation: from the middle of one tire to the middle of the other (in the URDF: base
#   y_size 0.08 + 2 * (y_offset 0.0195 + length / 2 0.0125), base.yaml and front_wheel.yaml)
# The launch files load it after diffbot_control.yaml, with the model argument
diffbot:
  mobile_base_controller:
    wheel_radius: 0.0330      # m
    wheel_separation: 0.144   # m

What roslaunch reports: roslaunch --dump-params prints the parameters in alphabetical order. The controller's parameters come under /diffbot/mobile_base_controller/, and the hardware interface's under /diffbot/hardware_interface/. /diffbot/robot_description is the expanded URDF, 912 lines for Remo as one string, cut after its first characters here. The test reads six of the parameters, highlighted below: wheel_radius, wheel_separation, left_wheel and right_wheel of the controller, the hardware interface's joints, and robot_description. The outputs are Remo's, checked in the dev container with diffbot c00651e (noetic-devel) and remo_description 2.0.0. With model:=diffbot, the wheel radius is 0.0333 m and the wheel separation is 0.138 m.

On the robot, diffbot.launch loads the URDF and geometry.yaml for the hardware interface and the controllers:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
$ roslaunch --dump-params diffbot_base diffbot.launch model:=remo
/diffbot/debug/base_controller: true
/diffbot/debug/hardware_interface: false
/diffbot/encoder_resolution: 542
/diffbot/gain: 1.0
/diffbot/hardware_interface/joints:
- front_left_wheel_joint
- front_right_wheel_joint
/diffbot/joint_state_controller/extra_joints:
- effort: 0.0
  name: rear_caster_rotation_joint
  position: 0.01
  velocity: 0.0
- effort: 0.0
  name: rear_caster_wheel_joint
  position: 0.01
  velocity: 0.0
/diffbot/joint_state_controller/publish_rate: 50
/diffbot/joint_state_controller/type: joint_state_controller/JointStateController
/diffbot/mobile_base_controller/angular/z/has_acceleration_limits: true
/diffbot/mobile_base_controller/angular/z/has_velocity_limits: true
/diffbot/mobile_base_controller/angular/z/max_acceleration: 6.0
/diffbot/mobile_base_controller/angular/z/max_velocity: 8.0
/diffbot/mobile_base_controller/base_frame_id: base_footprint
/diffbot/mobile_base_controller/left_wheel: front_left_wheel_joint
/diffbot/mobile_base_controller/linear/x/has_acceleration_limits: true
/diffbot/mobile_base_controller/linear/x/has_velocity_limits: true
/diffbot/mobile_base_controller/linear/x/max_acceleration: 0.6
/diffbot/mobile_base_controller/linear/x/max_velocity: 0.3
/diffbot/mobile_base_controller/pose_covariance_diagonal:
- 0.001
- 0.001
- 0.001
- 0.001
- 0.001
- 0.03
/diffbot/mobile_base_controller/publish_rate: 50
/diffbot/mobile_base_controller/right_wheel: front_right_wheel_joint
/diffbot/mobile_base_controller/twist_covariance_diagonal:
- 0.001
- 0.001
- 0.001
- 0.001
- 0.001
- 0.03
/diffbot/mobile_base_controller/type: diff_drive_controller/DiffDriveController
/diffbot/mobile_base_controller/wheel_radius: 0.033
/diffbot/mobile_base_controller/wheel_separation: 0.144
/diffbot/motor_constant: 1.0
/diffbot/pwm_limit: 1.0
/diffbot/robot_description: "<?xml version=\"1.0\" ?>\n<!-- …
/diffbot/trim: 0.0

In the simulation, two launch files share the work. diffbot_view.launch loads the URDF for Gazebo, and diffbot_control.launch loads geometry.yaml. The test combines their parameters:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
$ roslaunch --dump-params diffbot_gazebo diffbot_view.launch model:=remo
/diffbot/robot_description: "<?xml version=\"1.0\" ?>\n<!-- …
/gazebo/enable_ros_network: true
/use_sim_time: true
$ roslaunch --dump-params diffbot_control diffbot_control.launch model:=remo rqt_robot_steering:=false
/diffbot/gazebo_ros_control/pid_gains/front_left_wheel_joint/antiwindup: true
/diffbot/gazebo_ros_control/pid_gains/front_left_wheel_joint/d: 0.0
/diffbot/gazebo_ros_control/pid_gains/front_left_wheel_joint/i: 0.6
/diffbot/gazebo_ros_control/pid_gains/front_left_wheel_joint/i_clamp: 0.05
/diffbot/gazebo_ros_control/pid_gains/front_left_wheel_joint/p: 0.03
/diffbot/gazebo_ros_control/pid_gains/front_right_wheel_joint/antiwindup: true
/diffbot/gazebo_ros_control/pid_gains/front_right_wheel_joint/d: 0.0
/diffbot/gazebo_ros_control/pid_gains/front_right_wheel_joint/i: 0.6
/diffbot/gazebo_ros_control/pid_gains/front_right_wheel_joint/i_clamp: 0.05
/diffbot/gazebo_ros_control/pid_gains/front_right_wheel_joint/p: 0.03
/diffbot/hardware_interface/joints:
- front_left_wheel_joint
- front_right_wheel_joint
/diffbot/joint_state_controller/extra_joints:
- effort: 0.0
  name: rear_caster_rotation_joint
  position: 0.01
  velocity: 0.0
- effort: 0.0
  name: rear_caster_wheel_joint
  position: 0.01
  velocity: 0.0
/diffbot/joint_state_controller/publish_rate: 50
/diffbot/joint_state_controller/type: joint_state_controller/JointStateController
/diffbot/mobile_base_controller/angular/z/has_acceleration_limits: true
/diffbot/mobile_base_controller/angular/z/has_velocity_limits: true
/diffbot/mobile_base_controller/angular/z/max_acceleration: 6.0
/diffbot/mobile_base_controller/angular/z/max_velocity: 8.0
/diffbot/mobile_base_controller/base_frame_id: base_footprint
/diffbot/mobile_base_controller/left_wheel: front_left_wheel_joint
/diffbot/mobile_base_controller/linear/x/has_acceleration_limits: true
/diffbot/mobile_base_controller/linear/x/has_velocity_limits: true
/diffbot/mobile_base_controller/linear/x/max_acceleration: 0.6
/diffbot/mobile_base_controller/linear/x/max_velocity: 0.3
/diffbot/mobile_base_controller/pose_covariance_diagonal:
- 0.001
- 0.001
- 0.001
- 0.001
- 0.001
- 0.03
/diffbot/mobile_base_controller/publish_rate: 50
/diffbot/mobile_base_controller/right_wheel: front_right_wheel_joint
/diffbot/mobile_base_controller/twist_covariance_diagonal:
- 0.001
- 0.001
- 0.001
- 0.001
- 0.001
- 0.03
/diffbot/mobile_base_controller/type: diff_drive_controller/DiffDriveController
/diffbot/mobile_base_controller/wheel_radius: 0.033
/diffbot/mobile_base_controller/wheel_separation: 0.144

The test reads robot_description as a whole. These are its parts for Remo's left wheel: the collision sphere's radius (line 241 of 912) and the wheel joint with its origin (lines 253–258), with the parts in between left out:

robot_description: Remo's left wheel
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
<link name="front_left_wheel">
  …
  <collision>
    <geometry>
      <sphere radius="0.033"/>
    </geometry>
  </collision>
  …
</link>
<joint name="front_left_wheel_joint" type="continuous">
  <axis rpy="0 0 0" xyz="0 1 0"/>
  <parent link="chassis_link"/>
  <child link="front_left_wheel"/>
  <origin rpy="0 0 0" xyz="0.036000000000000004 0.072 -0.0336"/>
</joint>

2. The wheel setup and the checks

wheel_setup() reads what the checks need from params once, into a WheelSetup, lines 193–240:

test_geometry_consistency.py: WheelSetup and wheel_setup()
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
class WheelSetup(NamedTuple):
    """The drive wheels as the URDF and the controller's parameters describe them."""

    robot: ET.Element
    joints_by_name: Dict[str, ET.Element]
    left_joint_names: List[str]
    right_joint_names: List[str]
    wheel_radius: float
    wheel_separation: float
    # A list of joint names when the parameter is set right, but it can be anything or missing
    hardware_joint_names: object

    @property
    def wheel_joint_names(self) -> List[str]:
        """Return the controller's wheel joints, the left ones first."""
        return self.left_joint_names + self.right_joint_names

    def urdf_wheel_joints(self) -> List[ET.Element]:
        """Return the controller's wheel joints that are in the URDF."""
        return [self.joints_by_name[name] for name in self.wheel_joint_names
                if name in self.joints_by_name]


def wheel_joint_names(params: Params, side: str) -> List[str]:
    """Return the controller's wheel joints on one side, 'left' or 'right'.

    diff_drive_controller takes a joint name or a list of joint names for each side.
    """
    names = params[CONTROLLER + side + '_wheel']
    return [names] if isinstance(names, str) else list(names)


def wheel_setup(params: Params) -> WheelSetup:
    """Return the drive wheels as the launch files' parameters describe them.

    params holds the URDF under ROBOT_DESCRIPTION, next to the parameters of the controller and
    of the hardware interface, as roslaunch --dump-params prints them.
    """
    robot = ET.fromstring(params[ROBOT_DESCRIPTION])
    return WheelSetup(
        robot=robot,
        joints_by_name={joint.get('name', ''): joint for joint in robot.findall('joint')},
        left_joint_names=wheel_joint_names(params, 'left'),
        right_joint_names=wheel_joint_names(params, 'right'),
        wheel_radius=params[CONTROLLER + 'wheel_radius'],
        wheel_separation=params[CONTROLLER + 'wheel_separation'],
        hardware_joint_names=params.get(HARDWARE_JOINTS),
    )

ET.fromstring() parses the URDF's XML from ROBOT_DESCRIPTION, and robot is its root element. The controller takes a name or a list of names for each side, and wheel_joint_names() turns both into a list. hardware_joint_names stays what the parameter holds, because a check reports it when it's wrong.

Each check is a function of its own, named for what it checks. It takes the wheel setup and returns its problems as messages, an empty list when there's none. mismatches() runs them in the order of CHECKS, lines 321–337:

test_geometry_consistency.py: CHECKS and mismatches()
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
CHECKS: List[Callable[[WheelSetup], List[str]]] = [
    check_wheels_named,
    check_wheel_joints,
    check_velocity_interfaces,
    check_hardware_joint_order,
    check_wheel_radius,
    check_wheel_separation,
]


def mismatches(params: Params) -> List[str]:
    """Return where the URDF and the controller's parameters disagree, as messages.

    It runs each check in CHECKS on the same wheel setup. The list is empty when they agree.
    """
    setup = wheel_setup(params)
    return [problem for check in CHECKS for problem in check(setup)]

The next section explains the checks in this order.

3. The checks

The controller names a wheel on each side

The controller finds its wheels by the names of their joints, in diffbot_control.yaml, lines 27–28:

diffbot_control.yaml: the controller's wheels
27
28
left_wheel: 'front_left_wheel_joint'
right_wheel: 'front_right_wheel_joint'

check_wheels_named() reports a side without a name, lines 243–247:

test_geometry_consistency.py: check_wheels_named()
243
244
245
246
247
def check_wheels_named(setup: WheelSetup) -> List[str]:
    """Check that the controller names a wheel joint on each side."""
    sides = (('left', setup.left_joint_names), ('right', setup.right_joint_names))
    return ["the controller's {}_wheel names no joint".format(side)
            for side, joint_names in sides if not joint_names]

The wheel joints are continuous joints in the URDF

A joint connects two links of the URDF. A wheel joint is where the wheel turns, and a continuous joint turns without limits, as a wheel does. In DiffBot's model, the wheel macro creates each wheel joint, lines 89–94. xacro replaces ${prefix} with front_left or front_right, and the test reads the expanded URDF:

common_macros.urdf.xacro: the wheel joint
89
90
91
92
93
94
<joint name="${prefix}_wheel_joint" type="continuous">
  <axis xyz="0 1 0" rpy="0 0 0" />
  <parent link="chassis_link"/>
  <child link="${prefix}_wheel"/>
  <origin xyz="${pos_x_joint} ${reflect* pos_y_joint} ${pos_z_joint}" rpy="0 0 0"/>
</joint>

Remo's model has the same macro in remo_description. check_wheel_joints() looks for each of the controller's wheel joints in the URDF and checks its type, lines 250–259:

test_geometry_consistency.py: check_wheel_joints()
250
251
252
253
254
255
256
257
258
259
def check_wheel_joints(setup: WheelSetup) -> List[str]:
    """Check that the controller's wheel joints are continuous joints in the URDF."""
    problems = []
    for joint_name in setup.wheel_joint_names:
        joint = setup.joints_by_name.get(joint_name)
        if joint is None:
            problems.append("the controller's wheel joint {} isn't in the URDF".format(joint_name))
        elif joint.get('type') != 'continuous':
            problems.append('{} is {}, not continuous'.format(joint_name, joint.get('type')))
    return problems

The other checks only look at the wheel joints that the URDF has (urdf_wheel_joints()), so a missing joint is reported once, here.

A velocity interface for each wheel

In ros_control, a hardware interface is how a controller reads and commands a joint. diff_drive_controller commands each wheel's velocity, so it needs a velocity interface. In the simulation, Gazebo's ros_control plugin gives it one for each joint from the joint's transmission in the URDF, lines 96–105:

common_macros.urdf.xacro: the wheel's transmission
 96
 97
 98
 99
100
101
102
103
104
105
<transmission name="${prefix}_wheel_transmission">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="${prefix}_wheel_joint">
    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
  </joint>
  <actuator name="${prefix}_motor">
    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>

The plugin, DefaultRobotHWSim, skips a transmission with more than one joint and uses the first hardwareInterface of the joint. transmission_interfaces() reads the transmissions the same way, lines 164–181:

test_geometry_consistency.py: transmission_interfaces()
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
def transmission_interfaces(robot: ET.Element) -> Tuple[Dict[str, str], Set[str]]:
    """Return each joint's first transmission interface, and the joints in shared transmissions.

    Gazebo's DefaultRobotHWSim gives a joint the first interface of its transmission, and skips a
    transmission with more than one joint, so only single-joint transmissions give an interface.
    """
    interface_by_joint: Dict[str, str] = {}
    joints_in_shared_transmissions: Set[str] = set()
    for transmission in robot.findall('transmission'):
        transmission_joints = transmission.findall('joint')
        if len(transmission_joints) == 1:
            joint = transmission_joints[0]
            interface = joint.findtext('hardwareInterface', '').strip()
            interface_by_joint[joint.get('name', '')] = interface
        else:
            joints_in_shared_transmissions.update(
                joint.get('name', '') for joint in transmission_joints)
    return interface_by_joint, joints_in_shared_transmissions

check_velocity_interfaces() uses the result for each wheel joint, lines 262–275:

test_geometry_consistency.py: check_velocity_interfaces()
262
263
264
265
266
267
268
269
270
271
272
273
274
275
def check_velocity_interfaces(setup: WheelSetup) -> List[str]:
    """Check that Gazebo gives the controller a velocity interface for each wheel joint."""
    interface_by_joint, joints_in_shared_transmissions = transmission_interfaces(setup.robot)
    problems = []
    for joint in setup.urdf_wheel_joints():
        joint_name = joint.get('name', '')
        if joint_name in joints_in_shared_transmissions and joint_name not in interface_by_joint:
            problems.append(
                '{} is in a transmission with more than one joint, which Gazebo skips'.format(
                    joint_name))
        elif interface_by_joint.get(joint_name) not in VELOCITY_INTERFACES:
            problems.append("{}'s transmission doesn't start with {}".format(
                joint_name, VELOCITY_INTERFACES[0]))
    return problems

What this check covers

It checks the transmissions that these robots need: one joint per transmission, with the velocity interface first. Gazebo accepts more forms, which the test doesn't try to recognize.

The hardware interface's joint order

On the real robot, diffbot_base's hardware interface reads its joints from the same file, lines 1–6:

diffbot_control.yaml: the hardware interface's joints
1
2
3
4
5
6
diffbot:
  # Settings for ros_control hardware interface
  hardware_interface:
     joints:
        - front_left_wheel_joint
        - front_right_wheel_joint

They must be the controller's wheels, the left one first. check_hardware_joint_order() compares the two lists, lines 278–287:

test_geometry_consistency.py: check_hardware_joint_order()
278
279
280
281
282
283
284
285
286
287
def check_hardware_joint_order(setup: WheelSetup) -> List[str]:
    """Check that the hardware interface drives the controller's wheel joints, the left first.

    diffbot_base's hardware interface drives its joints in the order of its list, and the
    Teensy firmware takes the first one for the left motor.
    """
    if setup.hardware_joint_names == setup.wheel_joint_names:
        return []
    return ["the hardware interface's joints {} aren't the controller's wheels {}, in order"
            .format(setup.hardware_joint_names, setup.wheel_joint_names)]

The simulation loads the same list without using it, so there it's a check of the shared configuration.

Why the order matters on the robot

The hardware interface registers each joint in the order of the list, lines 82–99:

diffbot_hw_interface.cpp: registering the joints
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
num_joints_ = joint_names_.size();
ROS_INFO("Number of joints: %d", (int)num_joints_);
std::array<std::string, NUM_JOINTS> motor_names = {"left_motor", "right_motor"};
for (unsigned int i = 0; i < num_joints_; i++)
{
    // Create a JointStateHandle for each joint and register them with the 
    // JointStateInterface.
    hardware_interface::JointStateHandle joint_state_handle(joint_names_[i],
                                                            &joint_positions_[i], 
                                                            &joint_velocities_[i],
                                                            &joint_efforts_[i]);
    joint_state_interface_.registerHandle(joint_state_handle);

    // Create a JointHandle (read and write) for each controllable joint
    // using the read-only joint handles within the JointStateInterface and 
    // register them with the JointVelocityInterface.
    hardware_interface::JointHandle joint_handle(joint_state_handle, &joint_velocity_commands_[i]);
    velocity_joint_interface_.registerHandle(joint_handle);

It sends the wheel velocity commands to the Teensy in the same order, lines 178–186:

diffbot_hw_interface.cpp: the wheel commands
178
179
180
181
182
183
184
185
186
// Publish the desired (commanded) angular wheel joint velocities
diffbot_msgs::WheelsCmdStamped wheel_cmd_msg;
wheel_cmd_msg.header.stamp = ros::Time::now();
for (int i = 0; i < NUM_JOINTS; ++i)
{
    wheel_cmd_msg.wheels_cmd.angular_velocities.joint.push_back(joint_velocity_commands_[i]);
}

pub_wheel_cmd_velocities_.publish(wheel_cmd_msg);

The firmware takes the first value for the left wheel, lines 472–478:

base_controller.h: commandCallback()
472
473
474
475
476
477
478
void diffbot::BaseController<TMotorController, TMotorDriver>::commandCallback(const diffbot_msgs::WheelsCmdStamped& cmd_msg)
{
    // Callback function every time the angular wheel commands for each wheel joint are received from 'wheel_cmd_velocities' topic
    // This callback function receives diffbot_msgs::WheelsCmdStamped message object
    // where diffbot_msgs::AngularVelocities for both joints are stored
    wheel_cmd_velocity_left_ = cmd_msg.wheels_cmd.angular_velocities.joint[0];
    wheel_cmd_velocity_right_ = cmd_msg.wheels_cmd.angular_velocities.joint[1];

With the list reversed, the left wheel's commands would drive the right motor. The planned refactor of diffbot_base and the firmware removes this trap: the configuration binds each joint to an actuator channel by name, and the messages carry each channel's ID instead of a position in a list. That also leaves room for robots with four wheels or steering (diffbot#148).

The wheel radius

check_wheel_radius() compares each wheel's collision radius with wheel_radius, lines 290–299:

test_geometry_consistency.py: check_wheel_radius()
290
291
292
293
294
295
296
297
298
299
def check_wheel_radius(setup: WheelSetup) -> List[str]:
    """Check that each wheel's collision radius is the controller's wheel_radius."""
    problems = []
    for joint in setup.urdf_wheel_joints():
        link_name = required(joint, 'child').get('link', '')
        urdf_radius = collision_radius(setup.robot, link_name)
        if not within_tolerance(urdf_radius, setup.wheel_radius):
            problems.append('{} collision radius {:.4f} m, but wheel_radius is {:.4f} m'.format(
                link_name, urdf_radius, setup.wheel_radius))
    return problems

collision_radius() reads a wheel link's collision shape, a sphere or a cylinder, lines 154–161:

test_geometry_consistency.py: collision_radius()
154
155
156
157
158
159
160
161
def collision_radius(robot: ET.Element, link_name: str) -> float:
    """Return the radius of a wheel link's collision, a sphere or a cylinder."""
    link = next(link for link in robot.findall('link') if link.get('name') == link_name)
    geometry = required(link, 'collision/geometry')
    shape = geometry.find('sphere')
    if shape is None:
        shape = required(geometry, 'cylinder')
    return float(shape.attrib['radius'])

within_tolerance() checks that the difference is at most 0.1 mm, lines 184–190. It checks "within" rather than "too large", because ordered comparisons with a NaN, a value that isn't a number, are false, so a NaN would never be "too large":

test_geometry_consistency.py: within_tolerance()
184
185
186
187
188
189
190
def within_tolerance(urdf_value: float, controller_value: float) -> bool:
    """Return whether the two values differ by at most TOLERANCE.

    A NaN isn't within any tolerance: ordered comparisons with it are false, so a check that a
    difference is too large would let it pass.
    """
    return abs(urdf_value - controller_value) <= TOLERANCE

The wheel separation

check_wheel_separation() compares the distance between the first wheel joint of each side with wheel_separation, as diff_drive_controller takes the first wheel on each side, lines 302–317:

test_geometry_consistency.py: check_wheel_separation()
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
def check_wheel_separation(setup: WheelSetup) -> List[str]:
    """Check that the first wheel joints of both sides are wheel_separation apart.

    diff_drive_controller takes the separation of the first wheel on each side, so this does too.
    """
    first_joint_names = setup.left_joint_names[:1] + setup.right_joint_names[:1]
    if len(first_joint_names) != 2 or any(
            name not in setup.joints_by_name for name in first_joint_names):
        return []  # check_wheels_named() and check_wheel_joints() report this
    left_position, right_position = (
        joint_position(setup.robot, name) for name in first_joint_names)
    urdf_separation = math.dist(left_position, right_position)
    if within_tolerance(urdf_separation, setup.wheel_separation):
        return []
    return ['the wheel joints are {:.4f} m apart, but wheel_separation is {:.4f} m'.format(
        urdf_separation, setup.wheel_separation)]

Each joint's origin in the URDF is given in its parent link's frame. To compare the two wheel joints, joint_position() moves a joint's position up the tree into the frame of the URDF's root link, base_footprint, lines 134–151:

test_geometry_consistency.py: joint_position()
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
def joint_position(robot: ET.Element, joint_name: str) -> Vector:
    """Return the position of a joint in the frame of the URDF's root link.

    The joint's origin is given in its parent link's frame. Each joint further up the tree moves
    it into the frame of that joint's parent link, up to the root link.
    """
    joints_by_name = {joint.get('name', ''): joint for joint in robot.findall('joint')}
    joints_by_child_link = {
        required(joint, 'child').get('link'): joint for joint in robot.findall('joint')}
    joint = joints_by_name[joint_name]
    position, _ = joint_origin(joint)
    link_name = required(joint, 'parent').get('link')
    while link_name in joints_by_child_link:
        parent_joint = joints_by_child_link[link_name]
        offset, rotation = joint_origin(parent_joint)
        position = transform(offset, rotation, position)
        link_name = required(parent_joint, 'parent').get('link')
    return position

Each step rotates the point into the parent's frame and adds the offset of the joint between them, lines 127–131:

test_geometry_consistency.py: transform()
127
128
129
130
131
def transform(offset: Vector, rotation: Matrix, point: Vector) -> Vector:
    """Return a point in the parent frame: rotated into it, then moved by the offset."""
    x, y, z = (offset[row] + sum(rotation[row][column] * point[column] for column in range(3))
               for row in range(3))
    return (x, y, z)

For Remo's left wheel joint, none of the joints on the way is rotated, so only the offsets add up:

In the frame ofStepPosition (m)
chassis_linkthe wheel joint's origin(0.036, 0.072, −0.0336)
base_linkplus the chassis joint's origin (−0.036, 0, 0)(0, 0.072, −0.0336)
base_footprintplus the base link joint's origin (0, 0, 0.0666)(0, 0.072, 0.033)

The right wheel joint ends at (0, −0.072, 0.033), 0.144 m away. joint_origin() reads an origin's position and its roll, pitch and yaw, lines 117–124, and rotation_matrix() turns them into a rotation, lines 97–114.

4. The comparison's own tests

TestComparison checks that each check notices its kind of problem. It uses a small URDF with Remo's wheels, lines 343–370, and launch parameters that hold it, with values that agree with it, lines 372–386:

test_geometry_consistency.py: TestComparison.launch_params()
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
def launch_params(self, urdf: Optional[str] = None, wheel_radius: float = 0.033,
                  wheel_separation: float = 0.144,
                  left_wheel: Any = 'front_left_wheel_joint',
                  right_wheel: Any = 'front_right_wheel_joint') -> Params:
    """Return launch parameters that agree, unless the arguments change them.

    They hold the class's URDF, or urdf when it's given, and the parameters that the
    controller and the hardware interface read, as roslaunch --dump-params prints them.
    """
    return {ROBOT_DESCRIPTION: self.URDF if urdf is None else urdf,
            CONTROLLER + 'wheel_radius': wheel_radius,
            CONTROLLER + 'wheel_separation': wheel_separation,
            CONTROLLER + 'left_wheel': left_wheel,
            CONTROLLER + 'right_wheel': right_wheel,
            HARDWARE_JOINTS: ['front_left_wheel_joint', 'front_right_wheel_joint']}

Each test changes one thing and checks the message, for example a wrong separation, lines 400–403:

test_geometry_consistency.py: TestComparison.test_wrong_separation()
400
401
402
403
def test_wrong_separation(self) -> None:
    problems = mismatches(self.launch_params(wheel_separation=0.145))
    self.assertEqual(len(problems), 1)
    self.assertIn('wheel_separation', problems[0])

The others: a wrong radius, a list of wheels instead of a name, an unknown wheel joint, a fixed wheel joint, renamed joints and links, an effort interface instead of a velocity interface, the velocity interface second, both wheels in one transmission, a NaN radius and separation, no right wheel, and the hardware interface's joints reversed or missing.

Comments

Comments are GitHub Discussions, shown with giscus. Loading them connects to giscus.app (hosted by Vercel, USA) and GitHub (USA); you need a GitHub account to comment.