# ORIon-UTBMan 2023 Team Description Paper

Samuel Sze, Ricardo Cannizzaro, Matthew Munks, Kim Tien Ly, Ana Deligny, Arundathi Shanthini, Lars Kunze, Ioannis Havoutis, Nick Hawes, Victor Pozo-Fernandez, et al.

## ► To cite this version:

Samuel Sze, Ricardo Cannizzaro, Matthew Munks, Kim Tien Ly, Ana Deligny, et al. ORIon-UTBMan 2023 Team Description Paper. 2023, pp.9. hal-04500704

HAL Id: hal-04500704

https://hal.science/hal-04500704v1

Submitted on 12 Mar 2024

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons CC BY 4.0 - Attribution - International License
# ORIon-UTBMan 2023 Team Description Paper

Samuel Sze¹, Ricardo Cannizzaro¹, Matthew Munks¹, Kim Tien Ly¹, Ana Deligny¹, Arundathi Shanthini¹, Lars Kunze¹, Ioannis Havouris¹, Nick Hawes¹, Victor Pozo-Fernandez², Iaroslav Okunevich², and Zhi Yan²

¹Oxford Robotics Institute, Dept. of Engineering Science, University of Oxford, UK

²University of Technology of Belfort-Montbéliard (UTBM), France

orion@oxfordrobotics.institute firstname.lastname@utbm.fr

https://ori-orion.github.io/

**Abstract.** This document describes the scientific and technical approach used by ORIon-UTBMan, the joint team of the University of Oxford (UK) and UTBM (France), to participate in the 2023 RoboCup@Home (DSPL league) with the Toyota HSR robot. It mainly includes object detection and manipulation, gesture recognition and human tracking, and loosely-coupled system integration with high modularity. Our contribution is to provide the community with a service robotics system based on state-of-the-art open-source methods especially for computer vision.

## 1 Background

ORIon-UTBMan is a joint force formed by the Oxford Robotics Institute (ORI) at the University of Oxford in the UK and UTBM in France. Both parties are members of the Toyota Partner Robot joint research project and each owns a HSR prototype (No.72 for ORIon and No.88 for UTBM). ORIon and UTBMan (formerly EPANer) were competitors in World Robot Challenge (WRC) 2018 and each grew up in subsequent international competitions. Team ORIon participated in 2019 and 2022 RoboCup@Home on-site and finished 6th (out of ten) in the former, while UTBMan participated in WRC 2021 remotely and finished second (out of five). Since the two parties not only have common research interests such as long-term robot autonomy [1,2] but also complement each other in different research fields such as object recognition and manipulation [3] and human-aware navigation [4], the joint team was born naturally in 2023. This joint venture endows the new team with the ability to confidently take on various scientific and technical challenges brought by 2023 RoboCup@Home whilst also making more socially impactful robotics research.

## 2 Research Contributions

### 2.1 Object Detection

Perception allows the robot to use the visual and depth input from its RGB-D cameras to determine the identity and location of objects in the surroundings.
Currently, ORIon-UTBMan implements a Faster R-CNN model with a ResNet-50-FPN backbone [5], which has been pre-trained on the COCO datasets <fnref n="1" /> ². The image captured by the camera is parsed by the network to output an array of bounding boxes for all detected object within the frame, their identity labels, and their detection scores (confidence level). These results are processed by applying non-maximum suppression to remove duplicate detections, then applying a minimum confidence threshold. Image coordinates of detected objects are transformed into 3D using ROS *tf*.

On top of detecting objects, storing these detections with respect to the global map helps evaluate queries on relative position relationships between objects (e.g., is object A *above* object B?, are there any objects *in the living room*?), as well as the similarity of two object classes (eg. how similar is an apple to a banana or a pear?). This is done via maintaining a semantic object map on top of a MongoDB database.

## 2.2 Manipulation

<figure><img src="image_2.png" /><figcaption>**Fig. 1.** Manipulation grasp synthesis and execution.</figcaption></figure>

The manipulation stack³ incorporates grasp synthesis directly on target object point clouds, including efficient point cloud filtering and segmentation. We use Grasp Pose Detection(GPD) [6] for grasp pose synthesis, giving us the 6-dimensional pose of approach for the highest quality grasp generated. An overview of the grasp generation and execution process can be seen in Figure 1.

Manipulation actions to be carried out are specified by the task-level planner, which provides the target transformation. At the same time, we use point clouds from the Xtion RGB-D camera for online collision mapping (with *OctoMap* occupancy estimation), detection of horizontal (e.g., table top) and vertical (e.g., door) surfaces, and detection of handles for drawers and doors. Given the object's 3D pose provided by the object detection system, the manipulation system

<footnote n="1">¹ https://github.com/ori-orion/orion_recognition
² https://github.com/ori-orion/orionFace_recognition
³ https://github.com/ori-orion/orion_manipulation</footnote>
segments the object's point cloud out of the point cloud input and feeds it into the grasp synthesis module. This module then returns a ranked list of feasible candidate poses for the HSR end effector to approach from. Accordingly, the system moves to the highest-ranked end effector pose to attempt to grasp the object while avoiding collisions. Motion control for the HSR arm and gripper is done via the Toyota joint motion control interface.

An observed flaw is that the grasp synthesis pipeline is less reliable for small and flat objects such as cutlery. This is due to GPD lacking knowledge of the gripper's shape and the difficulty of extracting a representative point cloud of these objects. Thus, we implemented a custom visual feedback system using the in-hand camera to align and orient the end effector from above to ensure a successful grasp. We also perform object-dependent selection between the gripper and the vacuum pad to allow the manipulation system to make full use of the HSR's manipulation hardware.

Team ORIon guidance faculty Lars Kunze's previous work includes knowledge-enabled manipulation, demonstrated in a system which could grasp an egg<fnref n="4" /> and make a pancake<fnref n="5" />. Further, Team ORIon's Ioannis Havoutis has an extensive background in learning, synthesis and control of complex motions.

## 2.3 Human-Robot Interaction

**Gesture Recognition** ORIon-UTBMan currently uses two state-of-the-art methods, MediaPipe<fnref n="6" /> and OpenPose<fnref n="7" /> for human skeleton tracking. The reason for using the former is its strong performance in real-time accuracy, while the latter has better integration with ROS. Combining the two provides the foundation for keypoints registration of the human body, face and hands through 2D images. Upon obtaining the keypoints, we employ Long-Short Term Memory (LSTM), a deep Artificial Neural Network (ANN) for processing sequential data. The gesture data is then presented by a temporal sequence containing keypoints<fnref n="8" />. The architecture of the neural network used is shown in Figure 2.

Three LSTM layers are used, followed by three Dense layers. RELU is the activation function chosen for all layers except the last, which uses a Softmax activation function:

$$ \sigma(z_i) = \frac{e^{z_i}}{\sum_{j=1}^{K} e^{z_j}}, i = 1, 2, \dots, K \qquad (1) $$

Equation 1 converts the output vector of K real numbers from the previous Dense layer into a probability distribution of K possible outcomes, where K is the number of gestures to predict. This takes the form $[\sigma(z_1) \sigma(z_2) \dots \sigma(z_K)]$, where the probability of each action takes a value between [0.0, 1.0]. As for other

<footnote n="4">⁴ https://www.youtube.com/watch?v=jLz87H4q3hU</footnote>

<footnote n="5">⁵ https://www.youtube.com/watch?v=YQs5gRei8k4</footnote>

<footnote n="6">⁶ https://google.github.io/mediapipe/</footnote>

<footnote n="7">⁷ https://github.com/CMU-Perceptual-Computing-Lab/openpose</footnote>

<footnote n="8">⁸ https://github.com/VictorPozoFernandez/Action-Recognition-HSR</footnote>
model parameters, Adam is the optimizer of choice, and categorical cross-entropy (CCE) is the loss function of choice due to its good performance with multiple classes and labels being one-hot encoded like as in our case:

$$L_{CCE} = \sum_{i=1}^{K} y_i \log(\sigma(z_i)) \qquad (2)$$

For evaluation, the proposed network is trained with the objective of distinguishing between the actions "coffee" and "human" given a recorded behavior, and "none" for none of the above. Two different metrics will be used: categorical accuracy and categorical cross-entropy loss. The first one calculates the percentage of predicted actions over ground-truth actions of the recorded behaviors, and the second one calculates multi-class classification accuracy. With reference to figure 3, results show that 100 epoch is sufficient to classify successfully the three previous actions. This means that not only is the model able to correctly classify the training data but also able to classify unseen data as well.

**Human Tracking** Besides being used for gesture recognition, keypoints are also used for another downstream task: human detection and tracking. The keypoints in these 2D images can be projected into 3D<fnref n="9" /> since the intrinsic parameters (i.e. the alignment between the color camera and the infrared sensor) of the current off-the-shelf RGB-D cameras are well calibrated.

The available 3D keypoints closest to the RGB-D camera frame are taken as the position of the detected human in 3D space:

$$i = \arg \min_{i \in I} (d(o_c, p_i)) \qquad (3)$$

where $d(o_c, p_i)$ is the Euclidean distance between keypoint $i$ and the coordinate origin of the RGB-D camera reference frame.

For many tasks, the HSR needs to perform in a multi-person environment. To this end, multi-target tracking is thus an essential requirement for the HSR to follow service objects. ORIon-UTBMan approach uses an efficient Unscented

<figure><img src="image_3.png" /><figcaption>Fig. 2. The architecture of the LSTM neural network.</figcaption></figure>

<footnote n="9">⁹ https://github.com/yzrobot/rgbd_detection2d_3d</footnote>
<figure><img src="image_4.png" /><figcaption>Fig. 3. Categorical cross-entropy loss (left) and accuracy metric (right) during 100 epochs (90 seq./gesture).</figcaption></figure>

Kalman Filter (UKF)-based implementation and leverages Global Nearest Neighbor (GNN) data association to process multiple human subjects simultaneously<fnref n="10" /> [7]. In particular, with logical assumptions, humans are tracked on a 2D plane (via $p'_i = \lambda p_i$, $\lambda = (1, 1, 0)$) corresponding to a flat floor, and estimating horizontal coordinates and velocities with respect to the robot base frame of reference (for mapless tasks). The prediction step of the estimation is based on the following constant velocity model:

$$ 
\left\{ 
\begin{aligned}
x_k &= x_{k-1} + \Delta t \dot{x}_{k-1} \\
\dot{x}_k &= \dot{x}_{k-1} \\
y_k &= y_{k-1} + \Delta t \dot{y}_{k-1} \\
\dot{y}_k &= \dot{y}_{k-1}
\end{aligned}
\right. 
\qquad (4) 
$$

where $x_k$ and $y_k$ are the Cartesian coordinates of the target at time $t_k$, $\dot{x}_k$ and $\dot{y}_k$ are the respective velocities, and $\Delta t = t_k - t_{k-1}$. The update step of the estimation uses a 2D Cartesian observation model to represent the position of the human:

$$ 
\left\{ 
\begin{aligned}
\mu_k &= x_k \\
\nu_k &= y_k
\end{aligned}
\right. 
\qquad (5) 
$$

where $\mu_k$ and $\nu_k$ are the Cartesian coordinates of the human from the camera. Note that noises and coordinate transformations, including those relative to the robot motion, are omitted in the above equations for the sake of simplicity. Finally, the covariance matrices $Q$ and $R$ of the noises for the prediction and observation models, respectively, are the following:

$$ 
Q = 
\begin{bmatrix}
\frac{\Delta t^4}{4} \sigma_x^2 & \frac{\Delta t^3}{2} \sigma_x^2 & 0 & 0 \\
\frac{\Delta t^3}{2} \sigma_x^2 & \Delta t^2 \sigma_x^2 & 0 & 0 \\
0 & 0 & \frac{\Delta t^4}{4} \sigma_y^2 & \frac{\Delta t^3}{2} \sigma_y^2 \\
0 & 0 & \frac{\Delta t^3}{2} \sigma_y^2 & \Delta t^2 \sigma_y^2
\end{bmatrix} 
R = 
\begin{bmatrix}
\sigma_{\theta}^2 & 0 \\
0 & \sigma_{\gamma}^2
\end{bmatrix} 
\qquad (6) 
$$

<footnote n="10">¹⁰ https://github.com/LCAS/FLOBOT</footnote>
where the noise standard deviations $\sigma_x$, $\sigma_y$, $\sigma_\theta$ and $\sigma_\gamma$ were empirically determined to optimize the human tracking performance of our robot platform.

**Social Navigation** Social navigation requires the robot to consider some conventions of human society when planning its path, such as but not limited to: 1) maintaining social distance, 2) detouring behind others instead of intercepting them, 3) facing humans as much as possible while interacting with them, 4) moving to the right and overtaking from the left in certain areas. Our current approach incorporates the implementation of the first three items, where 1) and 2) are achieved by adding two corresponding social navigation layers<fnref n="11" />, while 3) is achieved through the face detection provided by MediaPipe or OpenPose plus the calculation of the position of the human body relative to the robot in 3D space.

**Speech Recognition** To allow humans to interact with the robot using natural language, ORIon-UTBMan have developed a sub-system that performs the classification of speech commands <fnref n="12" />. This system interfaces with the cloud-based Google Speech-to-Text<fnref n="13" /> as well as local methods such as PocketSphinx<fnref n="14" /> and a pre-trained WaveNet [8] when an internet connection is not available. For classifying speech commands, we measure the Levenstein distance with candidate texts, with added semantic robustness by looking up synonymous words in Word-Net [9]. This allows an operator to communicate with the robot in predefined dialogues, such as issue commands ("tidy up", "bring me something", "move to location") and/or respond to requests made by the robot.

## 2.4 Task Level Planning

The task-level planning sub-system acts as the executive controller of the robot system <fnref n="15" />. Autonomous behaviours are defined through the use of hierarchical SMACH state machines<fnref n="16" />, which combine abilities provided by other sub-systems, accessible via ROS Action servers, to perform useful tasks in a domestic environment (e.g., fetching a bottle of water for a human). Task level planning takes in information from semantic map, navigation, speech, manipulation, and various intrinsic utility packages and generates the appropriate states for each sub-system. Subsequently, these states help guide the respective sub-system to understand which stage of the task it is on, and what is required to accomplish them.

<footnote n="11">¹¹ https://github.com/Nedzhaken/human_awarenavigation</footnote>

<footnote n="12">¹² https://github.com/ori-orion/orion_speech_recognition</footnote>

<footnote n="13">¹³ https://cloud.google.com/speech-to-text/</footnote>

<footnote n="14">¹⁴ https://github.com/cmusphinx/pocketsphinx</footnote>

<footnote n="15">¹⁵ https://github.com/ori-orion/orion_task_level_planning</footnote>

<footnote n="16">¹⁶ http://wiki.ros.org/smach</footnote>
## 2.5 Navigation

In terms of global path planning, we use a probabilistic navigation system in-
spired and modified from the STRANDS [10] project structured around a topo-
logical map (i.e., discrete locations connected by directed edges) <fnref n="17" />. Edges corre-
spond to a probabilistic navigation action the robot could perform to transition
between locations, where a higher probability represents a higher success rate.
At the same time, navigation takes in movement goals from the task-level plan-
ning subsystem. After obtaining the goal destination, the navigation package
performs a topological search through the map to plot an optimal draft path
along the edges. The HSR robot then follows the draft path via the ROS Action
server provided by the *tmc_move_base* ROS package.

# 3 Software Integration

Since the system integration (including hardware, sensors, communications and
software) of DSPL's standard platform HSR robot is already very high, we pay
more attention to software integration, which is especially critical for cross-
country alliances like ORIon-UTBMan. With the aid of the design concept of
ROS, we only need to define the communication structure between modules in
the system (such as topic and service types) according to the task needs, with-
out paying too much attention to how each module is implemented. Our current
software architecture is shown in Figure 4.

<figure><img src="image_5.png" /><figcaption>Fig. 4. The current software architecture of ORIon-UTBMan, with task level planner being the main controller. RGB-D camera is the main sensor input for our object detector. From there, useful surrounding information is passed onto the semantic object map. Voice inputs and tracked gesture key points are continuously fed from our human-robot submodule into task-level planning. Combining these information, the task-level planner is able to direct the HSR for manipulation and movement actions.</figcaption></figure>

<footnote n="17">¹⁷ https://github.com/ori-orion/strands_executive</footnote>
## 4 Conclusions and future work

Competing in the RoboCup @Home event in France will allow ORIon-UTBMan to demonstrate the new and presented capabilities of the HSR in a challenging environment and will provide valuable real-world robotics experience for our team. From now until competition day, ORIon-UTBMan will also focus on expanding the HSR's capabilities. Firstly, while the robot is working, especially when performing grasping and navigation tasks, it may lose tracked targets. This is catastrophic in some missions, such as person-following. Our current solution involves 1) recording the last observed spatial position of the target and re-observing it, and 2) implementing voice interaction to make the human target actively return to the robot's field of observation. Based on our past experiences, we also seek to define a better local path-planning methodology for the HSR to adapt to dynamic or previously unseen obstacles within the topological map. Additionally, it should also correct drifts in the robot's movement due to errors in localization or low level control. Our ambition is to demonstrate online mapping in the tasks as much as possible. We will open source our system after completing the competition.

## References

1. Bruno Lacerda and Nick Hawes. Data-driven model-based reasoning for long-term robot deployments. In *AAAI-23 Bridge Program on AI & Robotics*, 2023.

2. Zhi Yan, Li Sun, Tomas Krajnik, Tom Duckett, and Nicola Bellotto. Towards long-term autonomy: A perspective from robot learning. In *AAAI-23 Bridge Program on AI & Robotics*, 2023.

3. Lars Kunze and Michael Beetz. Envisioning the qualitative effects of robot manipulation actions using simulation-based projections. *Artificial Intelligence, Special Issue on AI and Robotic*, 2015.

4. Tomás Vintr and Zhi Yan. et al. Natural criteria for comparison of pedestrian flow forecasting models. In *IROS*, pages 11197-11204, 2020.

5. Shaoqing Ren, Kaiming He, Ross Girshick, and Jian Sun. Faster r-cnn: Towards real-time object detection with region proposal networks. *Advances in neural information processing systems*, 28:91-99, 2015.

6. Andreas ten Pas, Marcus Gualtieri, Kate Saenko, and Robert Platt. Grasp Pose Detection in Point Clouds. *The International Journal of Robotics Research*, 36(13-14):1455-1473, dec 2017.

7. Zhi Yan, Simon Schreiberhuber, Georg Halmetschlager, Tom Duckett, Markus Vincze, and Nicola Bellotto. Robot perception of static and dynamic objects with an autonomous floor scrubber. *Intelligent Service Robotics*, 13(3):403-417, 2020.

8. Aaron van den Oord, Sander Dieleman, Heiga Zen, Karen Simonyan, Oriol Vinyals, Alex Graves, Nal Kalchbrenner, Andrew Senior, and Koray Kavukcuoglu. Wavenet: A generative model for raw audio. *arXiv preprint arXiv:1609.03499*, 2016.

9. George A Miller. Wordnet: a lexical database for english. *Communications of the ACM*, 38(11):39-41, 1995.

10. Nick Hawes, Chris Burbridge, Ferdian Jovan, and Lars Kunze. et al. The STRANDS project: Long-term autonomy in everyday environments. *IEEE Robotics & Automation Magazine (RAM)*, 24(3), September 2017.
# HSR Software and External Devices

Both teams use a standard DSPL HSR robot from *Toyota*. No modifications have been applied.

## Team ORIon Robot's Software Description

For our robot we are using the following software:

- Platform: Ubuntu 20.04 / ROS Noetic

- Task-level planning: SMACH ROS library

- Navigation: *tmc_move_base* ORI Topological map and ROS package<fnref n="18" />

- Semantic mapping: *semantic_mapping*\*, MongoDB

- Object recognition and pose estimation: Faster R-CNN model with ResNet-50-FPN backbone (Yolov7 and StrongSORT planned for 2023)

- 3D occupancy mapping: OctoMap<fnref n="19" />

- Arm control and gripper coordination: Toyota HSR Motion Interface

- Grasp pose synthesis: Grasp Pose Detection (GPD)<fnref n="20" />, Grasp Pose Generator (GPG)<fnref n="21" />

- Speech recognition: PocketSphinx and Wavenet (offline speech recognition), Levenshtein<fnref n="22" /> (semantic similarity checking), WordNet (synonym checking)

<figure><img src="image_6.png" /><figcaption>Fig. 5. Team ORIon's HSR ("Bamm-Bamm")</figcaption></figure>

Items marked with an asterisk are Team ORIon custom solutions.

## External Devices

The HSR robot relies on the following external hardware:

- Alienware laptop in robot mount (for object recognition and pose estimation)

## Cloud Services

The HSR connects to the following cloud services:

- Speech recognition: All-purpose recogniser (Google API).

<footnote n="18">¹⁸ http://github.com/ori-orion/orion_topological</footnote>

<footnote n="19">¹⁹ https://octomap.github.io</footnote>

<footnote n="20">²⁰ https://github.com/atenpas/gpd</footnote>

<footnote n="21">²¹ https://github.com/atenpas/gpg</footnote>

<footnote n="22">²² https://github.com/maxbachmann/Levenshtein</footnote>