[Documentation] [TitleIndex] [WordIndex


1. Description

This package provides services to perform calculations related to the mild's kinematic model.

It's main function is to solve the robot's inverse kinematics in different variations.

Furthermore, various utility functions related to the robot's kinematics are provided by it and can be accessed via ROS service calls or by directly implementing the package library.

2. Functionality

The package contains algorithms and functions to the following tasks:

2.1 Inverse Kinematics - with base movement

Solving the inverse kinematics represents the primary function of this package.

This means that, provided a target camera pose

(x, y, z, uw, ux, uy, uz)

a robot configuration

(x, y, alpha, ptu-pan, ptu-tilt)

- with x, y as the 2D position of the robot's base, alpha as the robot's rotation on the 2D map and pan, tilt as the angles of the PTU unit -

should be found, that moves the camera as close as possible to this requested pose.

Since the perfect solution for this problem can be ambiguous, two algorithms for solving this problem have been implemented.

2.1.1 Approximated solution

The approximated solution assumes both pan and tilt joint as well as the camera's center point to be at the same position in the kinematic chain.

Also, the position of this rotation point is assumed to be directly above the center of the robot's base.

Considering this simplified version of the kinematic chain, an optimization algorithm provided by the glpk package is used to find an optimal solution for pan and tilt angles, as well as the base rotation.

The calculated base position will trivially be equal to the x- and y-position of the target camera pose.

2.1.2 Exact geometrical solution

The exact geometrical algorithm uses the actual proportions of the kinematic chain to find an optimal solution for a requested camera pose.

Since in almost all cases, a camera pose can not be reached exactly by the robot due to restrictions in the kinematic chain, this algorithm will focus on bringing the camera into a pose that approximates the requested field of view as closely as possible.


The goal is to find a camera pose (and a corresponding view frustum - red), that faces the center of the target view frustum (black) in the same distance l while minimizing the angle deviation U0001d6ff as much as possible.

Using this optimization goal, an optimal robot configuration is calculated in two consecutive steps:

I - Finding optimal tilt angle and the tilt joint's pose in 3D-space

First of all, a set of assumptions will be made, that can be considered to be true about the mild's kinematic chain:


Using these assumptions, the optimization problem can reduced to a 2-dimentional geometrical calculation and the optimal position of the tilt joint on this plane and its corresponding angle can be calculated directly.

Let's consider a side view of the camera facing the target view frustum center, as well as the tilt joint (TILTED). Note, that the dotted horizontal line corresponds to the blue plane in the graphic above.


Given the position of the target view frustum center PView, the camera distance to the center a, the projected distance between tilt joint and camera center point c as well as the tilt joint's constant height above ground h, the tilt angle can be determined.

Once a solution for the tilt angle has been found, the position of PTilted can be calculated directly.

II - Calculating optimal pan angle and the corresponding base pose

Once an optimal pose for the tilt joint in 3D space has been determined, all that is left is to find a robot pose as well as a pan angle, that would place the tilt joint in that position.

However, the solution to this problem is not unambiguous, since the pan axis and the robot's base rotational axis are parallel and therefore create a mathematical singularity with an infinite number of possible robot configurations.

In this approach, this problem will be solved by sampling possible pan angles and selecting the one, that will result in the most convenient robot pose.


Visualized sampling of possible pan angles and the resulting position of the robot's base (small dots). Green dots are more favourable poses whereas non reachable or less favourable poses are displayed in red. Notice, that the robot's kinematic chain has been altered in this example, to show the criterias of selecting the best angle more clearly.

Several heuristics have been implemented to assess the feasibility of a given robot pose. Since rotations of the base are the most time consuming factor in this optimization process, all heuristics will focus on minimizing the those:


    • This approach draws a straight line between the current robot position and the target position. It then sums up the absolute angle difference between the current orientation and this line as well as the line and the target robot orientation. The resulting value between 0° and 360° can be used to get a rough estimation of the traversed angle difference. However, this approach is only valid if the robot can reach the target pose in a roughly straight line. In a narrow space that includes a lot of obstacles, this heuristic might become imprecise.

    • This heuristic calculates the complete change of the base's orientation over the full path from the current base pose to the target pose. The absolute traversed angle difference of a path P will be calculated as:

      formel2.gif with α being the angle of the triangle defined by the three given points and x being the points defining the path P. This heuristic is the most exact one, however, since it requires the calculation of a complete navigation path, it is also the most perfomance-intensive one.

  3. SIMPLE:

    • The simple heuristic compares the current orientation of the base with the target orientation and returns an absolute angle difference between 0° and 180°. The same restrictions as for (1.) apply here as well.


The algorithm will return an optimal robot configuration, that would move the robot's camera into a pose with the same distance to the desired view frustums center and the least possible angle deviation from the desired view. However, if a requested camera pose is too far above or below the robot and therefore the view center point can not be focused by the camera within the optimal distance, no solution will be returned by the algorithm.

index5.png index6.png index7.png

Requested camera poses (green) and approximated robot configurations (blue). Poses close to the camera's movement space can be approximated almost exactly (left), while poses below or above can only be roughly approximated (center). Poses too far above or below can not be approximated at all (right).

2.2 Inverse Kinematics - without base movement (PTU pose correction)

For some situations, it can preferable not to move the robot's base in order to reach a desired camera position but instead only try to find the optimal PTU angles

(ptu-pan, ptu-tilt)

to focus the camera on a target view point. Notice that this is only possible if the PTU angle restrictions allow moving to such a configuration and that an optimal distance of the camera to the target point can not be guaranteed.

A more detailed description of how this problem is being solve can be found here posecorrection.pdf.

2.3 Miscellaneous utility functions

Other utitlity functions provided by this package include calculation of the camera pose from a robot configuration, measuring the length of a navigation path between two robot poses and checking, whether a point on the map can be reached by the robot.

3. Usage

3.1. Needed packages

3.2. Needed software

3.3. Start system

Starting the RobotModelService for the real robot model:

launch asr_robot_model_services RobotModelServiceReal.launch

Starting the RobotModelService for the simulated robot model:

launch asr_robot_model_services RobotModelServiceSim.launch

Starting the set_focus_point script (see tutorial):

launch asr_robot_model_services set_focus_point.launch

4. ROS Nodes

4.1. Published Topics

4.2. Parameters


This file specifies general settings for the robot model as well as debug options


This file specifies topic names, to which results will be published


This file specifies the names of key frames in the robot's kinematic chain. See asr_mild_kinematic_chain for an overview over the mild's kinematic chain.

4.3. Needed Services

4.4. Provided Services

5. Tutorials

6. References

The functionality for the pose correction is described here: posecorrection.pdf

2024-07-20 12:40